Linux_SDK_V0.9.5

lib/sbi/objects.mk

@@ -0,0 +1,43 @@
+#
+# SPDX-License-Identifier: BSD-2-Clause
+#
+# Copyright (c) 2019 Western Digital Corporation or its affiliates.
+#
+# Authors:
+#   Anup Patel <anup.patel@wdc.com>
+#
+
+libsbi-objs-y += riscv_asm.o
+libsbi-objs-y += riscv_atomic.o
+libsbi-objs-y += riscv_hardfp.o
+libsbi-objs-y += riscv_locks.o
+
+libsbi-objs-y += sbi_bitmap.o
+libsbi-objs-y += sbi_bitops.o
+libsbi-objs-y += sbi_console.o
+libsbi-objs-y += sbi_domain.o
+libsbi-objs-y += sbi_ecall.o
+libsbi-objs-y += sbi_ecall_base.o
+libsbi-objs-y += sbi_ecall_hsm.o
+libsbi-objs-y += sbi_ecall_legacy.o
+libsbi-objs-y += sbi_ecall_replace.o
+libsbi-objs-y += sbi_ecall_vendor.o
+libsbi-objs-y += sbi_emulate_csr.o
+libsbi-objs-y += sbi_fifo.o
+libsbi-objs-y += sbi_hart.o
+libsbi-objs-y += sbi_math.o
+libsbi-objs-y += sbi_hfence.o
+libsbi-objs-y += sbi_hsm.o
+libsbi-objs-y += sbi_illegal_insn.o
+libsbi-objs-y += sbi_init.o
+libsbi-objs-y += sbi_ipi.o
+libsbi-objs-y += sbi_misaligned_ldst.o
+libsbi-objs-y += sbi_platform.o
+libsbi-objs-y += sbi_scratch.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_unpriv.o
+libsbi-objs-y += sbi_expected_trap.o

lib/sbi/riscv_asm.c

@@ -0,0 +1,295 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_platform.h>
+
+/* determine CPU extension, return non-zero support */
+int misa_extension_imp(char ext)
+{
+	unsigned long misa = csr_read(CSR_MISA);
+
+	if (misa) {
+		if ('A' <= ext && ext <= 'Z')
+			return misa & (1 << (ext - 'A'));
+		if ('a' <= ext && ext <= 'z')
+			return misa & (1 << (ext - 'a'));
+		return 0;
+	}
+
+	return sbi_platform_misa_extension(sbi_platform_thishart_ptr(), ext);
+}
+
+int misa_xlen(void)
+{
+	long r;
+
+	if (csr_read(CSR_MISA) == 0)
+		return sbi_platform_misa_xlen(sbi_platform_thishart_ptr());
+
+	__asm__ __volatile__(
+		"csrr   t0, misa\n\t"
+		"slti   t1, t0, 0\n\t"
+		"slli   t1, t1, 1\n\t"
+		"slli   t0, t0, 1\n\t"
+		"slti   t0, t0, 0\n\t"
+		"add    %0, t0, t1"
+		: "=r"(r)
+		:
+		: "t0", "t1");
+
+	return r ? r : -1;
+}
+
+void misa_string(int xlen, char *out, unsigned int out_sz)
+{
+	unsigned int i, pos = 0;
+	const char valid_isa_order[] = "iemafdqclbjtpvnsuhkorwxyzg";
+
+	if (!out)
+		return;
+
+	if (5 <= (out_sz - pos)) {
+		out[pos++] = 'r';
+		out[pos++] = 'v';
+		switch (xlen) {
+		case 1:
+			out[pos++] = '3';
+			out[pos++] = '2';
+			break;
+		case 2:
+			out[pos++] = '6';
+			out[pos++] = '4';
+			break;
+		case 3:
+			out[pos++] = '1';
+			out[pos++] = '2';
+			out[pos++] = '8';
+			break;
+		default:
+			return;
+		}
+	}
+
+	for (i = 0; i < array_size(valid_isa_order) && (pos < out_sz); i++) {
+		if (misa_extension_imp(valid_isa_order[i]))
+			out[pos++] = valid_isa_order[i];
+	}
+
+	if (pos < out_sz)
+		out[pos++] = '\0';
+}
+
+unsigned long csr_read_num(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)
+#define switchcase_csr_read_32(__csr_num, __val)	\
+	switchcase_csr_read_16(__csr_num + 0, __val)	\
+	switchcase_csr_read_16(__csr_num + 16, __val)
+#define switchcase_csr_read_64(__csr_num, __val)	\
+	switchcase_csr_read_32(__csr_num + 0, __val)	\
+	switchcase_csr_read_32(__csr_num + 32, __val)
+
+	unsigned long ret = 0;
+
+	switch (csr_num) {
+	switchcase_csr_read_16(CSR_PMPCFG0, ret)
+	switchcase_csr_read_64(CSR_PMPADDR0, ret)
+	default:
+		break;
+	};
+
+	return ret;
+
+#undef switchcase_csr_read_64
+#undef switchcase_csr_read_32
+#undef switchcase_csr_read_16
+#undef switchcase_csr_read_8
+#undef switchcase_csr_read_4
+#undef switchcase_csr_read_2
+#undef switchcase_csr_read
+}
+
+void csr_write_num(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)
+#define switchcase_csr_write_32(__csr_num, __val)	\
+	switchcase_csr_write_16(__csr_num + 0, __val)	\
+	switchcase_csr_write_16(__csr_num + 16, __val)
+#define switchcase_csr_write_64(__csr_num, __val)	\
+	switchcase_csr_write_32(__csr_num + 0, __val)	\
+	switchcase_csr_write_32(__csr_num + 32, __val)
+
+	switch (csr_num) {
+	switchcase_csr_write_16(CSR_PMPCFG0, val)
+	switchcase_csr_write_64(CSR_PMPADDR0, val)
+	default:
+		break;
+	};
+
+#undef switchcase_csr_write_64
+#undef switchcase_csr_write_32
+#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 unsigned long ctz(unsigned long x)
+{
+	unsigned long ret = 0;
+
+	while (!(x & 1UL)) {
+		ret++;
+		x = x >> 1;
+	}
+
+	return ret;
+}
+
+int pmp_set(unsigned int n, unsigned long prot, unsigned long addr,
+	    unsigned long log2len)
+{
+	int pmpcfg_csr, pmpcfg_shift, pmpaddr_csr;
+	unsigned long cfgmask, pmpcfg;
+	unsigned long addrmask, pmpaddr;
+
+	/* check parameters */
+	if (n >= PMP_COUNT || log2len > __riscv_xlen || log2len < PMP_SHIFT)
+		return SBI_EINVAL;
+
+	/* calculate PMP register and offset */
+#if __riscv_xlen == 32
+	pmpcfg_csr   = CSR_PMPCFG0 + (n >> 2);
+	pmpcfg_shift = (n & 3) << 3;
+#elif __riscv_xlen == 64
+	pmpcfg_csr   = (CSR_PMPCFG0 + (n >> 2)) & ~1;
+	pmpcfg_shift = (n & 7) << 3;
+#else
+	pmpcfg_csr   = -1;
+	pmpcfg_shift = -1;
+#endif
+	pmpaddr_csr = CSR_PMPADDR0 + n;
+	if (pmpcfg_csr < 0 || pmpcfg_shift < 0)
+		return SBI_ENOTSUPP;
+
+	/* encode PMP config */
+	prot |= (log2len == PMP_SHIFT) ? PMP_A_NA4 : PMP_A_NAPOT;
+	cfgmask = ~(0xffUL << pmpcfg_shift);
+	pmpcfg	= (csr_read_num(pmpcfg_csr) & cfgmask);
+	pmpcfg |= ((prot << pmpcfg_shift) & ~cfgmask);
+
+	/* encode PMP address */
+	if (log2len == PMP_SHIFT) {
+		pmpaddr = (addr >> PMP_SHIFT);
+	} else {
+		if (log2len == __riscv_xlen) {
+			pmpaddr = -1UL;
+		} else {
+			addrmask = (1UL << (log2len - PMP_SHIFT)) - 1;
+			pmpaddr	 = ((addr >> PMP_SHIFT) & ~addrmask);
+			pmpaddr |= (addrmask >> 1);
+		}
+	}
+
+	/* write csrs */
+	csr_write_num(pmpaddr_csr, pmpaddr);
+	csr_write_num(pmpcfg_csr, pmpcfg);
+
+	return 0;
+}
+
+int pmp_get(unsigned int n, unsigned long *prot_out, unsigned long *addr_out,
+	    unsigned long *log2len)
+{
+	int pmpcfg_csr, pmpcfg_shift, pmpaddr_csr;
+	unsigned long cfgmask, pmpcfg, prot;
+	unsigned long t1, addr, len;
+
+	/* check parameters */
+	if (n >= PMP_COUNT || !prot_out || !addr_out || !log2len)
+		return SBI_EINVAL;
+	*prot_out = *addr_out = *log2len = 0;
+
+	/* calculate PMP register and offset */
+#if __riscv_xlen == 32
+	pmpcfg_csr   = CSR_PMPCFG0 + (n >> 2);
+	pmpcfg_shift = (n & 3) << 3;
+#elif __riscv_xlen == 64
+	pmpcfg_csr   = (CSR_PMPCFG0 + (n >> 2)) & ~1;
+	pmpcfg_shift = (n & 7) << 3;
+#else
+	pmpcfg_csr   = -1;
+	pmpcfg_shift = -1;
+#endif
+	pmpaddr_csr = CSR_PMPADDR0 + n;
+	if (pmpcfg_csr < 0 || pmpcfg_shift < 0)
+		return SBI_ENOTSUPP;
+
+	/* decode PMP config */
+	cfgmask = (0xffUL << pmpcfg_shift);
+	pmpcfg	= csr_read_num(pmpcfg_csr) & cfgmask;
+	prot	= pmpcfg >> pmpcfg_shift;
+
+	/* decode PMP address */
+	if ((prot & PMP_A) == PMP_A_NAPOT) {
+		addr = csr_read_num(pmpaddr_csr);
+		if (addr == -1UL) {
+			addr	= 0;
+			len	= __riscv_xlen;
+		} else {
+			t1	= ctz(~addr);
+			addr	= (addr & ~((1UL << t1) - 1)) << PMP_SHIFT;
+			len	= (t1 + PMP_SHIFT + 1);
+		}
+	} else {
+		addr	= csr_read_num(pmpaddr_csr) << PMP_SHIFT;
+		len	= PMP_SHIFT;
+	}
+
+	/* return details */
+	*prot_out    = prot;
+	*addr_out    = addr;
+	*log2len     = len;
+
+	return 0;
+}

lib/sbi/riscv_atomic.c

@@ -0,0 +1,253 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/sbi_bitops.h>
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_atomic.h>
+#include <sbi/riscv_barrier.h>
+
+long atomic_read(atomic_t *atom)
+{
+	long ret = atom->counter;
+	rmb();
+	return ret;
+}
+
+void atomic_write(atomic_t *atom, long value)
+{
+	atom->counter = value;
+	wmb();
+}
+
+long atomic_add_return(atomic_t *atom, long value)
+{
+	long ret;
+#if __SIZEOF_LONG__ == 4
+	__asm__ __volatile__("	amoadd.w.aqrl  %1, %2, %0"
+			     : "+A"(atom->counter), "=r"(ret)
+			     : "r"(value)
+			     : "memory");
+#elif __SIZEOF_LONG__ == 8
+	__asm__ __volatile__("	amoadd.d.aqrl  %1, %2, %0"
+			     : "+A"(atom->counter), "=r"(ret)
+			     : "r"(value)
+			     : "memory");
+#endif
+	return ret + value;
+}
+
+long atomic_sub_return(atomic_t *atom, long value)
+{
+	return atomic_add_return(atom, -value);
+}
+
+#define __axchg(ptr, new, size)							\
+	({									\
+		__typeof__(ptr) __ptr = (ptr);					\
+		__typeof__(new) __new = (new);					\
+		__typeof__(*(ptr)) __ret;					\
+		switch (size) {							\
+		case 4:								\
+			__asm__ __volatile__ (					\
+				"	amoswap.w.aqrl %0, %2, %1\n"		\
+				: "=r" (__ret), "+A" (*__ptr)			\
+				: "r" (__new)					\
+				: "memory");					\
+			break;							\
+		case 8:								\
+			__asm__ __volatile__ (					\
+				"	amoswap.d.aqrl %0, %2, %1\n"		\
+				: "=r" (__ret), "+A" (*__ptr)			\
+				: "r" (__new)					\
+				: "memory");					\
+			break;							\
+		default:							\
+			break;							\
+		}								\
+		__ret;								\
+	})
+
+#define axchg(ptr, x)								\
+	({									\
+		__typeof__(*(ptr)) _x_ = (x);					\
+		(__typeof__(*(ptr))) __axchg((ptr), _x_, sizeof(*(ptr)));	\
+	})
+
+
+#define __xchg(ptr, new, size)                                            \
+	({                                                                \
+		__typeof__(ptr) __ptr	 = (ptr);                         \
+		__typeof__(*(ptr)) __new = (new);                         \
+		__typeof__(*(ptr)) __ret;                                 \
+		register unsigned int __rc;                               \
+		switch (size) {                                           \
+		case 4:                                                   \
+			__asm__ __volatile__("0:	lr.w %0, %2\n"    \
+					     "	sc.w.rl %1, %z3, %2\n"    \
+					     "	bnez %1, 0b\n"            \
+					     "	fence rw, rw\n"           \
+					     : "=&r"(__ret), "=&r"(__rc), \
+					       "+A"(*__ptr)               \
+					     : "rJ"(__new)                \
+					     : "memory");                 \
+			break;                                            \
+		case 8:                                                   \
+			__asm__ __volatile__("0:	lr.d %0, %2\n"    \
+					     "	sc.d.rl %1, %z3, %2\n"    \
+					     "	bnez %1, 0b\n"            \
+					     "	fence rw, rw\n"           \
+					     : "=&r"(__ret), "=&r"(__rc), \
+					       "+A"(*__ptr)               \
+					     : "rJ"(__new)                \
+					     : "memory");                 \
+			break;                                            \
+		default:                                                  \
+			break;                                            \
+		}                                                         \
+		__ret;                                                    \
+	})
+
+#define xchg(ptr, n)                                                     \
+	({                                                               \
+		__typeof__(*(ptr)) _n_ = (n);                            \
+		(__typeof__(*(ptr))) __xchg((ptr), _n_, sizeof(*(ptr))); \
+	})
+
+#define __cmpxchg(ptr, old, new, size)                                    \
+	({                                                                \
+		__typeof__(ptr) __ptr	 = (ptr);                         \
+		__typeof__(*(ptr)) __old = (old);                         \
+		__typeof__(*(ptr)) __new = (new);                         \
+		__typeof__(*(ptr)) __ret;                                 \
+		register unsigned int __rc;                               \
+		switch (size) {                                           \
+		case 4:                                                   \
+			__asm__ __volatile__("0:	lr.w %0, %2\n"    \
+					     "	bne  %0, %z3, 1f\n"       \
+					     "	sc.w.rl %1, %z4, %2\n"    \
+					     "	bnez %1, 0b\n"            \
+					     "	fence rw, rw\n"           \
+					     "1:\n"                       \
+					     : "=&r"(__ret), "=&r"(__rc), \
+					       "+A"(*__ptr)               \
+					     : "rJ"(__old), "rJ"(__new)   \
+					     : "memory");                 \
+			break;                                            \
+		case 8:                                                   \
+			__asm__ __volatile__("0:	lr.d %0, %2\n"    \
+					     "	bne %0, %z3, 1f\n"        \
+					     "	sc.d.rl %1, %z4, %2\n"    \
+					     "	bnez %1, 0b\n"            \
+					     "	fence rw, rw\n"           \
+					     "1:\n"                       \
+					     : "=&r"(__ret), "=&r"(__rc), \
+					       "+A"(*__ptr)               \
+					     : "rJ"(__old), "rJ"(__new)   \
+					     : "memory");                 \
+			break;                                            \
+		default:                                                  \
+			break;                                            \
+		}                                                         \
+		__ret;                                                    \
+	})
+
+#define cmpxchg(ptr, o, n)                                          \
+	({                                                          \
+		__typeof__(*(ptr)) _o_ = (o);                       \
+		__typeof__(*(ptr)) _n_ = (n);                       \
+		(__typeof__(*(ptr)))                                \
+			__cmpxchg((ptr), _o_, _n_, sizeof(*(ptr))); \
+	})
+
+long atomic_cmpxchg(atomic_t *atom, long oldval, long newval)
+{
+#ifdef __riscv_atomic
+	return __sync_val_compare_and_swap(&atom->counter, oldval, newval);
+#else
+	return cmpxchg(&atom->counter, oldval, newval);
+#endif
+}
+
+long atomic_xchg(atomic_t *atom, long newval)
+{
+	/* Atomically set new value and return old value. */
+#ifdef __riscv_atomic
+	return axchg(&atom->counter, newval);
+#else
+	return xchg(&atom->counter, newval);
+#endif
+}
+
+unsigned int atomic_raw_xchg_uint(volatile unsigned int *ptr,
+				  unsigned int newval)
+{
+	/* Atomically set new value and return old value. */
+#ifdef __riscv_atomic
+	return axchg(ptr, newval);
+#else
+	return xchg(ptr, newval);
+#endif
+}
+
+unsigned long atomic_raw_xchg_ulong(volatile unsigned long *ptr,
+				    unsigned long newval)
+{
+	/* Atomically set new value and return old value. */
+#ifdef __riscv_atomic
+	return axchg(ptr, newval);
+#else
+	return xchg(ptr, newval);
+#endif
+}
+
+#if (__SIZEOF_POINTER__ == 8)
+#define __AMO(op) "amo" #op ".d"
+#elif (__SIZEOF_POINTER__ == 4)
+#define __AMO(op) "amo" #op ".w"
+#else
+#error "Unexpected __SIZEOF_POINTER__"
+#endif
+
+#define __atomic_op_bit_ord(op, mod, nr, addr, ord)                          \
+	({                                                                   \
+		unsigned long __res, __mask;                                 \
+		__mask = BIT_MASK(nr);                                       \
+		__asm__ __volatile__(__AMO(op) #ord " %0, %2, %1"            \
+				     : "=r"(__res), "+A"(addr[BIT_WORD(nr)]) \
+				     : "r"(mod(__mask))                      \
+				     : "memory");                            \
+		__res;                                                       \
+	})
+
+#define __atomic_op_bit(op, mod, nr, addr) \
+	__atomic_op_bit_ord(op, mod, nr, addr, .aqrl)
+
+/* Bitmask modifiers */
+#define __NOP(x) (x)
+#define __NOT(x) (~(x))
+
+inline int atomic_raw_set_bit(int nr, volatile unsigned long *addr)
+{
+	return __atomic_op_bit(or, __NOP, nr, addr);
+}
+
+inline int atomic_raw_clear_bit(int nr, volatile unsigned long *addr)
+{
+	return __atomic_op_bit(and, __NOT, nr, addr);
+}
+
+inline int atomic_set_bit(int nr, atomic_t *atom)
+{
+	return atomic_raw_set_bit(nr, (unsigned long *)&atom->counter);
+}
+
+inline int atomic_clear_bit(int nr, atomic_t *atom)
+{
+	return atomic_raw_clear_bit(nr, (unsigned long *)&atom->counter);
+}

lib/sbi/riscv_hardfp.S

@@ -0,0 +1,171 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#ifdef __riscv_flen
+
+#if __riscv_flen != 64
+# error single-float only is not supported
+#endif
+
+#define get_f32(which) fmv.x.s a0, which; jr t0
+#define put_f32(which) fmv.s.x which, a0; jr t0
+#if __riscv_xlen == 64
+# define get_f64(which) fmv.x.d a0, which; jr t0
+# define put_f64(which) fmv.d.x which, a0; jr t0
+#else
+# define get_f64(which) fsd which, 0(a0); jr t0
+# define put_f64(which) fld which, 0(a0); jr t0
+#endif
+
+	.text
+	.option norvc
+	.globl get_f32_reg
+	get_f32_reg:
+		get_f32(f0)
+		get_f32(f1)
+		get_f32(f2)
+		get_f32(f3)
+		get_f32(f4)
+		get_f32(f5)
+		get_f32(f6)
+		get_f32(f7)
+		get_f32(f8)
+		get_f32(f9)
+		get_f32(f10)
+		get_f32(f11)
+		get_f32(f12)
+		get_f32(f13)
+		get_f32(f14)
+		get_f32(f15)
+		get_f32(f16)
+		get_f32(f17)
+		get_f32(f18)
+		get_f32(f19)
+		get_f32(f20)
+		get_f32(f21)
+		get_f32(f22)
+		get_f32(f23)
+		get_f32(f24)
+		get_f32(f25)
+		get_f32(f26)
+		get_f32(f27)
+		get_f32(f28)
+		get_f32(f29)
+		get_f32(f30)
+		get_f32(f31)
+
+	.text
+	.globl put_f32_reg
+	put_f32_reg:
+		put_f32(f0)
+		put_f32(f1)
+		put_f32(f2)
+		put_f32(f3)
+		put_f32(f4)
+		put_f32(f5)
+		put_f32(f6)
+		put_f32(f7)
+		put_f32(f8)
+		put_f32(f9)
+		put_f32(f10)
+		put_f32(f11)
+		put_f32(f12)
+		put_f32(f13)
+		put_f32(f14)
+		put_f32(f15)
+		put_f32(f16)
+		put_f32(f17)
+		put_f32(f18)
+		put_f32(f19)
+		put_f32(f20)
+		put_f32(f21)
+		put_f32(f22)
+		put_f32(f23)
+		put_f32(f24)
+		put_f32(f25)
+		put_f32(f26)
+		put_f32(f27)
+		put_f32(f28)
+		put_f32(f29)
+		put_f32(f30)
+		put_f32(f31)
+
+	.text
+	.globl get_f64_reg
+	get_f64_reg:
+		get_f64(f0)
+		get_f64(f1)
+		get_f64(f2)
+		get_f64(f3)
+		get_f64(f4)
+		get_f64(f5)
+		get_f64(f6)
+		get_f64(f7)
+		get_f64(f8)
+		get_f64(f9)
+		get_f64(f10)
+		get_f64(f11)
+		get_f64(f12)
+		get_f64(f13)
+		get_f64(f14)
+		get_f64(f15)
+		get_f64(f16)
+		get_f64(f17)
+		get_f64(f18)
+		get_f64(f19)
+		get_f64(f20)
+		get_f64(f21)
+		get_f64(f22)
+		get_f64(f23)
+		get_f64(f24)
+		get_f64(f25)
+		get_f64(f26)
+		get_f64(f27)
+		get_f64(f28)
+		get_f64(f29)
+		get_f64(f30)
+		get_f64(f31)
+
+	.text
+	.globl put_f64_reg
+	put_f64_reg:
+		put_f64(f0)
+		put_f64(f1)
+		put_f64(f2)
+		put_f64(f3)
+		put_f64(f4)
+		put_f64(f5)
+		put_f64(f6)
+		put_f64(f7)
+		put_f64(f8)
+		put_f64(f9)
+		put_f64(f10)
+		put_f64(f11)
+		put_f64(f12)
+		put_f64(f13)
+		put_f64(f14)
+		put_f64(f15)
+		put_f64(f16)
+		put_f64(f17)
+		put_f64(f18)
+		put_f64(f19)
+		put_f64(f20)
+		put_f64(f21)
+		put_f64(f22)
+		put_f64(f23)
+		put_f64(f24)
+		put_f64(f25)
+		put_f64(f26)
+		put_f64(f27)
+		put_f64(f28)
+		put_f64(f29)
+		put_f64(f30)
+		put_f64(f31)
+
+#endif

lib/sbi/riscv_locks.c

@@ -0,0 +1,77 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ * Copyright (c) 2021 Christoph Müllner <cmuellner@linux.com>
+ */
+
+#include <sbi/riscv_barrier.h>
+#include <sbi/riscv_locks.h>
+
+static inline int spin_lock_unlocked(spinlock_t lock)
+{
+	return lock.owner == lock.next;
+}
+
+bool spin_lock_check(spinlock_t *lock)
+{
+	RISCV_FENCE(r, rw);
+	return !spin_lock_unlocked(*lock);
+}
+
+int spin_trylock(spinlock_t *lock)
+{
+	unsigned long inc = 1u << TICKET_SHIFT;
+	unsigned long mask = 0xffffu << TICKET_SHIFT;
+	u32 l0, tmp1, tmp2;
+
+	__asm__ __volatile__(
+		/* Get the current lock counters. */
+		"1:	lr.w.aq	%0, %3\n"
+		"	slli	%2, %0, %6\n"
+		"	and	%2, %2, %5\n"
+		"	and	%1, %0, %5\n"
+		/* Is the lock free right now? */
+		"	bne	%1, %2, 2f\n"
+		"	add	%0, %0, %4\n"
+		/* Acquire the lock. */
+		"	sc.w.rl	%0, %0, %3\n"
+		"	bnez	%0, 1b\n"
+		"2:"
+		: "=&r"(l0), "=&r"(tmp1), "=&r"(tmp2), "+A"(*lock)
+		: "r"(inc), "r"(mask), "I"(TICKET_SHIFT)
+		: "memory");
+
+	return !l0;
+}
+
+void spin_lock(spinlock_t *lock)
+{
+	unsigned long inc = 1u << TICKET_SHIFT;
+	unsigned long mask = 0xffffu;
+	u32 l0, tmp1, tmp2;
+
+	__asm__ __volatile__(
+		/* Atomically increment the next ticket. */
+		"	amoadd.w.aqrl	%0, %4, %3\n"
+
+		/* Did we get the lock? */
+		"	srli	%1, %0, %6\n"
+		"	and	%1, %1, %5\n"
+		"1:	and	%2, %0, %5\n"
+		"	beq	%1, %2, 2f\n"
+
+		/* If not, then spin on the lock. */
+		"	lw	%0, %3\n"
+		RISCV_ACQUIRE_BARRIER
+		"	j	1b\n"
+		"2:"
+		: "=&r"(l0), "=&r"(tmp1), "=&r"(tmp2), "+A"(*lock)
+		: "r"(inc), "r"(mask), "I"(TICKET_SHIFT)
+		: "memory");
+}
+
+void spin_unlock(spinlock_t *lock)
+{
+	__smp_store_release(&lock->owner, lock->owner + 1);
+}

lib/sbi/sbi_bitmap.c

@@ -0,0 +1,40 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/sbi_bitmap.h>
+
+void __bitmap_and(unsigned long *dst, const unsigned long *bitmap1,
+		  const unsigned long *bitmap2, int bits)
+{
+	int k;
+	int nr = BITS_TO_LONGS(bits);
+
+	for (k = 0; k < nr; k++)
+		dst[k] = bitmap1[k] & bitmap2[k];
+}
+
+void __bitmap_or(unsigned long *dst, const unsigned long *bitmap1,
+		 const unsigned long *bitmap2, int bits)
+{
+	int k;
+	int nr = BITS_TO_LONGS(bits);
+
+	for (k = 0; k < nr; k++)
+		dst[k] = bitmap1[k] | bitmap2[k];
+}
+
+void __bitmap_xor(unsigned long *dst, const unsigned long *bitmap1,
+		  const unsigned long *bitmap2, int bits)
+{
+	int k;
+	int nr = BITS_TO_LONGS(bits);
+
+	for (k = 0; k < nr; k++)
+		dst[k] = bitmap1[k] ^ bitmap2[k];
+}

lib/sbi/sbi_bitops.c

@@ -0,0 +1,200 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Atish Patra <atish.patra@wdc.com>
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/sbi_bitops.h>
+
+#define BITOP_WORD(nr)		((nr) / BITS_PER_LONG)
+
+/**
+ * find_first_bit - find the first set bit in a memory region
+ * @addr: The address to start the search at
+ * @size: The maximum size to search
+ *
+ * Returns the bit number of the first set bit.
+ */
+unsigned long find_first_bit(const unsigned long *addr,
+			     unsigned long size)
+{
+	const unsigned long *p = addr;
+	unsigned long result = 0;
+	unsigned long tmp;
+
+	while (size & ~(BITS_PER_LONG-1)) {
+		if ((tmp = *(p++)))
+			goto found;
+		result += BITS_PER_LONG;
+		size -= BITS_PER_LONG;
+	}
+	if (!size)
+		return result;
+
+	tmp = (*p) & (~0UL >> (BITS_PER_LONG - size));
+	if (tmp == 0UL)		/* Are any bits set? */
+		return result + size;	/* Nope. */
+found:
+	return result + __ffs(tmp);
+}
+
+/**
+ * find_first_zero_bit - find the first cleared bit in a memory region
+ * @addr: The address to start the search at
+ * @size: The maximum size to search
+ *
+ * Returns the bit number of the first cleared bit.
+ */
+unsigned long find_first_zero_bit(const unsigned long *addr,
+				  unsigned long size)
+{
+	const unsigned long *p = addr;
+	unsigned long result = 0;
+	unsigned long tmp;
+
+	while (size & ~(BITS_PER_LONG-1)) {
+		if (~(tmp = *(p++)))
+			goto found;
+		result += BITS_PER_LONG;
+		size -= BITS_PER_LONG;
+	}
+	if (!size)
+		return result;
+
+	tmp = (*p) | (~0UL << size);
+	if (tmp == ~0UL)	/* Are any bits zero? */
+		return result + size;	/* Nope. */
+found:
+	return result + ffz(tmp);
+}
+
+/**
+ * find_last_bit - find the last set bit in a memory region
+ * @addr: The address to start the search at
+ * @size: The maximum size to search
+ *
+ * Returns the bit number of the first set bit, or size.
+ */
+unsigned long find_last_bit(const unsigned long *addr,
+			    unsigned long size)
+{
+	unsigned long words;
+	unsigned long tmp;
+
+	/* Start at final word. */
+	words = size / BITS_PER_LONG;
+
+	/* Partial final word? */
+	if (size & (BITS_PER_LONG-1)) {
+		tmp = (addr[words] & (~0UL >> (BITS_PER_LONG
+					 - (size & (BITS_PER_LONG-1)))));
+		if (tmp)
+			goto found;
+	}
+
+	while (words) {
+		tmp = addr[--words];
+		if (tmp) {
+found:
+			return words * BITS_PER_LONG + __fls(tmp);
+		}
+	}
+
+	/* Not found */
+	return size;
+}
+
+/**
+ * find_next_bit - find the next set bit in a memory region
+ * @addr: The address to base the search on
+ * @offset: The bitnumber to start searching at
+ * @size: The bitmap size in bits
+ */
+unsigned long find_next_bit(const unsigned long *addr,
+			    unsigned long size, unsigned long offset)
+{
+	const unsigned long *p = addr + BITOP_WORD(offset);
+	unsigned long result = offset & ~(BITS_PER_LONG-1);
+	unsigned long tmp;
+
+	if (offset >= size)
+		return size;
+	size -= result;
+	offset %= BITS_PER_LONG;
+	if (offset) {
+		tmp = *(p++);
+		tmp &= (~0UL << offset);
+		if (size < BITS_PER_LONG)
+			goto found_first;
+		if (tmp)
+			goto found_middle;
+		size -= BITS_PER_LONG;
+		result += BITS_PER_LONG;
+	}
+	while (size & ~(BITS_PER_LONG-1)) {
+		if ((tmp = *(p++)))
+			goto found_middle;
+		result += BITS_PER_LONG;
+		size -= BITS_PER_LONG;
+	}
+	if (!size)
+		return result;
+	tmp = *p;
+
+found_first:
+	tmp &= (~0UL >> (BITS_PER_LONG - size));
+	if (tmp == 0UL)		/* Are any bits set? */
+		return result + size;	/* Nope. */
+found_middle:
+	return result + __ffs(tmp);
+}
+
+/**
+ * find_next_zero_bit - find the next cleared bit in a memory region
+ * @addr: The address to base the search on
+ * @offset: The bitnumber to start searching at
+ * @size: The bitmap size in bits
+ */
+unsigned long find_next_zero_bit(const unsigned long *addr,
+				 unsigned long size,
+				 unsigned long offset)
+{
+	const unsigned long *p = addr + BITOP_WORD(offset);
+	unsigned long result = offset & ~(BITS_PER_LONG-1);
+	unsigned long tmp;
+
+	if (offset >= size)
+		return size;
+	size -= result;
+	offset %= BITS_PER_LONG;
+	if (offset) {
+		tmp = *(p++);
+		tmp |= ~0UL >> (BITS_PER_LONG - offset);
+		if (size < BITS_PER_LONG)
+			goto found_first;
+		if (~tmp)
+			goto found_middle;
+		size -= BITS_PER_LONG;
+		result += BITS_PER_LONG;
+	}
+	while (size & ~(BITS_PER_LONG-1)) {
+		if (~(tmp = *(p++)))
+			goto found_middle;
+		result += BITS_PER_LONG;
+		size -= BITS_PER_LONG;
+	}
+	if (!size)
+		return result;
+	tmp = *p;
+
+found_first:
+	tmp |= ~0UL << size;
+	if (tmp == ~0UL)	/* Are any bits zero? */
+		return result + size;	/* Nope. */
+found_middle:
+	return result + ffz(tmp);
+}

lib/sbi/sbi_console.c

@@ -0,0 +1,413 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_locks.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_platform.h>
+#include <sbi/sbi_scratch.h>
+
+static const struct sbi_console_device *console_dev = NULL;
+static spinlock_t console_out_lock	       = SPIN_LOCK_INITIALIZER;
+
+bool sbi_isprintable(char c)
+{
+	if (((31 < c) && (c < 127)) || (c == '\f') || (c == '\r') ||
+	    (c == '\n') || (c == '\t')) {
+		return TRUE;
+	}
+	return FALSE;
+}
+
+int sbi_getc(void)
+{
+	if (console_dev && console_dev->console_getc)
+		return console_dev->console_getc();
+	return -1;
+}
+
+void sbi_putc(char ch)
+{
+	if (console_dev && console_dev->console_putc) {
+		if (ch == '\n')
+			console_dev->console_putc('\r');
+		console_dev->console_putc(ch);
+	}
+}
+
+void sbi_puts(const char *str)
+{
+	spin_lock(&console_out_lock);
+	while (*str) {
+		sbi_putc(*str);
+		str++;
+	}
+	spin_unlock(&console_out_lock);
+}
+
+void sbi_gets(char *s, int maxwidth, char endchar)
+{
+	int ch;
+	char *retval = s;
+
+	while ((ch = sbi_getc()) != endchar && ch >= 0 && maxwidth > 1) {
+		*retval = (char)ch;
+		retval++;
+		maxwidth--;
+	}
+	*retval = '\0';
+}
+
+#define PAD_RIGHT 1
+#define PAD_ZERO 2
+#define PAD_ALTERNATE 4
+#define PRINT_BUF_LEN 64
+
+#define va_start(v, l) __builtin_va_start((v), l)
+#define va_end __builtin_va_end
+#define va_arg __builtin_va_arg
+typedef __builtin_va_list va_list;
+
+static void printc(char **out, u32 *out_len, char ch)
+{
+	if (out) {
+		if (*out) {
+			if (out_len && (0 < *out_len)) {
+				**out = ch;
+				++(*out);
+				(*out_len)--;
+			} else {
+				**out = ch;
+				++(*out);
+			}
+		}
+	} else {
+		sbi_putc(ch);
+	}
+}
+
+static int prints(char **out, u32 *out_len, const char *string, int width,
+		  int flags)
+{
+	int pc	     = 0;
+	char padchar = ' ';
+
+	if (width > 0) {
+		int len = 0;
+		const char *ptr;
+		for (ptr = string; *ptr; ++ptr)
+			++len;
+		if (len >= width)
+			width = 0;
+		else
+			width -= len;
+		if (flags & PAD_ZERO)
+			padchar = '0';
+	}
+	if (!(flags & PAD_RIGHT)) {
+		for (; width > 0; --width) {
+			printc(out, out_len, padchar);
+			++pc;
+		}
+	}
+	for (; *string; ++string) {
+		printc(out, out_len, *string);
+		++pc;
+	}
+	for (; width > 0; --width) {
+		printc(out, out_len, padchar);
+		++pc;
+	}
+
+	return pc;
+}
+
+static int printi(char **out, u32 *out_len, long long i, int b, int sg,
+		  int width, int flags, int letbase)
+{
+	char print_buf[PRINT_BUF_LEN];
+	char *s;
+	int neg = 0, pc = 0;
+	u64 t;
+	unsigned long long u = i;
+
+	if (sg && b == 10 && i < 0) {
+		neg = 1;
+		u   = -i;
+	}
+
+	s  = print_buf + PRINT_BUF_LEN - 1;
+	*s = '\0';
+
+	if (!u) {
+		*--s = '0';
+	} else {
+		while (u) {
+			t = u % b;
+			u = u / b;
+			if (t >= 10)
+				t += letbase - '0' - 10;
+			*--s = t + '0';
+		}
+	}
+
+	if (flags & PAD_ALTERNATE) {
+		if ((b == 16) && (letbase == 'A')) {
+			*--s = 'X';
+		} else if ((b == 16) && (letbase == 'a')) {
+			*--s = 'x';
+		}
+		*--s = '0';
+	}
+
+	if (neg) {
+		if (width && (flags & PAD_ZERO)) {
+			printc(out, out_len, '-');
+			++pc;
+			--width;
+		} else {
+			*--s = '-';
+		}
+	}
+
+	return pc + prints(out, out_len, s, width, flags);
+}
+
+static int print(char **out, u32 *out_len, const char *format, va_list args)
+{
+	int width, flags, acnt = 0;
+	int pc = 0;
+	char scr[2];
+	unsigned long long tmp;
+
+	for (; *format != 0; ++format) {
+		if (*format == '%') {
+			++format;
+			width = flags = 0;
+			if (*format == '\0')
+				break;
+			if (*format == '%')
+				goto out;
+			/* Get flags */
+			if (*format == '-') {
+				++format;
+				flags = PAD_RIGHT;
+			}
+			if (*format == '#') {
+				++format;
+				flags |= PAD_ALTERNATE;
+			}
+			while (*format == '0') {
+				++format;
+				flags |= PAD_ZERO;
+			}
+			/* Get width */
+			for (; *format >= '0' && *format <= '9'; ++format) {
+				width *= 10;
+				width += *format - '0';
+			}
+			if (*format == 's') {
+				char *s = va_arg(args, char *);
+				acnt += sizeof(char *);
+				pc += prints(out, out_len, s ? s : "(null)",
+					     width, flags);
+				continue;
+			}
+			if ((*format == 'd') || (*format == 'i')) {
+				pc += printi(out, out_len, va_arg(args, int),
+					     10, 1, width, flags, '0');
+				acnt += sizeof(int);
+				continue;
+			}
+			if (*format == 'x') {
+				pc += printi(out, out_len,
+					     va_arg(args, unsigned int), 16, 0,
+					     width, flags, 'a');
+				acnt += sizeof(unsigned int);
+				continue;
+			}
+			if (*format == 'X') {
+				pc += printi(out, out_len,
+					     va_arg(args, unsigned int), 16, 0,
+					     width, flags, 'A');
+				acnt += sizeof(unsigned int);
+				continue;
+			}
+			if (*format == 'u') {
+				pc += printi(out, out_len,
+					     va_arg(args, unsigned int), 10, 0,
+					     width, flags, 'a');
+				acnt += sizeof(unsigned int);
+				continue;
+			}
+			if (*format == 'p') {
+				pc += printi(out, out_len,
+					     va_arg(args, unsigned long), 16, 0,
+					     width, flags, 'a');
+				acnt += sizeof(unsigned long);
+				continue;
+			}
+			if (*format == 'P') {
+				pc += printi(out, out_len,
+					     va_arg(args, unsigned long), 16, 0,
+					     width, flags, 'A');
+				acnt += sizeof(unsigned long);
+				continue;
+			}
+			if (*format == 'l' && *(format + 1) == 'l') {
+				while (acnt &
+				       (sizeof(unsigned long long) - 1)) {
+					va_arg(args, int);
+					acnt += sizeof(int);
+				}
+				if (sizeof(unsigned long long) ==
+				    sizeof(unsigned long)) {
+					tmp = va_arg(args, unsigned long long);
+					acnt += sizeof(unsigned long long);
+				} else {
+					((unsigned long *)&tmp)[0] =
+						va_arg(args, unsigned long);
+					((unsigned long *)&tmp)[1] =
+						va_arg(args, unsigned long);
+					acnt += 2 * sizeof(unsigned long);
+				}
+				if (*(format + 2) == 'u') {
+					format += 2;
+					pc += printi(out, out_len, tmp, 10, 0,
+						     width, flags, 'a');
+				} else if (*(format + 2) == 'x') {
+					format += 2;
+					pc += printi(out, out_len, tmp, 16, 0,
+						     width, flags, 'a');
+				} else if (*(format + 2) == 'X') {
+					format += 2;
+					pc += printi(out, out_len, tmp, 16, 0,
+						     width, flags, 'A');
+				} else {
+					format += 1;
+					pc += printi(out, out_len, tmp, 10, 1,
+						     width, flags, '0');
+				}
+				continue;
+			} else if (*format == 'l') {
+				if (*(format + 1) == 'u') {
+					format += 1;
+					pc += printi(
+						out, out_len,
+						va_arg(args, unsigned long), 10,
+						0, width, flags, 'a');
+				} else if (*(format + 1) == 'x') {
+					format += 1;
+					pc += printi(
+						out, out_len,
+						va_arg(args, unsigned long), 16,
+						0, width, flags, 'a');
+					acnt += sizeof(unsigned long);
+				} else if (*(format + 1) == 'X') {
+					format += 1;
+					pc += printi(
+						out, out_len,
+						va_arg(args, unsigned long), 16,
+						0, width, flags, 'A');
+					acnt += sizeof(unsigned long);
+				} else {
+					pc += printi(out, out_len,
+						     va_arg(args, long), 10, 1,
+						     width, flags, '0');
+					acnt += sizeof(long);
+				}
+			}
+			if (*format == 'c') {
+				/* char are converted to int then pushed on the stack */
+				scr[0] = va_arg(args, int);
+				scr[1] = '\0';
+				pc += prints(out, out_len, scr, width, flags);
+				acnt += sizeof(int);
+				continue;
+			}
+		} else {
+		out:
+			printc(out, out_len, *format);
+			++pc;
+		}
+	}
+	if (out)
+		**out = '\0';
+
+	return pc;
+}
+
+int sbi_sprintf(char *out, const char *format, ...)
+{
+	va_list args;
+	int retval;
+
+	va_start(args, format);
+	retval = print(&out, NULL, format, args);
+	va_end(args);
+
+	return retval;
+}
+
+int sbi_snprintf(char *out, u32 out_sz, const char *format, ...)
+{
+	va_list args;
+	int retval;
+
+	va_start(args, format);
+	retval = print(&out, &out_sz, format, args);
+	va_end(args);
+
+	return retval;
+}
+
+int sbi_printf(const char *format, ...)
+{
+	va_list args;
+	int retval;
+
+	spin_lock(&console_out_lock);
+	va_start(args, format);
+	retval = print(NULL, NULL, format, args);
+	va_end(args);
+	spin_unlock(&console_out_lock);
+
+	return retval;
+}
+
+int sbi_dprintf(const char *format, ...)
+{
+	va_list args;
+	int retval = 0;
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+
+	va_start(args, format);
+	if (scratch->options & SBI_SCRATCH_DEBUG_PRINTS)
+		retval = print(NULL, NULL, format, args);
+	va_end(args);
+
+	return retval;
+}
+
+const struct sbi_console_device *sbi_console_get_device(void)
+{
+	return console_dev;
+}
+
+void sbi_console_set_device(const struct sbi_console_device *dev)
+{
+	if (!dev || console_dev)
+		return;
+
+	console_dev = dev;
+}
+
+int sbi_console_init(struct sbi_scratch *scratch)
+{
+	return sbi_platform_console_init(sbi_platform_ptr(scratch));
+}

lib/sbi/sbi_domain.c

@@ -0,0 +1,629 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_domain.h>
+#include <sbi/sbi_hartmask.h>
+#include <sbi/sbi_hsm.h>
+#include <sbi/sbi_math.h>
+#include <sbi/sbi_platform.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi/sbi_string.h>
+
+struct sbi_domain *hartid_to_domain_table[SBI_HARTMASK_MAX_BITS] = { 0 };
+struct sbi_domain *domidx_to_domain_table[SBI_DOMAIN_MAX_INDEX] = { 0 };
+static u32 domain_count = 0;
+static bool domain_finalized = false;
+
+static struct sbi_hartmask root_hmask = { 0 };
+
+#define ROOT_REGION_MAX	16
+static u32 root_memregs_count = 0;
+static struct sbi_domain_memregion root_fw_region;
+static struct sbi_domain_memregion root_memregs[ROOT_REGION_MAX + 1] = { 0 };
+
+struct sbi_domain root = {
+	.name = "root",
+	.possible_harts = &root_hmask,
+	.regions = root_memregs,
+	.system_reset_allowed = TRUE,
+};
+
+bool sbi_domain_is_assigned_hart(const struct sbi_domain *dom, u32 hartid)
+{
+	if (dom)
+		return sbi_hartmask_test_hart(hartid, &dom->assigned_harts);
+
+	return FALSE;
+}
+
+ulong sbi_domain_get_assigned_hartmask(const struct sbi_domain *dom,
+				       ulong hbase)
+{
+	ulong ret, bword, boff;
+
+	if (!dom)
+		return 0;
+
+	bword = BIT_WORD(hbase);
+	boff = BIT_WORD_OFFSET(hbase);
+
+	ret = sbi_hartmask_bits(&dom->assigned_harts)[bword++] >> boff;
+	if (boff && bword < BIT_WORD(SBI_HARTMASK_MAX_BITS)) {
+		ret |= (sbi_hartmask_bits(&dom->assigned_harts)[bword] &
+			(BIT(boff) - 1UL)) << (BITS_PER_LONG - boff);
+	}
+
+	return ret;
+}
+
+static void domain_memregion_initfw(struct sbi_domain_memregion *reg)
+{
+	if (!reg)
+		return;
+
+	sbi_memcpy(reg, &root_fw_region, sizeof(*reg));
+}
+
+void sbi_domain_memregion_init(unsigned long addr,
+				unsigned long size,
+				unsigned long flags,
+				struct sbi_domain_memregion *reg)
+{
+	unsigned long base = 0, order;
+
+	for (order = log2roundup(size) ; order <= __riscv_xlen; order++) {
+		if (order < __riscv_xlen) {
+			base = addr & ~((1UL << order) - 1UL);
+			if ((base <= addr) &&
+			    (addr < (base + (1UL << order))) &&
+			    (base <= (addr + size - 1UL)) &&
+			    ((addr + size - 1UL) < (base + (1UL << order))))
+				break;
+		} else {
+			base = 0;
+			break;
+		}
+
+	}
+
+	if (reg) {
+		reg->base = base;
+		reg->order = order;
+		reg->flags = flags;
+	}
+}
+
+bool sbi_domain_check_addr(const struct sbi_domain *dom,
+			   unsigned long addr, unsigned long mode,
+			   unsigned long access_flags)
+{
+	bool mmio = FALSE;
+	struct sbi_domain_memregion *reg;
+	unsigned long rstart, rend, rflags, rwx = 0;
+
+	if (!dom)
+		return FALSE;
+
+	if (access_flags & SBI_DOMAIN_READ)
+		rwx |= SBI_DOMAIN_MEMREGION_READABLE;
+	if (access_flags & SBI_DOMAIN_WRITE)
+		rwx |= SBI_DOMAIN_MEMREGION_WRITEABLE;
+	if (access_flags & SBI_DOMAIN_EXECUTE)
+		rwx |= SBI_DOMAIN_MEMREGION_EXECUTABLE;
+	if (access_flags & SBI_DOMAIN_MMIO)
+		mmio = TRUE;
+
+	sbi_domain_for_each_memregion(dom, reg) {
+		rflags = reg->flags;
+		if (mode == PRV_M && !(rflags & SBI_DOMAIN_MEMREGION_MMODE))
+			continue;
+
+		rstart = reg->base;
+		rend = (reg->order < __riscv_xlen) ?
+			rstart + ((1UL << reg->order) - 1) : -1UL;
+		if (rstart <= addr && addr <= rend) {
+			if ((mmio && !(rflags & SBI_DOMAIN_MEMREGION_MMIO)) ||
+			    (!mmio && (rflags & SBI_DOMAIN_MEMREGION_MMIO)))
+				return FALSE;
+			return ((rflags & rwx) == rwx) ? TRUE : FALSE;
+		}
+	}
+
+	return (mode == PRV_M) ? TRUE : FALSE;
+}
+
+/* Check if region complies with constraints */
+static bool is_region_valid(const struct sbi_domain_memregion *reg)
+{
+	if (reg->order < 3 || __riscv_xlen < reg->order)
+		return FALSE;
+
+	if (reg->base & (BIT(reg->order) - 1))
+		return FALSE;
+
+	return TRUE;
+}
+
+/** Check if regionA is sub-region of regionB */
+static bool is_region_subset(const struct sbi_domain_memregion *regA,
+			     const struct sbi_domain_memregion *regB)
+{
+	ulong regA_start = regA->base;
+	ulong regA_end = regA->base + (BIT(regA->order) - 1);
+	ulong regB_start = regB->base;
+	ulong regB_end = regB->base + (BIT(regA->order) - 1);
+
+	if ((regB_start <= regA_start) &&
+	    (regA_start < regB_end) &&
+	    (regB_start < regA_end) &&
+	    (regA_end <= regB_end))
+		return TRUE;
+
+	return FALSE;
+}
+
+/** Check if regionA conflicts regionB */
+static bool is_region_conflict(const struct sbi_domain_memregion *regA,
+				const struct sbi_domain_memregion *regB)
+{
+	if ((is_region_subset(regA, regB) || is_region_subset(regB, regA)) &&
+	    regA->flags == regB->flags)
+		return TRUE;
+
+	return FALSE;
+}
+
+/** Check if regionA should be placed before regionB */
+static bool is_region_before(const struct sbi_domain_memregion *regA,
+			     const struct sbi_domain_memregion *regB)
+{
+	if (regA->order < regB->order)
+		return TRUE;
+
+	if ((regA->order == regB->order) &&
+	    (regA->base < regB->base))
+		return TRUE;
+
+	return FALSE;
+}
+
+static int sanitize_domain(const struct sbi_platform *plat,
+			   struct sbi_domain *dom)
+{
+	u32 i, j, count;
+	bool have_fw_reg;
+	struct sbi_domain_memregion treg, *reg, *reg1;
+
+	/* Check possible HARTs */
+	if (!dom->possible_harts) {
+		sbi_printf("%s: %s possible HART mask is NULL\n",
+			   __func__, dom->name);
+		return SBI_EINVAL;
+	}
+	sbi_hartmask_for_each_hart(i, dom->possible_harts) {
+		if (sbi_platform_hart_invalid(plat, i)) {
+			sbi_printf("%s: %s possible HART mask has invalid "
+				   "hart %d\n", __func__, dom->name, i);
+			return SBI_EINVAL;
+		}
+	};
+
+	/* Check memory regions */
+	if (!dom->regions) {
+		sbi_printf("%s: %s regions is NULL\n",
+			   __func__, dom->name);
+		return SBI_EINVAL;
+	}
+	sbi_domain_for_each_memregion(dom, reg) {
+		if (!is_region_valid(reg)) {
+			sbi_printf("%s: %s has invalid region base=0x%lx "
+				   "order=%lu flags=0x%lx\n", __func__,
+				   dom->name, reg->base, reg->order,
+				   reg->flags);
+			return SBI_EINVAL;
+		}
+	}
+
+	/* Count memory regions and check presence of firmware region */
+	count = 0;
+	have_fw_reg = FALSE;
+	sbi_domain_for_each_memregion(dom, reg) {
+		if (reg->order == root_fw_region.order &&
+		    reg->base == root_fw_region.base &&
+		    reg->flags == root_fw_region.flags)
+			have_fw_reg = TRUE;
+		count++;
+	}
+	if (!have_fw_reg) {
+		sbi_printf("%s: %s does not have firmware region\n",
+			   __func__, dom->name);
+		return SBI_EINVAL;
+	}
+
+	/* Sort the memory regions */
+	for (i = 0; i < (count - 1); i++) {
+		reg = &dom->regions[i];
+		for (j = i + 1; j < count; j++) {
+			reg1 = &dom->regions[j];
+
+			if (is_region_conflict(reg1, reg)) {
+				sbi_printf("%s: %s conflict between regions "
+					"(base=0x%lx order=%lu flags=0x%lx) and "
+					"(base=0x%lx order=%lu flags=0x%lx)\n",
+					__func__, dom->name,
+					reg->base, reg->order, reg->flags,
+					reg1->base, reg1->order, reg1->flags);
+				return SBI_EINVAL;
+			}
+
+			if (!is_region_before(reg1, reg))
+				continue;
+
+			sbi_memcpy(&treg, reg1, sizeof(treg));
+			sbi_memcpy(reg1, reg, sizeof(treg));
+			sbi_memcpy(reg, &treg, sizeof(treg));
+		}
+	}
+
+	/*
+	 * We don't need to check boot HART id of domain because if boot
+	 * HART id is not possible/assigned to this domain then it won't
+	 * be started at boot-time by sbi_domain_finalize().
+	 */
+
+	/*
+	 * Check next mode
+	 *
+	 * We only allow next mode to be S-mode or U-mode.so that we can
+	 * protect M-mode context and enforce checks on memory accesses.
+	 */
+	if (dom->next_mode != PRV_S &&
+	    dom->next_mode != PRV_U) {
+		sbi_printf("%s: %s invalid next booting stage mode 0x%lx\n",
+			   __func__, dom->name, dom->next_mode);
+		return SBI_EINVAL;
+	}
+
+	/* Check next address and next mode*/
+	if (!sbi_domain_check_addr(dom, dom->next_addr, dom->next_mode,
+				   SBI_DOMAIN_EXECUTE)) {
+		sbi_printf("%s: %s next booting stage addres 0x%lx can't "
+			   "execute\n", __func__, dom->name, dom->next_addr);
+		return SBI_EINVAL;
+	}
+
+	return 0;
+}
+
+void sbi_domain_dump(const struct sbi_domain *dom, const char *suffix)
+{
+	u32 i, k;
+	unsigned long rstart, rend;
+	struct sbi_domain_memregion *reg;
+
+	sbi_printf("Domain%d Name        %s: %s\n",
+		   dom->index, suffix, dom->name);
+
+	sbi_printf("Domain%d Boot HART   %s: %d\n",
+		   dom->index, suffix, dom->boot_hartid);
+
+	k = 0;
+	sbi_printf("Domain%d HARTs       %s: ", dom->index, suffix);
+	sbi_hartmask_for_each_hart(i, dom->possible_harts)
+		sbi_printf("%s%d%s", (k++) ? "," : "",
+			   i, sbi_domain_is_assigned_hart(dom, i) ? "*" : "");
+	sbi_printf("\n");
+
+	i = 0;
+	sbi_domain_for_each_memregion(dom, reg) {
+		rstart = reg->base;
+		rend = (reg->order < __riscv_xlen) ?
+			rstart + ((1UL << reg->order) - 1) : -1UL;
+
+#if __riscv_xlen == 32
+		sbi_printf("Domain%d Region%02d    %s: 0x%08lx-0x%08lx ",
+#else
+		sbi_printf("Domain%d Region%02d    %s: 0x%016lx-0x%016lx ",
+#endif
+			   dom->index, i, suffix, rstart, rend);
+
+		k = 0;
+		if (reg->flags & SBI_DOMAIN_MEMREGION_MMODE)
+			sbi_printf("%cM", (k++) ? ',' : '(');
+		if (reg->flags & SBI_DOMAIN_MEMREGION_MMIO)
+			sbi_printf("%cI", (k++) ? ',' : '(');
+		if (reg->flags & SBI_DOMAIN_MEMREGION_READABLE)
+			sbi_printf("%cR", (k++) ? ',' : '(');
+		if (reg->flags & SBI_DOMAIN_MEMREGION_WRITEABLE)
+			sbi_printf("%cW", (k++) ? ',' : '(');
+		if (reg->flags & SBI_DOMAIN_MEMREGION_EXECUTABLE)
+			sbi_printf("%cX", (k++) ? ',' : '(');
+		sbi_printf("%s\n", (k++) ? ")" : "()");
+
+		i++;
+	}
+
+#if __riscv_xlen == 32
+	sbi_printf("Domain%d Next Address%s: 0x%08lx\n",
+#else
+	sbi_printf("Domain%d Next Address%s: 0x%016lx\n",
+#endif
+		   dom->index, suffix, dom->next_addr);
+
+#if __riscv_xlen == 32
+	sbi_printf("Domain%d Next Arg1   %s: 0x%08lx\n",
+#else
+	sbi_printf("Domain%d Next Arg1   %s: 0x%016lx\n",
+#endif
+		   dom->index, suffix, dom->next_arg1);
+
+	sbi_printf("Domain%d Next Mode   %s: ", dom->index, suffix);
+	switch (dom->next_mode) {
+	case PRV_M:
+		sbi_printf("M-mode\n");
+		break;
+	case PRV_S:
+		sbi_printf("S-mode\n");
+		break;
+	case PRV_U:
+		sbi_printf("U-mode\n");
+		break;
+	default:
+		sbi_printf("Unknown\n");
+		break;
+	};
+
+	sbi_printf("Domain%d SysReset    %s: %s\n",
+		   dom->index, suffix, (dom->system_reset_allowed) ? "yes" : "no");
+}
+
+void sbi_domain_dump_all(const char *suffix)
+{
+	u32 i;
+	const struct sbi_domain *dom;
+
+	sbi_domain_for_each(i, dom) {
+		sbi_domain_dump(dom, suffix);
+		sbi_printf("\n");
+	}
+}
+
+int sbi_domain_register(struct sbi_domain *dom,
+			const struct sbi_hartmask *assign_mask)
+{
+	u32 i;
+	int rc;
+	struct sbi_domain *tdom;
+	u32 cold_hartid = current_hartid();
+	const struct sbi_platform *plat = sbi_platform_thishart_ptr();
+
+	/* Sanity checks */
+	if (!dom || !assign_mask || domain_finalized)
+		return SBI_EINVAL;
+
+	/* Check if domain already discovered */
+	sbi_domain_for_each(i, tdom) {
+		if (tdom == dom)
+			return SBI_EALREADY;
+	}
+
+	/*
+	 * Ensure that we have room for Domain Index to
+	 * HART ID mapping
+	 */
+	if (SBI_DOMAIN_MAX_INDEX <= domain_count) {
+		sbi_printf("%s: No room for %s\n",
+			   __func__, dom->name);
+		return SBI_ENOSPC;
+	}
+
+	/* Sanitize discovered domain */
+	rc = sanitize_domain(plat, dom);
+	if (rc) {
+		sbi_printf("%s: sanity checks failed for"
+			   " %s (error %d)\n", __func__,
+			   dom->name, rc);
+		return rc;
+	}
+
+	/* Assign index to domain */
+	dom->index = domain_count++;
+	domidx_to_domain_table[dom->index] = dom;
+
+	/* Clear assigned HARTs of domain */
+	sbi_hartmask_clear_all(&dom->assigned_harts);
+
+	/* Assign domain to HART if HART is a possible HART */
+	sbi_hartmask_for_each_hart(i, assign_mask) {
+		if (!sbi_hartmask_test_hart(i, dom->possible_harts))
+			continue;
+
+		tdom = hartid_to_domain_table[i];
+		if (tdom)
+			sbi_hartmask_clear_hart(i,
+					&tdom->assigned_harts);
+		hartid_to_domain_table[i] = dom;
+		sbi_hartmask_set_hart(i, &dom->assigned_harts);
+
+		/*
+		 * If cold boot HART is assigned to this domain then
+		 * override boot HART of this domain.
+		 */
+		if (i == cold_hartid &&
+		    dom->boot_hartid != cold_hartid) {
+			sbi_printf("Domain%d Boot HARTID forced to"
+				   " %d\n", dom->index, cold_hartid);
+			dom->boot_hartid = cold_hartid;
+		}
+	}
+
+	return 0;
+}
+
+int sbi_domain_root_add_memregion(const struct sbi_domain_memregion *reg)
+{
+	int rc;
+	bool reg_merged;
+	struct sbi_domain_memregion *nreg, *nreg1, *nreg2;
+	const struct sbi_platform *plat = sbi_platform_thishart_ptr();
+
+	/* Sanity checks */
+	if (!reg || domain_finalized ||
+	    (root.regions != root_memregs) ||
+	    (ROOT_REGION_MAX <= root_memregs_count))
+		return SBI_EINVAL;
+
+	/* Check for conflicts */
+	sbi_domain_for_each_memregion(&root, nreg) {
+		if (is_region_conflict(reg, nreg))
+			return SBI_EINVAL;
+	}
+
+	/* Append the memregion to root memregions */
+	nreg = &root_memregs[root_memregs_count];
+	sbi_memcpy(nreg, reg, sizeof(*reg));
+	root_memregs_count++;
+	root_memregs[root_memregs_count].order = 0;
+
+	/* Sort and optimize root regions */
+	do {
+		/* Sanitize the root domain so that memregions are sorted */
+		rc = sanitize_domain(plat, &root);
+		if (rc) {
+			sbi_printf("%s: sanity checks failed for"
+				   " %s (error %d)\n", __func__,
+				   root.name, rc);
+			return rc;
+		}
+
+		/* Merge consecutive memregions with same order and flags */
+		reg_merged = false;
+		sbi_domain_for_each_memregion(&root, nreg) {
+			nreg1 = nreg + 1;
+			if (!nreg1->order)
+				continue;
+
+			if ((nreg->base + BIT(nreg->order)) == nreg1->base &&
+			    nreg->order == nreg1->order &&
+			    nreg->flags == nreg1->flags) {
+				nreg->order++;
+				while (nreg1->order) {
+					nreg2 = nreg1 + 1;
+					sbi_memcpy(nreg1, nreg2, sizeof(*nreg1));
+					nreg1++;
+				}
+				reg_merged = true;
+				root_memregs_count--;
+			}
+		}
+	} while (reg_merged);
+
+	return 0;
+}
+
+int sbi_domain_finalize(struct sbi_scratch *scratch, u32 cold_hartid)
+{
+	int rc;
+	u32 i, dhart;
+	struct sbi_domain *dom;
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+
+	/* Initialize and populate domains for the platform */
+	rc = sbi_platform_domains_init(plat);
+	if (rc) {
+		sbi_printf("%s: platform domains_init() failed (error %d)\n",
+			   __func__, rc);
+		return rc;
+	}
+
+	/* Startup boot HART of domains */
+	sbi_domain_for_each(i, dom) {
+		/* Domain boot HART */
+		dhart = dom->boot_hartid;
+
+		/* Ignore of boot HART is off limits */
+		if (SBI_HARTMASK_MAX_BITS <= dhart)
+			continue;
+
+		/* Ignore if boot HART not possible for this domain */
+		if (!sbi_hartmask_test_hart(dhart, dom->possible_harts))
+			continue;
+
+		/* Ignore if boot HART assigned different domain */
+		if (sbi_hartid_to_domain(dhart) != dom ||
+		    !sbi_hartmask_test_hart(dhart, &dom->assigned_harts))
+			continue;
+
+		/* Startup boot HART of domain */
+		if (dhart == cold_hartid) {
+			scratch->next_addr = dom->next_addr;
+			scratch->next_mode = dom->next_mode;
+			scratch->next_arg1 = dom->next_arg1;
+		} else {
+			rc = sbi_hsm_hart_start(scratch, NULL, dhart,
+						dom->next_addr,
+						dom->next_mode,
+						dom->next_arg1);
+			if (rc) {
+				sbi_printf("%s: failed to start boot HART %d"
+					   " for %s (error %d)\n", __func__,
+					   dhart, dom->name, rc);
+				return rc;
+			}
+		}
+	}
+
+	/*
+	 * Set the finalized flag so that the root domain
+	 * regions can't be changed.
+	 */
+	domain_finalized = true;
+
+	return 0;
+}
+
+int sbi_domain_init(struct sbi_scratch *scratch, u32 cold_hartid)
+{
+	u32 i;
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+
+	/* Root domain firmware memory region */
+	sbi_domain_memregion_init(scratch->fw_start, scratch->fw_size, 0,
+				  &root_fw_region);
+	domain_memregion_initfw(&root_memregs[root_memregs_count++]);
+
+	/* Root domain allow everything memory region */
+	sbi_domain_memregion_init(0, ~0UL,
+				  (SBI_DOMAIN_MEMREGION_READABLE |
+				   SBI_DOMAIN_MEMREGION_WRITEABLE |
+				   SBI_DOMAIN_MEMREGION_EXECUTABLE),
+				  &root_memregs[root_memregs_count++]);
+
+	/* Root domain memory region end */
+	root_memregs[root_memregs_count].order = 0;
+
+	/* Root domain boot HART id is same as coldboot HART id */
+	root.boot_hartid = cold_hartid;
+
+	/* Root domain next booting stage details */
+	root.next_arg1 = scratch->next_arg1;
+	root.next_addr = scratch->next_addr;
+	root.next_mode = scratch->next_mode;
+
+	/* Root domain possible and assigned HARTs */
+	for (i = 0; i < SBI_HARTMASK_MAX_BITS; i++) {
+		if (sbi_platform_hart_invalid(plat, i))
+			continue;
+		sbi_hartmask_set_hart(i, &root_hmask);
+	}
+
+	return sbi_domain_register(&root, &root_hmask);
+}

lib/sbi/sbi_ecall.c

@@ -0,0 +1,175 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_ecall.h>
+#include <sbi/sbi_ecall_interface.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_trap.h>
+
+u16 sbi_ecall_version_major(void)
+{
+	return SBI_ECALL_VERSION_MAJOR;
+}
+
+u16 sbi_ecall_version_minor(void)
+{
+	return SBI_ECALL_VERSION_MINOR;
+}
+
+static unsigned long ecall_impid = SBI_OPENSBI_IMPID;
+
+unsigned long sbi_ecall_get_impid(void)
+{
+	return ecall_impid;
+}
+
+void sbi_ecall_set_impid(unsigned long impid)
+{
+	ecall_impid = impid;
+}
+
+static SBI_LIST_HEAD(ecall_exts_list);
+
+struct sbi_ecall_extension *sbi_ecall_find_extension(unsigned long extid)
+{
+	struct sbi_ecall_extension *t, *ret = NULL;
+
+	sbi_list_for_each_entry(t, &ecall_exts_list, head) {
+		if (t->extid_start <= extid && extid <= t->extid_end) {
+			ret = t;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+int sbi_ecall_register_extension(struct sbi_ecall_extension *ext)
+{
+	struct sbi_ecall_extension *t;
+
+	if (!ext || (ext->extid_end < ext->extid_start) || !ext->handle)
+		return SBI_EINVAL;
+
+	sbi_list_for_each_entry(t, &ecall_exts_list, head) {
+		unsigned long start = t->extid_start;
+		unsigned long end = t->extid_end;
+		if (end < ext->extid_start || ext->extid_end < start)
+			/* no overlap */;
+		else
+			return SBI_EINVAL;
+	}
+
+	SBI_INIT_LIST_HEAD(&ext->head);
+	sbi_list_add_tail(&ext->head, &ecall_exts_list);
+
+	return 0;
+}
+
+void sbi_ecall_unregister_extension(struct sbi_ecall_extension *ext)
+{
+	bool found = FALSE;
+	struct sbi_ecall_extension *t;
+
+	if (!ext)
+		return;
+
+	sbi_list_for_each_entry(t, &ecall_exts_list, head) {
+		if (t == ext) {
+			found = TRUE;
+			break;
+		}
+	}
+
+	if (found)
+		sbi_list_del_init(&ext->head);
+}
+
+int sbi_ecall_handler(struct sbi_trap_regs *regs)
+{
+	int ret = 0;
+	struct sbi_ecall_extension *ext;
+	unsigned long extension_id = regs->a7;
+	unsigned long func_id = regs->a6;
+	struct sbi_trap_info trap = {0};
+	unsigned long out_val = 0;
+	bool is_0_1_spec = 0;
+
+	ext = sbi_ecall_find_extension(extension_id);
+	if (ext && ext->handle) {
+		ret = ext->handle(extension_id, func_id,
+				  regs, &out_val, &trap);
+		if (extension_id >= SBI_EXT_0_1_SET_TIMER &&
+		    extension_id <= SBI_EXT_0_1_SHUTDOWN)
+			is_0_1_spec = 1;
+	} else {
+		ret = SBI_ENOTSUPP;
+	}
+
+	if (ret == SBI_ETRAP) {
+		trap.epc = regs->mepc;
+		sbi_trap_redirect(regs, &trap);
+	} else {
+		if (ret < SBI_LAST_ERR) {
+			sbi_printf("%s: Invalid error %d for ext=0x%lx "
+				   "func=0x%lx\n", __func__, ret,
+				   extension_id, func_id);
+			ret = SBI_ERR_FAILED;
+		}
+
+		/*
+		 * This function should return non-zero value only in case of
+		 * fatal error. However, there is no good way to distinguish
+		 * between a fatal and non-fatal errors yet. That's why we treat
+		 * every return value except ETRAP as non-fatal and just return
+		 * accordingly for now. Once fatal errors are defined, that
+		 * case should be handled differently.
+		 */
+		regs->mepc += 4;
+		regs->a0 = ret;
+		if (!is_0_1_spec)
+			regs->a1 = out_val;
+	}
+
+	return 0;
+}
+
+int sbi_ecall_init(void)
+{
+	int ret;
+
+	/* The order of below registrations is performance optimized */
+	ret = sbi_ecall_register_extension(&ecall_time);
+	if (ret)
+		return ret;
+	ret = sbi_ecall_register_extension(&ecall_rfence);
+	if (ret)
+		return ret;
+	ret = sbi_ecall_register_extension(&ecall_ipi);
+	if (ret)
+		return ret;
+	ret = sbi_ecall_register_extension(&ecall_base);
+	if (ret)
+		return ret;
+	ret = sbi_ecall_register_extension(&ecall_hsm);
+	if (ret)
+		return ret;
+	ret = sbi_ecall_register_extension(&ecall_srst);
+	if (ret)
+		return ret;
+	ret = sbi_ecall_register_extension(&ecall_legacy);
+	if (ret)
+		return ret;
+	ret = sbi_ecall_register_extension(&ecall_vendor);
+	if (ret)
+		return ret;
+
+	return 0;
+}

lib/sbi/sbi_ecall_base.c

@@ -0,0 +1,79 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ *   Atish Patra <atish.patra@wdc.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_version.h>
+#include <sbi/riscv_asm.h>
+
+static int sbi_ecall_base_probe(unsigned long extid, unsigned long *out_val)
+{
+	struct sbi_ecall_extension *ext;
+
+	ext = sbi_ecall_find_extension(extid);
+	if (!ext) {
+		*out_val = 0;
+		return 0;
+	}
+
+	if (ext->probe)
+		return ext->probe(extid, out_val);
+
+	*out_val = 1;
+	return 0;
+}
+
+static int sbi_ecall_base_handler(unsigned long extid, unsigned long funcid,
+				  const struct sbi_trap_regs *regs,
+				  unsigned long *out_val,
+				  struct sbi_trap_info *out_trap)
+{
+	int ret = 0;
+
+	switch (funcid) {
+	case SBI_EXT_BASE_GET_SPEC_VERSION:
+		*out_val = (SBI_ECALL_VERSION_MAJOR <<
+			   SBI_SPEC_VERSION_MAJOR_OFFSET) &
+			   (SBI_SPEC_VERSION_MAJOR_MASK <<
+			    SBI_SPEC_VERSION_MAJOR_OFFSET);
+		*out_val = *out_val | SBI_ECALL_VERSION_MINOR;
+		break;
+	case SBI_EXT_BASE_GET_IMP_ID:
+		*out_val = sbi_ecall_get_impid();
+		break;
+	case SBI_EXT_BASE_GET_IMP_VERSION:
+		*out_val = OPENSBI_VERSION;
+		break;
+	case SBI_EXT_BASE_GET_MVENDORID:
+		*out_val = csr_read(CSR_MVENDORID);
+		break;
+	case SBI_EXT_BASE_GET_MARCHID:
+		*out_val = csr_read(CSR_MARCHID);
+		break;
+	case SBI_EXT_BASE_GET_MIMPID:
+		*out_val = csr_read(CSR_MIMPID);
+		break;
+	case SBI_EXT_BASE_PROBE_EXT:
+		ret = sbi_ecall_base_probe(regs->a0, out_val);
+		break;
+	default:
+		ret = SBI_ENOTSUPP;
+	}
+
+	return ret;
+}
+
+struct sbi_ecall_extension ecall_base = {
+	.extid_start = SBI_EXT_BASE,
+	.extid_end = SBI_EXT_BASE,
+	.handle = sbi_ecall_base_handler,
+};

lib/sbi/sbi_ecall_hsm.c

@@ -0,0 +1,61 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Atish Patra <atish.patra@wdc.com>
+ */
+
+#include <sbi/sbi_domain.h>
+#include <sbi/sbi_ecall.h>
+#include <sbi/sbi_ecall_interface.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_trap.h>
+#include <sbi/sbi_version.h>
+#include <sbi/sbi_hsm.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi/riscv_asm.h>
+
+static int sbi_ecall_hsm_handler(unsigned long extid, unsigned long funcid,
+				 const struct sbi_trap_regs *regs,
+				 unsigned long *out_val,
+				 struct sbi_trap_info *out_trap)
+{
+	int ret = 0;
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+	ulong smode = (csr_read(CSR_MSTATUS) & MSTATUS_MPP) >>
+			MSTATUS_MPP_SHIFT;
+
+	switch (funcid) {
+	case SBI_EXT_HSM_HART_START:
+		ret = sbi_hsm_hart_start(scratch, sbi_domain_thishart_ptr(),
+					 regs->a0, regs->a1, smode, regs->a2);
+		break;
+	case SBI_EXT_HSM_HART_STOP:
+		ret = sbi_hsm_hart_stop(scratch, TRUE);
+		break;
+	case SBI_EXT_HSM_HART_GET_STATUS:
+		ret = sbi_hsm_hart_get_state(sbi_domain_thishart_ptr(),
+					     regs->a0);
+		break;
+	case SBI_EXT_HSM_HART_SUSPEND:
+		ret = sbi_hsm_hart_suspend(scratch, regs->a0, regs->a1,
+					   smode, regs->a2);
+		break;
+	default:
+		ret = SBI_ENOTSUPP;
+	};
+	if (ret >= 0) {
+		*out_val = ret;
+		ret = 0;
+	}
+
+	return ret;
+}
+
+struct sbi_ecall_extension ecall_hsm = {
+	.extid_start = SBI_EXT_HSM,
+	.extid_end = SBI_EXT_HSM,
+	.handle = sbi_ecall_hsm_handler,
+};

lib/sbi/sbi_ecall_legacy.c

@@ -0,0 +1,124 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ *   Atish Patra <atish.patra@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_domain.h>
+#include <sbi/sbi_ecall.h>
+#include <sbi/sbi_ecall_interface.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_hsm.h>
+#include <sbi/sbi_ipi.h>
+#include <sbi/sbi_platform.h>
+#include <sbi/sbi_system.h>
+#include <sbi/sbi_timer.h>
+#include <sbi/sbi_tlb.h>
+#include <sbi/sbi_trap.h>
+#include <sbi/sbi_unpriv.h>
+#include <sbi/sbi_hart.h>
+
+static int sbi_load_hart_mask_unpriv(ulong *pmask, ulong *hmask,
+				     struct sbi_trap_info *uptrap)
+{
+	ulong mask = 0;
+
+	if (pmask) {
+		mask = sbi_load_ulong(pmask, uptrap);
+		if (uptrap->cause)
+			return SBI_ETRAP;
+	} else {
+		sbi_hsm_hart_interruptible_mask(sbi_domain_thishart_ptr(),
+						0, &mask);
+	}
+	*hmask = mask;
+
+	return 0;
+}
+
+static int sbi_ecall_legacy_handler(unsigned long extid, unsigned long funcid,
+				    const struct sbi_trap_regs *regs,
+				    unsigned long *out_val,
+				    struct sbi_trap_info *out_trap)
+{
+	int ret = 0;
+	struct sbi_tlb_info tlb_info;
+	u32 source_hart = current_hartid();
+	ulong hmask = 0;
+
+	switch (extid) {
+	case SBI_EXT_0_1_SET_TIMER:
+#if __riscv_xlen == 32
+		sbi_timer_event_start((((u64)regs->a1 << 32) | (u64)regs->a0));
+#else
+		sbi_timer_event_start((u64)regs->a0);
+#endif
+		break;
+	case SBI_EXT_0_1_CONSOLE_PUTCHAR:
+		sbi_putc(regs->a0);
+		break;
+	case SBI_EXT_0_1_CONSOLE_GETCHAR:
+		ret = sbi_getc();
+		break;
+	case SBI_EXT_0_1_CLEAR_IPI:
+		sbi_ipi_clear_smode();
+		break;
+	case SBI_EXT_0_1_SEND_IPI:
+		ret = sbi_load_hart_mask_unpriv((ulong *)regs->a0,
+						&hmask, out_trap);
+		if (ret != SBI_ETRAP)
+			ret = sbi_ipi_send_smode(hmask, 0);
+		break;
+	case SBI_EXT_0_1_REMOTE_FENCE_I:
+		ret = sbi_load_hart_mask_unpriv((ulong *)regs->a0,
+						&hmask, out_trap);
+		if (ret != SBI_ETRAP) {
+			SBI_TLB_INFO_INIT(&tlb_info, 0, 0, 0, 0,
+					  sbi_tlb_local_fence_i,
+					  source_hart);
+			ret = sbi_tlb_request(hmask, 0, &tlb_info);
+		}
+		break;
+	case SBI_EXT_0_1_REMOTE_SFENCE_VMA:
+		ret = sbi_load_hart_mask_unpriv((ulong *)regs->a0,
+						&hmask, out_trap);
+		if (ret != SBI_ETRAP) {
+			SBI_TLB_INFO_INIT(&tlb_info, regs->a1, regs->a2, 0, 0,
+					  sbi_tlb_local_sfence_vma,
+					  source_hart);
+			ret = sbi_tlb_request(hmask, 0, &tlb_info);
+		}
+		break;
+	case SBI_EXT_0_1_REMOTE_SFENCE_VMA_ASID:
+		ret = sbi_load_hart_mask_unpriv((ulong *)regs->a0,
+						&hmask, out_trap);
+		if (ret != SBI_ETRAP) {
+			SBI_TLB_INFO_INIT(&tlb_info, regs->a1,
+					  regs->a2, regs->a3, 0,
+					  sbi_tlb_local_sfence_vma_asid,
+					  source_hart);
+			ret = sbi_tlb_request(hmask, 0, &tlb_info);
+		}
+		break;
+	case SBI_EXT_0_1_SHUTDOWN:
+		sbi_system_reset(SBI_SRST_RESET_TYPE_SHUTDOWN,
+				 SBI_SRST_RESET_REASON_NONE);
+		break;
+	default:
+		ret = SBI_ENOTSUPP;
+	};
+
+	return ret;
+}
+
+struct sbi_ecall_extension ecall_legacy = {
+	.extid_start = SBI_EXT_0_1_SET_TIMER,
+	.extid_end = SBI_EXT_0_1_SHUTDOWN,
+	.handle = sbi_ecall_legacy_handler,
+};

lib/sbi/sbi_ecall_replace.c

@@ -0,0 +1,196 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ *   Atish Patra <atish.patra@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/sbi_ecall.h>
+#include <sbi/sbi_ecall_interface.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_ipi.h>
+#include <sbi/sbi_system.h>
+#include <sbi/sbi_timer.h>
+#include <sbi/sbi_tlb.h>
+#include <sbi/sbi_trap.h>
+
+static int sbi_ecall_time_handler(unsigned long extid, unsigned long funcid,
+				  const struct sbi_trap_regs *regs,
+				  unsigned long *out_val,
+				  struct sbi_trap_info *out_trap)
+{
+	int ret = 0;
+
+	if (funcid == SBI_EXT_TIME_SET_TIMER) {
+#if __riscv_xlen == 32
+		sbi_timer_event_start((((u64)regs->a1 << 32) | (u64)regs->a0));
+#else
+		sbi_timer_event_start((u64)regs->a0);
+#endif
+	} else
+		ret = SBI_ENOTSUPP;
+
+	return ret;
+}
+
+struct sbi_ecall_extension ecall_time = {
+	.extid_start = SBI_EXT_TIME,
+	.extid_end = SBI_EXT_TIME,
+	.handle = sbi_ecall_time_handler,
+};
+
+static int sbi_ecall_rfence_handler(unsigned long extid, unsigned long funcid,
+				    const struct sbi_trap_regs *regs,
+				    unsigned long *out_val,
+				    struct sbi_trap_info *out_trap)
+{
+	int ret = 0;
+	unsigned long vmid;
+	struct sbi_tlb_info tlb_info;
+	u32 source_hart = current_hartid();
+
+	if (funcid >= SBI_EXT_RFENCE_REMOTE_HFENCE_GVMA &&
+	    funcid <= SBI_EXT_RFENCE_REMOTE_HFENCE_VVMA_ASID)
+		if (!misa_extension('H'))
+			return SBI_ENOTSUPP;
+
+	switch (funcid) {
+	case SBI_EXT_RFENCE_REMOTE_FENCE_I:
+		SBI_TLB_INFO_INIT(&tlb_info, 0, 0, 0, 0,
+				  sbi_tlb_local_fence_i, source_hart);
+		ret = sbi_tlb_request(regs->a0, regs->a1, &tlb_info);
+		break;
+	case SBI_EXT_RFENCE_REMOTE_HFENCE_GVMA:
+		SBI_TLB_INFO_INIT(&tlb_info, regs->a2, regs->a3, 0, 0,
+				  sbi_tlb_local_hfence_gvma, source_hart);
+		ret = sbi_tlb_request(regs->a0, regs->a1, &tlb_info);
+		break;
+	case SBI_EXT_RFENCE_REMOTE_HFENCE_GVMA_VMID:
+		SBI_TLB_INFO_INIT(&tlb_info, regs->a2, regs->a3, 0, regs->a4,
+				  sbi_tlb_local_hfence_gvma_vmid,
+				  source_hart);
+		ret = sbi_tlb_request(regs->a0, regs->a1, &tlb_info);
+		break;
+	case SBI_EXT_RFENCE_REMOTE_HFENCE_VVMA:
+		vmid = (csr_read(CSR_HGATP) & HGATP_VMID_MASK);
+		vmid = vmid >> HGATP_VMID_SHIFT;
+		SBI_TLB_INFO_INIT(&tlb_info, regs->a2, regs->a3, 0, vmid,
+				  sbi_tlb_local_hfence_vvma, source_hart);
+		ret = sbi_tlb_request(regs->a0, regs->a1, &tlb_info);
+		break;
+	case SBI_EXT_RFENCE_REMOTE_HFENCE_VVMA_ASID:
+		vmid = (csr_read(CSR_HGATP) & HGATP_VMID_MASK);
+		vmid = vmid >> HGATP_VMID_SHIFT;
+		SBI_TLB_INFO_INIT(&tlb_info, regs->a2, regs->a3, regs->a4,
+				  vmid, sbi_tlb_local_hfence_vvma_asid,
+				  source_hart);
+		ret = sbi_tlb_request(regs->a0, regs->a1, &tlb_info);
+		break;
+	case SBI_EXT_RFENCE_REMOTE_SFENCE_VMA:
+		SBI_TLB_INFO_INIT(&tlb_info, regs->a2, regs->a3, 0, 0,
+				  sbi_tlb_local_sfence_vma, source_hart);
+		ret = sbi_tlb_request(regs->a0, regs->a1, &tlb_info);
+		break;
+	case SBI_EXT_RFENCE_REMOTE_SFENCE_VMA_ASID:
+		SBI_TLB_INFO_INIT(&tlb_info, regs->a2, regs->a3, regs->a4, 0,
+				  sbi_tlb_local_sfence_vma_asid, source_hart);
+		ret = sbi_tlb_request(regs->a0, regs->a1, &tlb_info);
+		break;
+	default:
+		ret = SBI_ENOTSUPP;
+	};
+
+	return ret;
+}
+
+struct sbi_ecall_extension ecall_rfence = {
+	.extid_start = SBI_EXT_RFENCE,
+	.extid_end = SBI_EXT_RFENCE,
+	.handle = sbi_ecall_rfence_handler,
+};
+
+static int sbi_ecall_ipi_handler(unsigned long extid, unsigned long funcid,
+				 const struct sbi_trap_regs *regs,
+				 unsigned long *out_val,
+				 struct sbi_trap_info *out_trap)
+{
+	int ret = 0;
+
+	if (funcid == SBI_EXT_IPI_SEND_IPI)
+		ret = sbi_ipi_send_smode(regs->a0, regs->a1);
+	else
+		ret = SBI_ENOTSUPP;
+
+	return ret;
+}
+
+struct sbi_ecall_extension ecall_ipi = {
+	.extid_start = SBI_EXT_IPI,
+	.extid_end = SBI_EXT_IPI,
+	.handle = sbi_ecall_ipi_handler,
+};
+
+static int sbi_ecall_srst_handler(unsigned long extid, unsigned long funcid,
+				  const struct sbi_trap_regs *regs,
+				  unsigned long *out_val,
+				  struct sbi_trap_info *out_trap)
+{
+	if (funcid == SBI_EXT_SRST_RESET) {
+		if ((((u32)-1U) <= ((u64)regs->a0)) ||
+		    (((u32)-1U) <= ((u64)regs->a1)))
+			return SBI_EINVAL;
+
+		switch (regs->a0) {
+		case SBI_SRST_RESET_TYPE_SHUTDOWN:
+		case SBI_SRST_RESET_TYPE_COLD_REBOOT:
+		case SBI_SRST_RESET_TYPE_WARM_REBOOT:
+			break;
+		default:
+			return SBI_ENOTSUPP;
+		}
+
+		switch (regs->a1) {
+		case SBI_SRST_RESET_REASON_NONE:
+		case SBI_SRST_RESET_REASON_SYSFAIL:
+			break;
+		default:
+			return SBI_ENOTSUPP;
+		}
+
+		if (sbi_system_reset_supported(regs->a0, regs->a1))
+			sbi_system_reset(regs->a0, regs->a1);
+	}
+
+	return SBI_ENOTSUPP;
+}
+
+static int sbi_ecall_srst_probe(unsigned long extid, unsigned long *out_val)
+{
+	u32 type, count = 0;
+
+	/*
+	 * At least one standard reset types should be supported by
+	 * the platform for SBI SRST extension to be usable.
+	 */
+
+	for (type = 0; type <= SBI_SRST_RESET_TYPE_LAST; type++) {
+		if (sbi_system_reset_supported(type,
+					SBI_SRST_RESET_REASON_NONE))
+			count++;
+	}
+
+	*out_val = (count) ? 1 : 0;
+	return 0;
+}
+
+struct sbi_ecall_extension ecall_srst = {
+	.extid_start = SBI_EXT_SRST,
+	.extid_end = SBI_EXT_SRST,
+	.handle = sbi_ecall_srst_handler,
+	.probe = sbi_ecall_srst_probe,
+};

lib/sbi/sbi_ecall_vendor.c

@@ -0,0 +1,40 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ *   Atish Patra <atish.patra@wdc.com>
+ */
+
+#include <sbi/sbi_ecall.h>
+#include <sbi/sbi_ecall_interface.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_platform.h>
+#include <sbi/sbi_trap.h>
+
+static int sbi_ecall_vendor_probe(unsigned long extid,
+				  unsigned long *out_val)
+{
+	*out_val = sbi_platform_vendor_ext_check(sbi_platform_thishart_ptr(),
+						 extid);
+	return 0;
+}
+
+static int sbi_ecall_vendor_handler(unsigned long extid, unsigned long funcid,
+				    const struct sbi_trap_regs *regs,
+				    unsigned long *out_val,
+				    struct sbi_trap_info *out_trap)
+{
+	return sbi_platform_vendor_ext_provider(sbi_platform_thishart_ptr(),
+						extid, funcid, regs,
+						out_val, out_trap);
+}
+
+struct sbi_ecall_extension ecall_vendor = {
+	.extid_start = SBI_EXT_VENDOR_START,
+	.extid_end = SBI_EXT_VENDOR_END,
+	.probe = sbi_ecall_vendor_probe,
+	.handle = sbi_ecall_vendor_handler,
+};

lib/sbi/sbi_emulate_csr.c

@@ -0,0 +1,197 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#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>
+#include <sbi/sbi_scratch.h>
+#include <sbi/sbi_timer.h>
+#include <sbi/sbi_trap.h>
+
+static bool hpm_allowed(int hpm_num, ulong prev_mode, bool virt)
+{
+	ulong cen = -1UL;
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+
+	if (prev_mode <= PRV_S) {
+		if (sbi_hart_has_feature(scratch, SBI_HART_HAS_MCOUNTEREN)) {
+			cen &= csr_read(CSR_MCOUNTEREN);
+			if (virt)
+				cen &= csr_read(CSR_HCOUNTEREN);
+		} else {
+			cen = 0;
+		}
+	}
+	if (prev_mode == PRV_U) {
+		if (sbi_hart_has_feature(scratch, SBI_HART_HAS_SCOUNTEREN))
+			cen &= csr_read(CSR_SCOUNTEREN);
+		else
+			cen = 0;
+	}
+
+	return ((cen >> hpm_num) & 1) ? TRUE : FALSE;
+}
+
+int sbi_emulate_csr_read(int csr_num, struct sbi_trap_regs *regs,
+			 ulong *csr_val)
+{
+	int ret = 0;
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+	ulong prev_mode = (regs->mstatus & MSTATUS_MPP) >> MSTATUS_MPP_SHIFT;
+#if __riscv_xlen == 32
+	bool virt = (regs->mstatusH & MSTATUSH_MPV) ? TRUE : FALSE;
+#else
+	bool virt = (regs->mstatus & MSTATUS_MPV) ? TRUE : FALSE;
+#endif
+
+	switch (csr_num) {
+	case CSR_HTIMEDELTA:
+		if (prev_mode == PRV_S && !virt)
+			*csr_val = sbi_timer_get_delta();
+		else
+			ret = SBI_ENOTSUPP;
+		break;
+	case CSR_CYCLE:
+		if (!hpm_allowed(csr_num - CSR_CYCLE, prev_mode, virt))
+			return SBI_ENOTSUPP;
+		*csr_val = csr_read(CSR_MCYCLE);
+		break;
+	case CSR_TIME:
+		/*
+		 * We emulate TIME CSR for both Host (HS/U-mode) and
+		 * Guest (VS/VU-mode).
+		 *
+		 * Faster TIME CSR reads are critical for good performance
+		 * in S-mode software so we don't check CSR permissions.
+		 */
+		*csr_val = (virt) ? sbi_timer_virt_value():
+				    sbi_timer_value();
+		break;
+	case CSR_INSTRET:
+		if (!hpm_allowed(csr_num - CSR_CYCLE, prev_mode, virt))
+			return SBI_ENOTSUPP;
+		*csr_val = csr_read(CSR_MINSTRET);
+		break;
+
+#if __riscv_xlen == 32
+	case CSR_HTIMEDELTAH:
+		if (prev_mode == PRV_S && !virt)
+			*csr_val = sbi_timer_get_delta() >> 32;
+		else
+			ret = SBI_ENOTSUPP;
+		break;
+	case CSR_CYCLEH:
+		if (!hpm_allowed(csr_num - CSR_CYCLEH, prev_mode, virt))
+			return SBI_ENOTSUPP;
+		*csr_val = csr_read(CSR_MCYCLEH);
+		break;
+	case CSR_TIMEH:
+		/* Refer comments on TIME CSR above. */
+		*csr_val = (virt) ? sbi_timer_virt_value() >> 32:
+				    sbi_timer_value() >> 32;
+		break;
+	case CSR_INSTRETH:
+		if (!hpm_allowed(csr_num - CSR_CYCLEH, prev_mode, virt))
+			return SBI_ENOTSUPP;
+		*csr_val = csr_read(CSR_MINSTRETH);
+		break;
+#endif
+
+#define switchcase_hpm(__uref, __mref, __csr)				\
+	case __csr:							\
+		if ((sbi_hart_mhpm_count(scratch) + 3) <= (__csr - __uref))\
+			return SBI_ENOTSUPP;				\
+		if (!hpm_allowed(__csr - __uref, prev_mode, virt))	\
+			return SBI_ENOTSUPP;				\
+		*csr_val = csr_read(__mref + __csr - __uref);		\
+		break;
+#define switchcase_hpm_2(__uref, __mref, __csr)			\
+	switchcase_hpm(__uref, __mref, __csr + 0)			\
+	switchcase_hpm(__uref, __mref, __csr + 1)
+#define switchcase_hpm_4(__uref, __mref, __csr)			\
+	switchcase_hpm_2(__uref, __mref, __csr + 0)			\
+	switchcase_hpm_2(__uref, __mref, __csr + 2)
+#define switchcase_hpm_8(__uref, __mref, __csr)			\
+	switchcase_hpm_4(__uref, __mref, __csr + 0)			\
+	switchcase_hpm_4(__uref, __mref, __csr + 4)
+#define switchcase_hpm_16(__uref, __mref, __csr)			\
+	switchcase_hpm_8(__uref, __mref, __csr + 0)			\
+	switchcase_hpm_8(__uref, __mref, __csr + 8)
+
+	switchcase_hpm(CSR_CYCLE, CSR_MCYCLE, CSR_HPMCOUNTER3)
+	switchcase_hpm_4(CSR_CYCLE, CSR_MCYCLE, CSR_HPMCOUNTER4)
+	switchcase_hpm_8(CSR_CYCLE, CSR_MCYCLE, CSR_HPMCOUNTER8)
+	switchcase_hpm_16(CSR_CYCLE, CSR_MCYCLE, CSR_HPMCOUNTER16)
+
+#if __riscv_xlen == 32
+	switchcase_hpm(CSR_CYCLEH, CSR_MCYCLEH, CSR_HPMCOUNTER3H)
+	switchcase_hpm_4(CSR_CYCLEH, CSR_MCYCLEH, CSR_HPMCOUNTER4H)
+	switchcase_hpm_8(CSR_CYCLEH, CSR_MCYCLEH, CSR_HPMCOUNTER8H)
+	switchcase_hpm_16(CSR_CYCLEH, CSR_MCYCLEH, CSR_HPMCOUNTER16H)
+#endif
+
+#undef switchcase_hpm_16
+#undef switchcase_hpm_8
+#undef switchcase_hpm_4
+#undef switchcase_hpm_2
+#undef switchcase_hpm
+
+	default:
+		ret = SBI_ENOTSUPP;
+		break;
+	};
+
+	if (ret)
+		sbi_dprintf("%s: hartid%d: invalid csr_num=0x%x\n",
+			    __func__, current_hartid(), csr_num);
+
+	return ret;
+}
+
+int sbi_emulate_csr_write(int csr_num, struct sbi_trap_regs *regs,
+			  ulong csr_val)
+{
+	int ret = 0;
+	ulong prev_mode = (regs->mstatus & MSTATUS_MPP) >> MSTATUS_MPP_SHIFT;
+#if __riscv_xlen == 32
+	bool virt = (regs->mstatusH & MSTATUSH_MPV) ? TRUE : FALSE;
+#else
+	bool virt = (regs->mstatus & MSTATUS_MPV) ? TRUE : FALSE;
+#endif
+
+	switch (csr_num) {
+	case CSR_HTIMEDELTA:
+		if (prev_mode == PRV_S && !virt)
+			sbi_timer_set_delta(csr_val);
+		else
+			ret = SBI_ENOTSUPP;
+		break;
+#if __riscv_xlen == 32
+	case CSR_HTIMEDELTAH:
+		if (prev_mode == PRV_S && !virt)
+			sbi_timer_set_delta_upper(csr_val);
+		else
+			ret = SBI_ENOTSUPP;
+		break;
+#endif
+	default:
+		ret = SBI_ENOTSUPP;
+		break;
+	};
+
+	if (ret)
+		sbi_dprintf("%s: hartid%d: invalid csr_num=0x%x\n",
+			    __func__, current_hartid(), csr_num);
+
+	return ret;
+}

lib/sbi/sbi_expected_trap.S

@@ -0,0 +1,56 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/sbi_trap.h>
+
+	/*
+	 * We assume that faulting instruction is is 4-byte long and blindly
+	 * increment SEPC by 4.
+	 *
+	 * The trap info will be saved as follows:
+	 * A3 <- pointer struct sbi_trap_info
+	 * A4 <- temporary
+	 */
+
+	.align 3
+	.global __sbi_expected_trap
+__sbi_expected_trap:
+	/* Without H-extension so, MTVAL2 and MTINST CSRs 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
+	REG_S	a4, SBI_TRAP_INFO_OFFSET(tval)(a3)
+	REG_S	zero, SBI_TRAP_INFO_OFFSET(tval2)(a3)
+	REG_S	zero, SBI_TRAP_INFO_OFFSET(tinst)(a3)
+	csrr	a4, CSR_MEPC
+	addi	a4, a4, 4
+	csrw	CSR_MEPC, a4
+	mret
+
+	.align 3
+	.global __sbi_expected_trap_hext
+__sbi_expected_trap_hext:
+	/* With H-extension so, MTVAL2 and MTINST CSRs 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
+	REG_S	a4, SBI_TRAP_INFO_OFFSET(tval)(a3)
+	csrr	a4, CSR_MTVAL2
+	REG_S	a4, SBI_TRAP_INFO_OFFSET(tval2)(a3)
+	csrr	a4, CSR_MTINST
+	REG_S	a4, SBI_TRAP_INFO_OFFSET(tinst)(a3)
+	csrr	a4, CSR_MEPC
+	addi	a4, a4, 4
+	csrw	CSR_MEPC, a4
+	mret

lib/sbi/sbi_fifo.c

@@ -0,0 +1,192 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Atish Patra<atish.patra@wdc.com>
+ *
+ */
+#include <sbi/riscv_locks.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_fifo.h>
+#include <sbi/sbi_string.h>
+
+void sbi_fifo_init(struct sbi_fifo *fifo, void *queue_mem, u16 entries,
+		   u16 entry_size)
+{
+	fifo->queue	  = queue_mem;
+	fifo->num_entries = entries;
+	fifo->entry_size  = entry_size;
+	SPIN_LOCK_INIT(fifo->qlock);
+	fifo->avail = fifo->tail = 0;
+	sbi_memset(fifo->queue, 0, (size_t)entries * entry_size);
+}
+
+/* Note: must be called with fifo->qlock held */
+static inline bool __sbi_fifo_is_full(struct sbi_fifo *fifo)
+{
+	return (fifo->avail == fifo->num_entries) ? TRUE : FALSE;
+}
+
+u16 sbi_fifo_avail(struct sbi_fifo *fifo)
+{
+	u16 ret;
+
+	if (!fifo)
+		return 0;
+
+	spin_lock(&fifo->qlock);
+	ret = fifo->avail;
+	spin_unlock(&fifo->qlock);
+
+	return ret;
+}
+
+bool sbi_fifo_is_full(struct sbi_fifo *fifo)
+{
+	bool ret;
+
+	spin_lock(&fifo->qlock);
+	ret = __sbi_fifo_is_full(fifo);
+	spin_unlock(&fifo->qlock);
+
+	return ret;
+}
+
+/* Note: must be called with fifo->qlock held */
+static inline void  __sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data)
+{
+	u32 head;
+
+	head = (u32)fifo->tail + fifo->avail;
+	if (head >= fifo->num_entries)
+		head = head - fifo->num_entries;
+
+	sbi_memcpy(fifo->queue + head * fifo->entry_size, data, fifo->entry_size);
+
+	fifo->avail++;
+}
+
+
+/* Note: must be called with fifo->qlock held */
+static inline bool __sbi_fifo_is_empty(struct sbi_fifo *fifo)
+{
+	return (fifo->avail == 0) ? TRUE : FALSE;
+}
+
+bool sbi_fifo_is_empty(struct sbi_fifo *fifo)
+{
+	bool ret;
+
+	spin_lock(&fifo->qlock);
+	ret = __sbi_fifo_is_empty(fifo);
+	spin_unlock(&fifo->qlock);
+
+	return ret;
+}
+
+/* Note: must be called with fifo->qlock held */
+static inline void __sbi_fifo_reset(struct sbi_fifo *fifo)
+{
+	size_t size = (size_t)fifo->num_entries * fifo->entry_size;
+
+	fifo->avail = 0;
+	fifo->tail  = 0;
+	sbi_memset(fifo->queue, 0, size);
+}
+
+bool sbi_fifo_reset(struct sbi_fifo *fifo)
+{
+	if (!fifo)
+		return FALSE;
+
+	spin_lock(&fifo->qlock);
+	__sbi_fifo_reset(fifo);
+	spin_unlock(&fifo->qlock);
+
+	return TRUE;
+}
+
+/**
+ * Provide a helper function to do inplace update to the fifo.
+ * Note: The callback function is called with lock being held.
+ *
+ * **Do not** invoke any other fifo function from callback. Otherwise, it will
+ * lead to deadlock.
+ */
+int sbi_fifo_inplace_update(struct sbi_fifo *fifo, void *in,
+			    int (*fptr)(void *in, void *data))
+{
+	int i, index = 0;
+	int ret = SBI_FIFO_UNCHANGED;
+	void *entry;
+
+	if (!fifo || !in)
+		return ret;
+
+	spin_lock(&fifo->qlock);
+
+	if (__sbi_fifo_is_empty(fifo)) {
+		spin_unlock(&fifo->qlock);
+		return ret;
+	}
+
+	for (i = 0; i < fifo->avail; i++) {
+		index = fifo->tail + i;
+		if (index >= fifo->num_entries)
+			index = index - fifo->num_entries;
+		entry = (void *)fifo->queue + (u32)index * fifo->entry_size;
+		ret = fptr(in, entry);
+
+		if (ret == SBI_FIFO_SKIP || ret == SBI_FIFO_UPDATED) {
+			break;
+		}
+	}
+	spin_unlock(&fifo->qlock);
+
+	return ret;
+}
+
+int sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data)
+{
+	if (!fifo || !data)
+		return SBI_EINVAL;
+
+	spin_lock(&fifo->qlock);
+
+	if (__sbi_fifo_is_full(fifo)) {
+		spin_unlock(&fifo->qlock);
+		return SBI_ENOSPC;
+	}
+	__sbi_fifo_enqueue(fifo, data);
+
+	spin_unlock(&fifo->qlock);
+
+	return 0;
+}
+
+int sbi_fifo_dequeue(struct sbi_fifo *fifo, void *data)
+{
+	if (!fifo || !data)
+		return SBI_EINVAL;
+
+	spin_lock(&fifo->qlock);
+
+	if (__sbi_fifo_is_empty(fifo)) {
+		spin_unlock(&fifo->qlock);
+		return SBI_ENOENT;
+	}
+
+	sbi_memcpy(data, fifo->queue + (u32)fifo->tail * fifo->entry_size,
+	       fifo->entry_size);
+
+	fifo->avail--;
+	fifo->tail++;
+	if (fifo->tail >= fifo->num_entries)
+		fifo->tail = 0;
+
+	spin_unlock(&fifo->qlock);
+
+	return 0;
+}

lib/sbi/sbi_hart.c

@@ -0,0 +1,538 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_barrier.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/riscv_fp.h>
+#include <sbi/sbi_bitops.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_domain.h>
+#include <sbi/sbi_csr_detect.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_math.h>
+#include <sbi/sbi_platform.h>
+#include <sbi/sbi_string.h>
+#include <sbi/sbi_trap.h>
+
+extern void __sbi_expected_trap(void);
+extern void __sbi_expected_trap_hext(void);
+
+void (*sbi_hart_expected_trap)(void) = &__sbi_expected_trap;
+
+struct hart_features {
+	unsigned long features;
+	unsigned int pmp_count;
+	unsigned int pmp_addr_bits;
+	unsigned long pmp_gran;
+	unsigned int mhpm_count;
+};
+static unsigned long hart_features_offset;
+
+static void mstatus_init(struct sbi_scratch *scratch)
+{
+	unsigned long mstatus_val = 0;
+
+	/* Enable FPU */
+	if (misa_extension('D') || misa_extension('F'))
+		mstatus_val |=  MSTATUS_FS;
+
+	/* Enable Vector context */
+	if (misa_extension('V'))
+		mstatus_val |=  MSTATUS_VS;
+
+	csr_write(CSR_MSTATUS, mstatus_val);
+
+	/* Enable user/supervisor use of perf counters */
+	if (misa_extension('S') &&
+	    sbi_hart_has_feature(scratch, SBI_HART_HAS_SCOUNTEREN))
+		csr_write(CSR_SCOUNTEREN, -1);
+	if (sbi_hart_has_feature(scratch, SBI_HART_HAS_MCOUNTEREN))
+		csr_write(CSR_MCOUNTEREN, -1);
+
+	/* Disable all interrupts */
+	csr_write(CSR_MIE, 0);
+
+	/* Disable S-mode paging */
+	if (misa_extension('S'))
+		csr_write(CSR_SATP, 0);
+}
+
+static int fp_init(struct sbi_scratch *scratch)
+{
+#ifdef __riscv_flen
+	int i;
+#endif
+
+	if (!misa_extension('D') && !misa_extension('F'))
+		return 0;
+
+	if (!(csr_read(CSR_MSTATUS) & MSTATUS_FS))
+		return SBI_EINVAL;
+
+#ifdef __riscv_flen
+	for (i = 0; i < 32; i++)
+		init_fp_reg(i);
+	csr_write(CSR_FCSR, 0);
+#endif
+
+	return 0;
+}
+
+static int delegate_traps(struct sbi_scratch *scratch)
+{
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+	unsigned long interrupts, exceptions;
+
+	if (!misa_extension('S'))
+		/* No delegation possible as mideleg does not exist */
+		return 0;
+
+	/* Send M-mode interrupts and most exceptions to S-mode */
+	interrupts = MIP_SSIP | MIP_STIP | MIP_SEIP;
+	exceptions = (1U << CAUSE_MISALIGNED_FETCH) | (1U << CAUSE_BREAKPOINT) |
+		     (1U << CAUSE_USER_ECALL);
+	if (sbi_platform_has_mfaults_delegation(plat))
+		exceptions |= (1U << CAUSE_FETCH_PAGE_FAULT) |
+			      (1U << CAUSE_LOAD_PAGE_FAULT) |
+			      (1U << CAUSE_STORE_PAGE_FAULT);
+
+	/*
+	 * If hypervisor extension available then we only handle hypervisor
+	 * calls (i.e. ecalls from HS-mode) in M-mode.
+	 *
+	 * The HS-mode will additionally handle supervisor calls (i.e. ecalls
+	 * from VS-mode), Guest page faults and Virtual interrupts.
+	 */
+	if (misa_extension('H')) {
+		exceptions |= (1U << CAUSE_VIRTUAL_SUPERVISOR_ECALL);
+		exceptions |= (1U << CAUSE_FETCH_GUEST_PAGE_FAULT);
+		exceptions |= (1U << CAUSE_LOAD_GUEST_PAGE_FAULT);
+		exceptions |= (1U << CAUSE_VIRTUAL_INST_FAULT);
+		exceptions |= (1U << CAUSE_STORE_GUEST_PAGE_FAULT);
+	}
+
+	csr_write(CSR_MIDELEG, interrupts);
+	csr_write(CSR_MEDELEG, exceptions);
+
+	return 0;
+}
+
+void sbi_hart_delegation_dump(struct sbi_scratch *scratch,
+			      const char *prefix, const char *suffix)
+{
+	if (!misa_extension('S'))
+		/* No delegation possible as mideleg does not exist*/
+		return;
+
+#if __riscv_xlen == 32
+	sbi_printf("%sMIDELEG%s: 0x%08lx\n",
+		   prefix, suffix, csr_read(CSR_MIDELEG));
+	sbi_printf("%sMEDELEG%s: 0x%08lx\n",
+		   prefix, suffix, csr_read(CSR_MEDELEG));
+#else
+	sbi_printf("%sMIDELEG%s: 0x%016lx\n",
+		   prefix, suffix, csr_read(CSR_MIDELEG));
+	sbi_printf("%sMEDELEG%s: 0x%016lx\n",
+		   prefix, suffix, csr_read(CSR_MEDELEG));
+#endif
+}
+
+unsigned int sbi_hart_mhpm_count(struct sbi_scratch *scratch)
+{
+	struct hart_features *hfeatures =
+			sbi_scratch_offset_ptr(scratch, hart_features_offset);
+
+	return hfeatures->mhpm_count;
+}
+
+unsigned int sbi_hart_pmp_count(struct sbi_scratch *scratch)
+{
+	struct hart_features *hfeatures =
+			sbi_scratch_offset_ptr(scratch, hart_features_offset);
+
+	return hfeatures->pmp_count;
+}
+
+unsigned long sbi_hart_pmp_granularity(struct sbi_scratch *scratch)
+{
+	struct hart_features *hfeatures =
+			sbi_scratch_offset_ptr(scratch, hart_features_offset);
+
+	return hfeatures->pmp_gran;
+}
+
+unsigned int sbi_hart_pmp_addrbits(struct sbi_scratch *scratch)
+{
+	struct hart_features *hfeatures =
+			sbi_scratch_offset_ptr(scratch, hart_features_offset);
+
+	return hfeatures->pmp_addr_bits;
+}
+
+int sbi_hart_pmp_configure(struct sbi_scratch *scratch)
+{
+	struct sbi_domain_memregion *reg;
+	struct sbi_domain *dom = sbi_domain_thishart_ptr();
+	unsigned int pmp_idx = 0, pmp_flags, pmp_bits, pmp_gran_log2;
+	unsigned int pmp_count = sbi_hart_pmp_count(scratch);
+	unsigned long pmp_addr = 0, pmp_addr_max = 0;
+
+	if (!pmp_count)
+		return 0;
+
+	pmp_gran_log2 = log2roundup(sbi_hart_pmp_granularity(scratch));
+	pmp_bits = sbi_hart_pmp_addrbits(scratch) - 1;
+	pmp_addr_max = (1UL << pmp_bits) | ((1UL << pmp_bits) - 1);
+
+	sbi_domain_for_each_memregion(dom, reg) {
+		if (pmp_count <= pmp_idx)
+			break;
+
+		pmp_flags = 0;
+		if (reg->flags & SBI_DOMAIN_MEMREGION_READABLE)
+			pmp_flags |= PMP_R;
+		if (reg->flags & SBI_DOMAIN_MEMREGION_WRITEABLE)
+			pmp_flags |= PMP_W;
+		if (reg->flags & SBI_DOMAIN_MEMREGION_EXECUTABLE)
+			pmp_flags |= PMP_X;
+		if (reg->flags & SBI_DOMAIN_MEMREGION_MMODE)
+			pmp_flags |= PMP_L;
+
+		pmp_addr =  reg->base >> PMP_SHIFT;
+		if (pmp_gran_log2 <= reg->order && pmp_addr < pmp_addr_max)
+			pmp_set(pmp_idx++, pmp_flags, reg->base, reg->order);
+		else {
+			sbi_printf("Can not configure pmp for domain %s", dom->name);
+			sbi_printf("because memory region address %lx or size %lx is not in range\n",
+				    reg->base, reg->order);
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * Check whether a particular hart feature is available
+ *
+ * @param scratch pointer to the HART scratch space
+ * @param feature the feature to check
+ * @returns true (feature available) or false (feature not available)
+ */
+bool sbi_hart_has_feature(struct sbi_scratch *scratch, unsigned long feature)
+{
+	struct hart_features *hfeatures =
+			sbi_scratch_offset_ptr(scratch, hart_features_offset);
+
+	if (hfeatures->features & feature)
+		return true;
+	else
+		return false;
+}
+
+static unsigned long hart_get_features(struct sbi_scratch *scratch)
+{
+	struct hart_features *hfeatures =
+			sbi_scratch_offset_ptr(scratch, hart_features_offset);
+
+	return hfeatures->features;
+}
+
+static inline char *sbi_hart_feature_id2string(unsigned long feature)
+{
+	char *fstr = NULL;
+
+	if (!feature)
+		return NULL;
+
+	switch (feature) {
+	case SBI_HART_HAS_SCOUNTEREN:
+		fstr = "scounteren";
+		break;
+	case SBI_HART_HAS_MCOUNTEREN:
+		fstr = "mcounteren";
+		break;
+	case SBI_HART_HAS_TIME:
+		fstr = "time";
+		break;
+	default:
+		break;
+	}
+
+	return fstr;
+}
+
+/**
+ * Get the hart features in string format
+ *
+ * @param scratch pointer to the HART scratch space
+ * @param features_str pointer to a char array where the features string will be
+ *		       updated
+ * @param nfstr length of the features_str. The feature string will be truncated
+ *		if nfstr is not long enough.
+ */
+void sbi_hart_get_features_str(struct sbi_scratch *scratch,
+			       char *features_str, int nfstr)
+{
+	unsigned long features, feat = 1UL;
+	char *temp;
+	int offset = 0;
+
+	if (!features_str || nfstr <= 0)
+		return;
+	sbi_memset(features_str, 0, nfstr);
+
+	features = hart_get_features(scratch);
+	if (!features)
+		goto done;
+
+	do {
+		if (features & feat) {
+			temp = sbi_hart_feature_id2string(feat);
+			if (temp) {
+				sbi_snprintf(features_str + offset, nfstr,
+					     "%s,", temp);
+				offset = offset + sbi_strlen(temp) + 1;
+			}
+		}
+		feat = feat << 1;
+	} while (feat <= SBI_HART_HAS_LAST_FEATURE);
+
+done:
+	if (offset)
+		features_str[offset - 1] = '\0';
+	else
+		sbi_strncpy(features_str, "none", nfstr);
+}
+
+static unsigned long hart_pmp_get_allowed_addr(void)
+{
+	unsigned long val = 0;
+	struct sbi_trap_info trap = {0};
+
+	csr_write_allowed(CSR_PMPADDR0, (ulong)&trap, PMP_ADDR_MASK);			\
+	if (!trap.cause) {
+		val = csr_read_allowed(CSR_PMPADDR0, (ulong)&trap);
+		if (trap.cause)
+			val = 0;
+	}
+
+	return val;
+}
+
+static void hart_detect_features(struct sbi_scratch *scratch)
+{
+	struct sbi_trap_info trap = {0};
+	struct hart_features *hfeatures;
+	unsigned long val;
+
+	/* Reset hart features */
+	hfeatures = sbi_scratch_offset_ptr(scratch, hart_features_offset);
+	hfeatures->features = 0;
+	hfeatures->pmp_count = 0;
+	hfeatures->mhpm_count = 0;
+
+#define __check_csr(__csr, __rdonly, __wrval, __field, __skip)	\
+	val = csr_read_allowed(__csr, (ulong)&trap);			\
+	if (!trap.cause) {						\
+		if (__rdonly) {						\
+			(hfeatures->__field)++;				\
+		} else {						\
+			csr_write_allowed(__csr, (ulong)&trap, __wrval);\
+			if (!trap.cause) {				\
+				if (csr_swap(__csr, val) == __wrval)	\
+					(hfeatures->__field)++;		\
+				else					\
+					goto __skip;			\
+			} else {					\
+				goto __skip;				\
+			}						\
+		}							\
+	} else {							\
+		goto __skip;						\
+	}
+#define __check_csr_2(__csr, __rdonly, __wrval, __field, __skip)	\
+	__check_csr(__csr + 0, __rdonly, __wrval, __field, __skip)	\
+	__check_csr(__csr + 1, __rdonly, __wrval, __field, __skip)
+#define __check_csr_4(__csr, __rdonly, __wrval, __field, __skip)	\
+	__check_csr_2(__csr + 0, __rdonly, __wrval, __field, __skip)	\
+	__check_csr_2(__csr + 2, __rdonly, __wrval, __field, __skip)
+#define __check_csr_8(__csr, __rdonly, __wrval, __field, __skip)	\
+	__check_csr_4(__csr + 0, __rdonly, __wrval, __field, __skip)	\
+	__check_csr_4(__csr + 4, __rdonly, __wrval, __field, __skip)
+#define __check_csr_16(__csr, __rdonly, __wrval, __field, __skip)	\
+	__check_csr_8(__csr + 0, __rdonly, __wrval, __field, __skip)	\
+	__check_csr_8(__csr + 8, __rdonly, __wrval, __field, __skip)
+#define __check_csr_32(__csr, __rdonly, __wrval, __field, __skip)	\
+	__check_csr_16(__csr + 0, __rdonly, __wrval, __field, __skip)	\
+	__check_csr_16(__csr + 16, __rdonly, __wrval, __field, __skip)
+#define __check_csr_64(__csr, __rdonly, __wrval, __field, __skip)	\
+	__check_csr_32(__csr + 0, __rdonly, __wrval, __field, __skip)	\
+	__check_csr_32(__csr + 32, __rdonly, __wrval, __field, __skip)
+
+	/**
+	 * Detect the allowed address bits & granularity. At least PMPADDR0
+	 * should be implemented.
+	 */
+	val = hart_pmp_get_allowed_addr();
+	if (val) {
+		hfeatures->pmp_gran =  1 << (__ffs(val) + 2);
+		hfeatures->pmp_addr_bits = __fls(val) + 1;
+		/* Detect number of PMP regions. At least PMPADDR0 should be implemented*/
+		__check_csr_64(CSR_PMPADDR0, 0, val, pmp_count, __pmp_skip);
+	}
+__pmp_skip:
+
+	/* Detect number of MHPM counters */
+	__check_csr(CSR_MHPMCOUNTER3, 0, 1UL, mhpm_count, __mhpm_skip);
+	__check_csr_4(CSR_MHPMCOUNTER4, 0, 1UL, mhpm_count, __mhpm_skip);
+	__check_csr_8(CSR_MHPMCOUNTER8, 0, 1UL, mhpm_count, __mhpm_skip);
+	__check_csr_16(CSR_MHPMCOUNTER16, 0, 1UL, mhpm_count, __mhpm_skip);
+__mhpm_skip:
+
+#undef __check_csr_64
+#undef __check_csr_32
+#undef __check_csr_16
+#undef __check_csr_8
+#undef __check_csr_4
+#undef __check_csr_2
+#undef __check_csr
+
+	/* Detect if hart supports SCOUNTEREN feature */
+	val = csr_read_allowed(CSR_SCOUNTEREN, (unsigned long)&trap);
+	if (!trap.cause) {
+		csr_write_allowed(CSR_SCOUNTEREN, (unsigned long)&trap, val);
+		if (!trap.cause)
+			hfeatures->features |= SBI_HART_HAS_SCOUNTEREN;
+	}
+
+	/* Detect if hart supports MCOUNTEREN feature */
+	val = csr_read_allowed(CSR_MCOUNTEREN, (unsigned long)&trap);
+	if (!trap.cause) {
+		csr_write_allowed(CSR_MCOUNTEREN, (unsigned long)&trap, val);
+		if (!trap.cause)
+			hfeatures->features |= SBI_HART_HAS_MCOUNTEREN;
+	}
+
+	/* Detect if hart supports time CSR */
+	csr_read_allowed(CSR_TIME, (unsigned long)&trap);
+	if (!trap.cause)
+		hfeatures->features |= SBI_HART_HAS_TIME;
+}
+
+int sbi_hart_reinit(struct sbi_scratch *scratch)
+{
+	int rc;
+
+	mstatus_init(scratch);
+
+	rc = fp_init(scratch);
+	if (rc)
+		return rc;
+
+	rc = delegate_traps(scratch);
+	if (rc)
+		return rc;
+
+	return 0;
+}
+
+int sbi_hart_init(struct sbi_scratch *scratch, bool cold_boot)
+{
+	if (cold_boot) {
+		if (misa_extension('H'))
+			sbi_hart_expected_trap = &__sbi_expected_trap_hext;
+
+		hart_features_offset = sbi_scratch_alloc_offset(
+						sizeof(struct hart_features),
+						"HART_FEATURES");
+		if (!hart_features_offset)
+			return SBI_ENOMEM;
+	}
+
+	hart_detect_features(scratch);
+
+	return sbi_hart_reinit(scratch);
+}
+
+void __attribute__((noreturn)) sbi_hart_hang(void)
+{
+	while (1)
+		wfi();
+	__builtin_unreachable();
+}
+
+void __attribute__((noreturn))
+sbi_hart_switch_mode(unsigned long arg0, unsigned long arg1,
+		     unsigned long next_addr, unsigned long next_mode,
+		     bool next_virt)
+{
+#if __riscv_xlen == 32
+	unsigned long val, valH;
+#else
+	unsigned long val;
+#endif
+
+	switch (next_mode) {
+	case PRV_M:
+		break;
+	case PRV_S:
+		if (!misa_extension('S'))
+			sbi_hart_hang();
+		break;
+	case PRV_U:
+		if (!misa_extension('U'))
+			sbi_hart_hang();
+		break;
+	default:
+		sbi_hart_hang();
+	}
+
+	val = csr_read(CSR_MSTATUS);
+	val = INSERT_FIELD(val, MSTATUS_MPP, next_mode);
+	val = INSERT_FIELD(val, MSTATUS_MPIE, 0);
+#if __riscv_xlen == 32
+	if (misa_extension('H')) {
+		valH = csr_read(CSR_MSTATUSH);
+		if (next_virt)
+			valH = INSERT_FIELD(valH, MSTATUSH_MPV, 1);
+		else
+			valH = INSERT_FIELD(valH, MSTATUSH_MPV, 0);
+		csr_write(CSR_MSTATUSH, valH);
+	}
+#else
+	if (misa_extension('H')) {
+		if (next_virt)
+			val = INSERT_FIELD(val, MSTATUS_MPV, 1);
+		else
+			val = INSERT_FIELD(val, MSTATUS_MPV, 0);
+	}
+#endif
+	csr_write(CSR_MSTATUS, val);
+	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);
+	} else if (next_mode == PRV_U) {
+		if (misa_extension('N')) {
+			csr_write(CSR_UTVEC, next_addr);
+			csr_write(CSR_USCRATCH, 0);
+			csr_write(CSR_UIE, 0);
+		}
+	}
+
+	register unsigned long a0 asm("a0") = arg0;
+	register unsigned long a1 asm("a1") = arg1;
+	__asm__ __volatile__("mret" : : "r"(a0), "r"(a1));
+	__builtin_unreachable();
+}

lib/sbi/sbi_hfence.S

@@ -0,0 +1,135 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ *   Atish Patra <anup.patel@wdc.com>
+ */
+
+	/*
+	 * HFENCE.GVMA rs1, rs2
+	 * HFENCE.GVMA zero, rs2
+	 * HFENCE.GVMA rs1
+	 * HFENCE.GVMA
+	 *
+	 * rs1!=zero and rs2!=zero ==> HFENCE.GVMA rs1, rs2
+	 * rs1==zero and rs2!=zero ==> HFENCE.GVMA zero, rs2
+	 * rs1!=zero and rs2==zero ==> HFENCE.GVMA rs1
+	 * rs1==zero and rs2==zero ==> HFENCE.GVMA
+	 *
+	 * Instruction encoding of HFENCE.GVMA is:
+	 * 0110001 rs2(5) rs1(5) 000 00000 1110011
+	 */
+
+	.align 3
+	.global __sbi_hfence_gvma_vmid_gpa
+__sbi_hfence_gvma_vmid_gpa:
+	/*
+	 * rs1 = a0 (GPA)
+	 * rs2 = a1 (VMID)
+	 * HFENCE.GVMA a0, a1
+	 * 0110001 01011 01010 000 00000 1110011
+	 */
+	.word 0x62b50073
+	ret
+
+	.align 3
+	.global __sbi_hfence_gvma_vmid
+__sbi_hfence_gvma_vmid:
+	/*
+	 * rs1 = zero
+	 * rs2 = a0 (VMID)
+	 * HFENCE.GVMA zero, a0
+	 * 0110001 01010 00000 000 00000 1110011
+	 */
+	.word 0x62a00073
+	ret
+
+	.align 3
+	.global __sbi_hfence_gvma_gpa
+__sbi_hfence_gvma_gpa:
+	/*
+	 * rs1 = a0 (GPA)
+	 * rs2 = zero
+	 * HFENCE.GVMA a0
+	 * 0110001 00000 01010 000 00000 1110011
+	 */
+	.word 0x62050073
+	ret
+
+	.align 3
+	.global __sbi_hfence_gvma_all
+__sbi_hfence_gvma_all:
+	/*
+	 * rs1 = zero
+	 * rs2 = zero
+	 * HFENCE.GVMA
+	 * 0110001 00000 00000 000 00000 1110011
+	 */
+	.word 0x62000073
+	ret
+
+	/*
+	 * HFENCE.VVMA rs1, rs2
+	 * HFENCE.VVMA zero, rs2
+	 * HFENCE.VVMA rs1
+	 * HFENCE.VVMA
+	 *
+	 * rs1!=zero and rs2!=zero ==> HFENCE.VVMA rs1, rs2
+	 * rs1==zero and rs2!=zero ==> HFENCE.VVMA zero, rs2
+	 * rs1!=zero and rs2==zero ==> HFENCE.VVMA rs1
+	 * rs1==zero and rs2==zero ==> HFENCE.vVMA
+	 *
+	 * Instruction encoding of HFENCE.VVMA is:
+	 * 0010001 rs2(5) rs1(5) 000 00000 1110011
+	 */
+
+	.align 3
+	.global __sbi_hfence_vvma_asid_va
+__sbi_hfence_vvma_asid_va:
+	/*
+	 * rs1 = a0 (VA)
+	 * rs2 = a1 (ASID)
+	 * HFENCE.VVMA a0, a1
+	 * 0010001 01011 01010 000 00000 1110011
+	 */
+	.word 0x22b50073
+	ret
+
+	.align 3
+	.global __sbi_hfence_vvma_asid
+__sbi_hfence_vvma_asid:
+	/*
+	 * rs1 = zero
+	 * rs2 = a0 (ASID)
+	 * HFENCE.VVMA zero, a0
+	 * 0010001 01010 00000 000 00000 1110011
+	 */
+	.word 0x22a00073
+	ret
+
+	.align 3
+	.global __sbi_hfence_vvma_va
+__sbi_hfence_vvma_va:
+	/*
+	 * rs1 = a0 (VA)
+	 * rs2 = zero
+	 * HFENCE.VVMA zero, a0
+	 * 0010001 00000 01010 000 00000 1110011
+	 */
+	.word 0x22050073
+	ret
+
+	.align 3
+	.global __sbi_hfence_vvma_all
+__sbi_hfence_vvma_all:
+	/*
+	 * rs1 = zero
+	 * rs2 = zero
+	 * HFENCE.VVMA
+	 * 0010001 00000 00000 000 00000 1110011
+	 */
+	.word 0x22000073
+	ret

lib/sbi/sbi_hsm.c

@@ -0,0 +1,493 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Atish Patra <atish.patra@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_barrier.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/riscv_atomic.h>
+#include <sbi/sbi_bitops.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_domain.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_ecall_interface.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_hartmask.h>
+#include <sbi/sbi_hsm.h>
+#include <sbi/sbi_init.h>
+#include <sbi/sbi_ipi.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi/sbi_system.h>
+#include <sbi/sbi_timer.h>
+#include <sbi/sbi_console.h>
+
+static const struct sbi_hsm_device *hsm_dev = NULL;
+static unsigned long hart_data_offset;
+
+/** Per hart specific data to manage state transition **/
+struct sbi_hsm_data {
+	atomic_t state;
+	unsigned long suspend_type;
+	unsigned long saved_mie;
+	unsigned long saved_mip;
+};
+
+static inline int __sbi_hsm_hart_get_state(u32 hartid)
+{
+	struct sbi_hsm_data *hdata;
+	struct sbi_scratch *scratch;
+
+	scratch = sbi_hartid_to_scratch(hartid);
+	if (!scratch)
+		return SBI_EINVAL;
+
+	hdata = sbi_scratch_offset_ptr(scratch, hart_data_offset);
+	return atomic_read(&hdata->state);
+}
+
+int sbi_hsm_hart_get_state(const struct sbi_domain *dom, u32 hartid)
+{
+	if (!sbi_domain_is_assigned_hart(dom, hartid))
+		return SBI_EINVAL;
+
+	return __sbi_hsm_hart_get_state(hartid);
+}
+
+/**
+ * Get ulong HART mask for given HART base ID
+ * @param dom the domain to be used for output HART mask
+ * @param hbase the HART base ID
+ * @param out_hmask the output ulong HART mask
+ * @return 0 on success and SBI_Exxx (< 0) on failure
+ * Note: the output HART mask will be set to zero on failure as well.
+ */
+int sbi_hsm_hart_interruptible_mask(const struct sbi_domain *dom,
+				    ulong hbase, ulong *out_hmask)
+{
+	int hstate;
+	ulong i, hmask, dmask;
+	ulong hend = sbi_scratch_last_hartid() + 1;
+
+	*out_hmask = 0;
+	if (hend <= hbase)
+		return SBI_EINVAL;
+	if (BITS_PER_LONG < (hend - hbase))
+		hend = hbase + BITS_PER_LONG;
+
+	dmask = sbi_domain_get_assigned_hartmask(dom, hbase);
+	for (i = hbase; i < hend; i++) {
+		hmask = 1UL << (i - hbase);
+		if (dmask & hmask) {
+			hstate = __sbi_hsm_hart_get_state(i);
+			if (hstate == SBI_HSM_STATE_STARTED ||
+			    hstate == SBI_HSM_STATE_SUSPENDED)
+				*out_hmask |= hmask;
+		}
+	}
+
+	return 0;
+}
+
+void sbi_hsm_prepare_next_jump(struct sbi_scratch *scratch, u32 hartid)
+{
+	u32 oldstate;
+	struct sbi_hsm_data *hdata = sbi_scratch_offset_ptr(scratch,
+							    hart_data_offset);
+
+	oldstate = atomic_cmpxchg(&hdata->state, SBI_HSM_STATE_START_PENDING,
+				  SBI_HSM_STATE_STARTED);
+	if (oldstate != SBI_HSM_STATE_START_PENDING)
+		sbi_hart_hang();
+}
+
+static void sbi_hsm_hart_wait(struct sbi_scratch *scratch, u32 hartid)
+{
+	unsigned long saved_mie;
+	struct sbi_hsm_data *hdata = sbi_scratch_offset_ptr(scratch,
+							    hart_data_offset);
+	/* Save MIE CSR */
+	saved_mie = csr_read(CSR_MIE);
+
+	/* Set MSIE bit to receive IPI */
+	csr_set(CSR_MIE, MIP_MSIP);
+
+	/* Wait for hart_add call*/
+	while (atomic_read(&hdata->state) != SBI_HSM_STATE_START_PENDING) {
+		wfi();
+	};
+
+	/* Restore MIE CSR */
+	csr_write(CSR_MIE, saved_mie);
+
+	/*
+	 * No need to clear IPI here because the sbi_ipi_init() will
+	 * clear it for current HART via sbi_platform_ipi_init().
+	 */
+}
+
+const struct sbi_hsm_device *sbi_hsm_get_device(void)
+{
+	return hsm_dev;
+}
+
+void sbi_hsm_set_device(const struct sbi_hsm_device *dev)
+{
+	if (!dev || hsm_dev)
+		return;
+
+	hsm_dev = dev;
+}
+
+static bool hsm_device_has_hart_hotplug(void)
+{
+	if (hsm_dev && hsm_dev->hart_start && hsm_dev->hart_stop)
+		return true;
+	return false;
+}
+
+static bool hsm_device_has_hart_secondary_boot(void)
+{
+	if (hsm_dev && hsm_dev->hart_start && !hsm_dev->hart_stop)
+		return true;
+	return false;
+}
+
+static int hsm_device_hart_start(u32 hartid, ulong saddr)
+{
+	if (hsm_dev && hsm_dev->hart_start)
+		return hsm_dev->hart_start(hartid, saddr);
+	return SBI_ENOTSUPP;
+}
+
+static int hsm_device_hart_stop(void)
+{
+	if (hsm_dev && hsm_dev->hart_stop)
+		return hsm_dev->hart_stop();
+	return SBI_ENOTSUPP;
+}
+
+static int hsm_device_hart_suspend(u32 suspend_type, ulong raddr)
+{
+	if (hsm_dev && hsm_dev->hart_suspend)
+		return hsm_dev->hart_suspend(suspend_type, raddr);
+	return SBI_ENOTSUPP;
+}
+
+int sbi_hsm_init(struct sbi_scratch *scratch, u32 hartid, bool cold_boot)
+{
+	u32 i;
+	struct sbi_scratch *rscratch;
+	struct sbi_hsm_data *hdata;
+
+	if (cold_boot) {
+		hart_data_offset = sbi_scratch_alloc_offset(sizeof(*hdata),
+							    "HART_DATA");
+		if (!hart_data_offset)
+			return SBI_ENOMEM;
+
+		/* Initialize hart state data for every hart */
+		for (i = 0; i <= sbi_scratch_last_hartid(); i++) {
+			rscratch = sbi_hartid_to_scratch(i);
+			if (!rscratch)
+				continue;
+
+			hdata = sbi_scratch_offset_ptr(rscratch,
+						       hart_data_offset);
+			ATOMIC_INIT(&hdata->state,
+				    (i == hartid) ?
+				    SBI_HSM_STATE_START_PENDING :
+				    SBI_HSM_STATE_STOPPED);
+		}
+	} else {
+		sbi_hsm_hart_wait(scratch, hartid);
+	}
+
+	return 0;
+}
+
+void __noreturn sbi_hsm_exit(struct sbi_scratch *scratch)
+{
+	u32 hstate;
+	struct sbi_hsm_data *hdata = sbi_scratch_offset_ptr(scratch,
+							    hart_data_offset);
+	void (*jump_warmboot)(void) = (void (*)(void))scratch->warmboot_addr;
+
+	hstate = atomic_cmpxchg(&hdata->state, SBI_HSM_STATE_STOP_PENDING,
+				SBI_HSM_STATE_STOPPED);
+	if (hstate != SBI_HSM_STATE_STOP_PENDING)
+		goto fail_exit;
+
+	if (hsm_device_has_hart_hotplug()) {
+		hsm_device_hart_stop();
+		/* It should never reach here */
+		goto fail_exit;
+	}
+
+	/**
+	 * As platform is lacking support for hotplug, directly jump to warmboot
+	 * and wait for interrupts in warmboot. We do it preemptively in order
+	 * preserve the hart states and reuse the code path for hotplug.
+	 */
+	jump_warmboot();
+
+fail_exit:
+	/* It should never reach here */
+	sbi_printf("ERR: Failed stop hart [%u]\n", current_hartid());
+	sbi_hart_hang();
+}
+
+int sbi_hsm_hart_start(struct sbi_scratch *scratch,
+		       const struct sbi_domain *dom,
+		       u32 hartid, ulong saddr, ulong smode, ulong priv)
+{
+	unsigned long init_count;
+	unsigned int hstate;
+	struct sbi_scratch *rscratch;
+	struct sbi_hsm_data *hdata;
+
+	/* For now, we only allow start mode to be S-mode or U-mode. */
+	if (smode != PRV_S && smode != PRV_U)
+		return SBI_EINVAL;
+	if (dom && !sbi_domain_is_assigned_hart(dom, hartid))
+		return SBI_EINVAL;
+	if (dom && !sbi_domain_check_addr(dom, saddr, smode,
+					  SBI_DOMAIN_EXECUTE))
+		return SBI_EINVALID_ADDR;
+
+	rscratch = sbi_hartid_to_scratch(hartid);
+	if (!rscratch)
+		return SBI_EINVAL;
+	hdata = sbi_scratch_offset_ptr(rscratch, hart_data_offset);
+	hstate = atomic_cmpxchg(&hdata->state, SBI_HSM_STATE_STOPPED,
+				SBI_HSM_STATE_START_PENDING);
+	if (hstate == SBI_HSM_STATE_STARTED)
+		return SBI_EALREADY;
+
+	/**
+	 * if a hart is already transition to start or stop, another start call
+	 * is considered as invalid request.
+	 */
+	if (hstate != SBI_HSM_STATE_STOPPED)
+		return SBI_EINVAL;
+
+	init_count = sbi_init_count(hartid);
+	rscratch->next_arg1 = priv;
+	rscratch->next_addr = saddr;
+	rscratch->next_mode = smode;
+
+	if (hsm_device_has_hart_hotplug() ||
+	   (hsm_device_has_hart_secondary_boot() && !init_count)) {
+		return hsm_device_hart_start(hartid, scratch->warmboot_addr);
+	} else {
+		sbi_ipi_raw_send(hartid);
+	}
+
+	return 0;
+}
+
+int sbi_hsm_hart_stop(struct sbi_scratch *scratch, bool exitnow)
+{
+	int oldstate;
+	const struct sbi_domain *dom = sbi_domain_thishart_ptr();
+	struct sbi_hsm_data *hdata = sbi_scratch_offset_ptr(scratch,
+							    hart_data_offset);
+
+	if (!dom)
+		return SBI_EFAIL;
+
+	oldstate = atomic_cmpxchg(&hdata->state, SBI_HSM_STATE_STARTED,
+				  SBI_HSM_STATE_STOP_PENDING);
+	if (oldstate != SBI_HSM_STATE_STARTED) {
+		sbi_printf("%s: ERR: The hart is in invalid state [%u]\n",
+			   __func__, oldstate);
+		return SBI_EFAIL;
+	}
+
+	if (exitnow)
+		sbi_exit(scratch);
+
+	return 0;
+}
+
+static int __sbi_hsm_suspend_ret_default(struct sbi_scratch *scratch)
+{
+	/* Wait for interrupt */
+	wfi();
+
+	return 0;
+}
+
+static void __sbi_hsm_suspend_non_ret_save(struct sbi_scratch *scratch)
+{
+	struct sbi_hsm_data *hdata = sbi_scratch_offset_ptr(scratch,
+							    hart_data_offset);
+
+	/*
+	 * We will be resuming in warm-boot path so the MIE and MIP CSRs
+	 * will be back to initial state. It is possible that HART has
+	 * configured timer event before going to suspend state so we
+	 * should save MIE and MIP CSRs and restore it after resuming.
+	 *
+	 * Further, the M-mode bits in MIP CSR are read-only and set by
+	 * external devices (such as interrupt controller) whereas all
+	 * VS-mode bits in MIP are read-only alias of bits in HVIP CSR.
+	 *
+	 * This means we should only save/restore S-mode bits of MIP CSR
+	 * such as MIP.SSIP and MIP.STIP.
+	 */
+
+	hdata->saved_mie = csr_read(CSR_MIE);
+	hdata->saved_mip = csr_read(CSR_MIP) & (MIP_SSIP | MIP_STIP);
+}
+
+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_MIE, hdata->saved_mie);
+	csr_write(CSR_MIP, (hdata->saved_mip & (MIP_SSIP | MIP_STIP)));
+}
+
+static int __sbi_hsm_suspend_non_ret_default(struct sbi_scratch *scratch,
+					     ulong raddr)
+{
+	void (*jump_warmboot)(void) = (void (*)(void))scratch->warmboot_addr;
+
+	/*
+	 * Save some of the M-mode CSRs which should be restored after
+	 * resuming from suspend state
+	 */
+	__sbi_hsm_suspend_non_ret_save(scratch);
+
+	/* Wait for interrupt */
+	wfi();
+
+	/*
+	 * Directly jump to warm reboot to simulate resume from a
+	 * non-retentive suspend.
+	 */
+	jump_warmboot();
+
+	return 0;
+}
+
+void sbi_hsm_hart_resume_start(struct sbi_scratch *scratch)
+{
+	int oldstate;
+	struct sbi_hsm_data *hdata = sbi_scratch_offset_ptr(scratch,
+							    hart_data_offset);
+
+	/* If current HART was SUSPENDED then set RESUME_PENDING state */
+	oldstate = atomic_cmpxchg(&hdata->state, SBI_HSM_STATE_SUSPENDED,
+			SBI_HSM_STATE_RESUME_PENDING);
+	if (oldstate != SBI_HSM_STATE_SUSPENDED) {
+		sbi_printf("%s: ERR: The hart is in invalid state [%u]\n",
+			   __func__, oldstate);
+		sbi_hart_hang();
+	}
+}
+
+void sbi_hsm_hart_resume_finish(struct sbi_scratch *scratch)
+{
+	u32 oldstate;
+	struct sbi_hsm_data *hdata = sbi_scratch_offset_ptr(scratch,
+							    hart_data_offset);
+
+	/* If current HART was RESUME_PENDING then set STARTED state */
+	oldstate = atomic_cmpxchg(&hdata->state, SBI_HSM_STATE_RESUME_PENDING,
+				  SBI_HSM_STATE_STARTED);
+	if (oldstate != SBI_HSM_STATE_RESUME_PENDING) {
+		sbi_printf("%s: ERR: The hart is in invalid state [%u]\n",
+			   __func__, oldstate);
+		sbi_hart_hang();
+	}
+
+	/*
+	 * Restore some of the M-mode CSRs which we are re-configured by
+	 * the warm-boot sequence.
+	 */
+	__sbi_hsm_suspend_non_ret_restore(scratch);
+}
+
+int sbi_hsm_hart_suspend(struct sbi_scratch *scratch, u32 suspend_type,
+			 ulong raddr, ulong rmode, ulong priv)
+{
+	int oldstate, ret;
+	const struct sbi_domain *dom = sbi_domain_thishart_ptr();
+	struct sbi_hsm_data *hdata = sbi_scratch_offset_ptr(scratch,
+							    hart_data_offset);
+
+	/* For now, we only allow suspend from S-mode or U-mode. */
+
+	/* Sanity check on domain assigned to current HART */
+	if (!dom)
+		return SBI_EINVAL;
+
+	/* Sanity check on suspend type */
+	if (SBI_HSM_SUSPEND_RET_DEFAULT < suspend_type &&
+	    suspend_type < SBI_HSM_SUSPEND_RET_PLATFORM)
+		return SBI_EINVAL;
+	if (SBI_HSM_SUSPEND_NON_RET_DEFAULT < suspend_type &&
+	    suspend_type < SBI_HSM_SUSPEND_NON_RET_PLATFORM)
+		return SBI_EINVAL;
+
+	/* Additional sanity check for non-retentive suspend */
+	if (suspend_type & SBI_HSM_SUSP_NON_RET_BIT) {
+		if (rmode != PRV_S && rmode != PRV_U)
+			return SBI_EINVAL;
+		if (dom && !sbi_domain_check_addr(dom, raddr, rmode,
+						  SBI_DOMAIN_EXECUTE))
+			return SBI_EINVALID_ADDR;
+	}
+
+	/* Save the resume address and resume mode */
+	scratch->next_arg1 = priv;
+	scratch->next_addr = raddr;
+	scratch->next_mode = rmode;
+
+	/* Directly move from STARTED to SUSPENDED state */
+	oldstate = atomic_cmpxchg(&hdata->state, SBI_HSM_STATE_STARTED,
+				  SBI_HSM_STATE_SUSPENDED);
+	if (oldstate != SBI_HSM_STATE_STARTED) {
+		sbi_printf("%s: ERR: The hart is in invalid state [%u]\n",
+			   __func__, oldstate);
+		ret = SBI_EDENIED;
+		goto fail_restore_state;
+	}
+
+	/* Save the suspend type */
+	hdata->suspend_type = suspend_type;
+
+	/* Try platform specific suspend */
+	ret = hsm_device_hart_suspend(suspend_type, scratch->warmboot_addr);
+	if (ret == SBI_ENOTSUPP) {
+		/* Try generic implementation of default suspend types */
+		if (suspend_type == SBI_HSM_SUSPEND_RET_DEFAULT) {
+			ret = __sbi_hsm_suspend_ret_default(scratch);
+		} else if (suspend_type == SBI_HSM_SUSPEND_NON_RET_DEFAULT) {
+			ret = __sbi_hsm_suspend_non_ret_default(scratch,
+						scratch->warmboot_addr);
+		}
+	}
+
+fail_restore_state:
+	/*
+	 * We might have successfully resumed from retentive suspend
+	 * or suspend failed. In both cases, we restore state of hart.
+	 */
+	oldstate = atomic_cmpxchg(&hdata->state, SBI_HSM_STATE_SUSPENDED,
+				  SBI_HSM_STATE_STARTED);
+	if (oldstate != SBI_HSM_STATE_SUSPENDED) {
+		sbi_printf("%s: ERR: The hart is in invalid state [%u]\n",
+			   __func__, oldstate);
+		sbi_hart_hang();
+	}
+
+	return ret;
+}

lib/sbi/sbi_illegal_insn.c

@@ -0,0 +1,143 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/sbi_bitops.h>
+#include <sbi/sbi_emulate_csr.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_illegal_insn.h>
+#include <sbi/sbi_trap.h>
+#include <sbi/sbi_unpriv.h>
+
+typedef int (*illegal_insn_func)(ulong insn, struct sbi_trap_regs *regs);
+
+static int truly_illegal_insn(ulong insn, struct sbi_trap_regs *regs)
+{
+	struct sbi_trap_info trap;
+
+	trap.epc = regs->mepc;
+	trap.cause = CAUSE_ILLEGAL_INSTRUCTION;
+	trap.tval = insn;
+	trap.tval2 = 0;
+	trap.tinst = 0;
+
+	return sbi_trap_redirect(regs, &trap);
+}
+
+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;
+	ulong csr_val, new_csr_val;
+
+	/* TODO: Ensure that we got CSR read/write instruction */
+
+	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:
+		new_csr_val = rs1_val;
+		do_write    = 1;
+		break;
+	case 2:
+		new_csr_val = csr_val | rs1_val;
+		break;
+	case 3:
+		new_csr_val = csr_val & ~rs1_val;
+		break;
+	case 5:
+		new_csr_val = rs1_num;
+		do_write    = 1;
+		break;
+	case 6:
+		new_csr_val = csr_val | rs1_num;
+		break;
+	case 7:
+		new_csr_val = csr_val & ~rs1_num;
+		break;
+	default:
+		return truly_illegal_insn(insn, regs);
+	};
+
+	if (do_write && sbi_emulate_csr_write(csr_num, regs, new_csr_val))
+		return truly_illegal_insn(insn, regs);
+
+	SET_RD(insn, regs, csr_val);
+
+	regs->mepc += 4;
+
+	return 0;
+}
+
+static illegal_insn_func illegal_insn_table[32] = {
+	truly_illegal_insn, /* 0 */
+	truly_illegal_insn, /* 1 */
+	truly_illegal_insn, /* 2 */
+	truly_illegal_insn, /* 3 */
+	truly_illegal_insn, /* 4 */
+	truly_illegal_insn, /* 5 */
+	truly_illegal_insn, /* 6 */
+	truly_illegal_insn, /* 7 */
+	truly_illegal_insn, /* 8 */
+	truly_illegal_insn, /* 9 */
+	truly_illegal_insn, /* 10 */
+	truly_illegal_insn, /* 11 */
+	truly_illegal_insn, /* 12 */
+	truly_illegal_insn, /* 13 */
+	truly_illegal_insn, /* 14 */
+	truly_illegal_insn, /* 15 */
+	truly_illegal_insn, /* 16 */
+	truly_illegal_insn, /* 17 */
+	truly_illegal_insn, /* 18 */
+	truly_illegal_insn, /* 19 */
+	truly_illegal_insn, /* 20 */
+	truly_illegal_insn, /* 21 */
+	truly_illegal_insn, /* 22 */
+	truly_illegal_insn, /* 23 */
+	truly_illegal_insn, /* 24 */
+	truly_illegal_insn, /* 25 */
+	truly_illegal_insn, /* 26 */
+	truly_illegal_insn, /* 27 */
+	system_opcode_insn, /* 28 */
+	truly_illegal_insn, /* 29 */
+	truly_illegal_insn, /* 30 */
+	truly_illegal_insn  /* 31 */
+};
+
+int sbi_illegal_insn_handler(ulong insn, struct sbi_trap_regs *regs)
+{
+	struct sbi_trap_info uptrap;
+
+	/*
+	 * We only deal with 32-bit (or longer) illegal instructions. If we
+	 * see instruction is zero OR instruction is 16-bit then we fetch and
+	 * check the instruction encoding using unprivilege access.
+	 *
+	 * The program counter (PC) in RISC-V world is always 2-byte aligned
+	 * so handling only 32-bit (or longer) illegal instructions also help
+	 * the case where MTVAL CSR contains instruction address for illegal
+	 * instruction trap.
+	 */
+
+	if (unlikely((insn & 3) != 3)) {
+		insn = sbi_get_insn(regs->mepc, &uptrap);
+		if (uptrap.cause) {
+			uptrap.epc = regs->mepc;
+			return sbi_trap_redirect(regs, &uptrap);
+		}
+		if ((insn & 3) != 3)
+			return truly_illegal_insn(insn, regs);
+	}
+
+	return illegal_insn_table[(insn & 0x7c) >> 2](insn, regs);
+}

lib/sbi/sbi_init.c

@@ -0,0 +1,530 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#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_domain.h>
+#include <sbi/sbi_ecall.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_hartmask.h>
+#include <sbi/sbi_hsm.h>
+#include <sbi/sbi_ipi.h>
+#include <sbi/sbi_platform.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>
+
+#define BANNER                                              \
+	"   ____                    _____ ____ _____\n"     \
+	"  / __ \\                  / ____|  _ \\_   _|\n"  \
+	" | |  | |_ __   ___ _ __ | (___ | |_) || |\n"      \
+	" | |  | | '_ \\ / _ \\ '_ \\ \\___ \\|  _ < | |\n" \
+	" | |__| | |_) |  __/ | | |____) | |_) || |_\n"     \
+	"  \\____/| .__/ \\___|_| |_|_____/|____/_____|\n"  \
+	"        | |\n"                                     \
+	"        |_|\n\n"
+
+static void sbi_boot_print_banner(struct sbi_scratch *scratch)
+{
+	if (scratch->options & SBI_SCRATCH_NO_BOOT_PRINTS)
+		return;
+
+#ifdef OPENSBI_VERSION_GIT
+	sbi_printf("\nOpenSBI %s\n", OPENSBI_VERSION_GIT);
+#else
+	sbi_printf("\nOpenSBI v%d.%d\n", OPENSBI_VERSION_MAJOR,
+		   OPENSBI_VERSION_MINOR);
+#endif
+
+	sbi_printf(BANNER);
+}
+
+static void sbi_boot_print_general(struct sbi_scratch *scratch)
+{
+	char str[128];
+	const struct sbi_hsm_device *hdev;
+	const struct sbi_ipi_device *idev;
+	const struct sbi_timer_device *tdev;
+	const struct sbi_console_device *cdev;
+	const struct sbi_system_reset_device *srdev;
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+
+	if (scratch->options & SBI_SCRATCH_NO_BOOT_PRINTS)
+		return;
+
+	/* Platform details */
+	sbi_printf("Platform Name             : %s\n",
+		   sbi_platform_name(plat));
+	sbi_platform_get_features_str(plat, str, sizeof(str));
+	sbi_printf("Platform Features         : %s\n", str);
+	sbi_printf("Platform HART Count       : %u\n",
+		   sbi_platform_hart_count(plat));
+	idev = sbi_ipi_get_device();
+	sbi_printf("Platform IPI Device       : %s\n",
+		   (idev) ? idev->name : "---");
+	tdev = sbi_timer_get_device();
+	sbi_printf("Platform Timer Device     : %s\n",
+		   (tdev) ? tdev->name : "---");
+	cdev = sbi_console_get_device();
+	sbi_printf("Platform Console Device   : %s\n",
+		   (cdev) ? cdev->name : "---");
+	hdev = sbi_hsm_get_device();
+	sbi_printf("Platform HSM Device       : %s\n",
+		   (hdev) ? hdev->name : "---");
+	srdev = sbi_system_reset_get_device();
+	sbi_printf("Platform SysReset Device  : %s\n",
+		   (srdev) ? srdev->name : "---");
+
+	/* Firmware details */
+	sbi_printf("Firmware Base             : 0x%lx\n", scratch->fw_start);
+	sbi_printf("Firmware Size             : %d KB\n",
+		   (u32)(scratch->fw_size / 1024));
+
+	/* SBI details */
+	sbi_printf("Runtime SBI Version       : %d.%d\n",
+		   sbi_ecall_version_major(), sbi_ecall_version_minor());
+	sbi_printf("\n");
+}
+
+static void sbi_boot_print_domains(struct sbi_scratch *scratch)
+{
+	if (scratch->options & SBI_SCRATCH_NO_BOOT_PRINTS)
+		return;
+
+	/* Domain details */
+	sbi_domain_dump_all("      ");
+}
+
+static void sbi_boot_print_hart(struct sbi_scratch *scratch, u32 hartid)
+{
+	int xlen;
+	char str[128];
+	const struct sbi_domain *dom = sbi_domain_thishart_ptr();
+
+	if (scratch->options & SBI_SCRATCH_NO_BOOT_PRINTS)
+		return;
+
+	/* Determine MISA XLEN and MISA string */
+	xlen = misa_xlen();
+	if (xlen < 1) {
+		sbi_printf("Error %d getting MISA XLEN\n", xlen);
+		sbi_hart_hang();
+	}
+
+	/* Boot HART details */
+	sbi_printf("Boot HART ID              : %u\n", hartid);
+	sbi_printf("Boot HART Domain          : %s\n", dom->name);
+	misa_string(xlen, str, sizeof(str));
+	sbi_printf("Boot HART ISA             : %s\n", str);
+	sbi_hart_get_features_str(scratch, str, sizeof(str));
+	sbi_printf("Boot HART Features        : %s\n", str);
+	sbi_printf("Boot HART PMP Count       : %d\n",
+		   sbi_hart_pmp_count(scratch));
+	sbi_printf("Boot HART PMP Granularity : %lu\n",
+		   sbi_hart_pmp_granularity(scratch));
+	sbi_printf("Boot HART PMP Address Bits: %d\n",
+		   sbi_hart_pmp_addrbits(scratch));
+	sbi_printf("Boot HART MHPM Count      : %d\n",
+		   sbi_hart_mhpm_count(scratch));
+	sbi_printf("Boot HART MHPM Count      : %d\n",
+		   sbi_hart_mhpm_count(scratch));
+	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;
+
+	/* Save MIE CSR */
+	saved_mie = csr_read(CSR_MIE);
+
+	/* Set MSIE bit to receive IPI */
+	csr_set(CSR_MIE, MIP_MSIP);
+
+	/* Acquire coldboot lock */
+	spin_lock(&coldboot_lock);
+
+	/* Mark current HART as waiting */
+	sbi_hartmask_set_hart(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));
+	};
+
+	/* Acquire coldboot lock */
+	spin_lock(&coldboot_lock);
+
+	/* Unmark current HART as waiting */
+	sbi_hartmask_clear_hart(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.
+	 */
+}
+
+static void wake_coldboot_harts(struct sbi_scratch *scratch, u32 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 */
+	for (int i = 0; i <= sbi_scratch_last_hartid(); i++) {
+		if ((i != hartid) &&
+		    sbi_hartmask_test_hart(i, &coldboot_wait_hmask))
+			sbi_ipi_raw_send(i);
+	}
+
+	/* Release coldboot lock */
+	spin_unlock(&coldboot_lock);
+}
+
+static unsigned long init_count_offset;
+
+static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
+{
+	int rc;
+	unsigned long *init_count;
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+
+	/* Note: This has to be first thing in coldboot init sequence */
+	rc = sbi_scratch_init(scratch);
+	if (rc)
+		sbi_hart_hang();
+
+	/* Note: This has to be second thing in coldboot init sequence */
+	rc = sbi_domain_init(scratch, hartid);
+	if (rc)
+		sbi_hart_hang();
+
+	init_count_offset = sbi_scratch_alloc_offset(__SIZEOF_POINTER__,
+						     "INIT_COUNT");
+	if (!init_count_offset)
+		sbi_hart_hang();
+
+	rc = sbi_hsm_init(scratch, hartid, TRUE);
+	if (rc)
+		sbi_hart_hang();
+
+	rc = sbi_platform_early_init(plat, TRUE);
+	if (rc)
+		sbi_hart_hang();
+
+	rc = sbi_hart_init(scratch, TRUE);
+	if (rc)
+		sbi_hart_hang();
+
+	rc = sbi_console_init(scratch);
+	if (rc)
+		sbi_hart_hang();
+
+	sbi_boot_print_banner(scratch);
+
+	rc = sbi_platform_irqchip_init(plat, TRUE);
+	if (rc) {
+		sbi_printf("%s: platform irqchip init failed (error %d)\n",
+			   __func__, rc);
+		sbi_hart_hang();
+	}
+
+	rc = sbi_ipi_init(scratch, TRUE);
+	if (rc) {
+		sbi_printf("%s: ipi init failed (error %d)\n", __func__, rc);
+		sbi_hart_hang();
+	}
+
+	rc = sbi_tlb_init(scratch, TRUE);
+	if (rc) {
+		sbi_printf("%s: tlb init failed (error %d)\n", __func__, rc);
+		sbi_hart_hang();
+	}
+
+	rc = sbi_timer_init(scratch, TRUE);
+	if (rc) {
+		sbi_printf("%s: timer init failed (error %d)\n", __func__, rc);
+		sbi_hart_hang();
+	}
+
+	rc = sbi_ecall_init();
+	if (rc) {
+		sbi_printf("%s: ecall init failed (error %d)\n", __func__, rc);
+		sbi_hart_hang();
+	}
+
+	sbi_boot_print_general(scratch);
+
+	/*
+	 * Note: Finalize domains after HSM initialization so that we
+	 * can startup non-root domains.
+	 * Note: Finalize domains before HART PMP configuration so
+	 * that we use correct domain for configuring PMP.
+	 */
+	rc = sbi_domain_finalize(scratch, hartid);
+	if (rc) {
+		sbi_printf("%s: domain finalize failed (error %d)\n",
+			   __func__, rc);
+		sbi_hart_hang();
+	}
+
+	sbi_boot_print_domains(scratch);
+
+	rc = sbi_hart_pmp_configure(scratch);
+	if (rc) {
+		sbi_printf("%s: PMP configure failed (error %d)\n",
+			   __func__, rc);
+		sbi_hart_hang();
+	}
+
+	/*
+	 * Note: Platform final initialization should be last so that
+	 * it sees correct domain assignment and PMP configuration.
+	 */
+	rc = sbi_platform_final_init(plat, TRUE);
+	if (rc) {
+		sbi_printf("%s: platform final init failed (error %d)\n",
+			   __func__, rc);
+		sbi_hart_hang();
+	}
+
+	sbi_boot_print_hart(scratch, hartid);
+
+	wake_coldboot_harts(scratch, hartid);
+
+	init_count = sbi_scratch_offset_ptr(scratch, init_count_offset);
+	(*init_count)++;
+
+	sbi_hsm_prepare_next_jump(scratch, hartid);
+	sbi_hart_switch_mode(hartid, scratch->next_arg1, scratch->next_addr,
+			     scratch->next_mode, FALSE);
+}
+
+static void init_warm_startup(struct sbi_scratch *scratch, u32 hartid)
+{
+	int rc;
+	unsigned long *init_count;
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+
+	if (!init_count_offset)
+		sbi_hart_hang();
+
+	rc = sbi_hsm_init(scratch, hartid, FALSE);
+	if (rc)
+		sbi_hart_hang();
+
+	rc = sbi_platform_early_init(plat, FALSE);
+	if (rc)
+		sbi_hart_hang();
+
+	rc = sbi_hart_init(scratch, FALSE);
+	if (rc)
+		sbi_hart_hang();
+
+	rc = sbi_platform_irqchip_init(plat, FALSE);
+	if (rc)
+		sbi_hart_hang();
+
+	rc = sbi_ipi_init(scratch, FALSE);
+	if (rc)
+		sbi_hart_hang();
+
+	rc = sbi_tlb_init(scratch, FALSE);
+	if (rc)
+		sbi_hart_hang();
+
+	rc = sbi_timer_init(scratch, FALSE);
+	if (rc)
+		sbi_hart_hang();
+
+	rc = sbi_hart_pmp_configure(scratch);
+	if (rc)
+		sbi_hart_hang();
+
+	rc = sbi_platform_final_init(plat, FALSE);
+	if (rc)
+		sbi_hart_hang();
+
+	init_count = sbi_scratch_offset_ptr(scratch, init_count_offset);
+	(*init_count)++;
+
+	sbi_hsm_prepare_next_jump(scratch, hartid);
+}
+
+static void init_warm_resume(struct sbi_scratch *scratch)
+{
+	int rc;
+
+	sbi_hsm_hart_resume_start(scratch);
+
+	rc = sbi_hart_reinit(scratch);
+	if (rc)
+		sbi_hart_hang();
+
+	rc = sbi_hart_pmp_configure(scratch);
+	if (rc)
+		sbi_hart_hang();
+
+	sbi_hsm_hart_resume_finish(scratch);
+}
+
+static void __noreturn init_warmboot(struct sbi_scratch *scratch, u32 hartid)
+{
+	int hstate;
+
+	wait_for_coldboot(scratch, hartid);
+
+	hstate = sbi_hsm_hart_get_state(sbi_domain_thishart_ptr(), hartid);
+	if (hstate < 0)
+		sbi_hart_hang();
+
+	if (hstate == SBI_HSM_STATE_SUSPENDED)
+		init_warm_resume(scratch);
+	else
+		init_warm_startup(scratch, hartid);
+
+	sbi_hart_switch_mode(hartid, scratch->next_arg1,
+			     scratch->next_addr,
+			     scratch->next_mode, FALSE);
+}
+
+static atomic_t coldboot_lottery = ATOMIC_INITIALIZER(0);
+
+/**
+ * Initialize OpenSBI library for current HART and jump to next
+ * booting stage.
+ *
+ * The function expects following:
+ * 1. The 'mscratch' CSR is pointing to sbi_scratch of current HART
+ * 2. Stack pointer (SP) is setup for current HART
+ * 3. Interrupts are disabled in MSTATUS CSR
+ * 4. All interrupts are disabled in MIE CSR
+ *
+ * @param scratch pointer to sbi_scratch of current HART
+ */
+void __noreturn sbi_init(struct sbi_scratch *scratch)
+{
+	bool next_mode_supported	= FALSE;
+	bool coldboot			= FALSE;
+	u32 hartid			= current_hartid();
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+
+	if ((SBI_HARTMASK_MAX_BITS <= hartid) ||
+	    sbi_platform_hart_invalid(plat, hartid))
+		sbi_hart_hang();
+
+	switch (scratch->next_mode) {
+	case PRV_M:
+		next_mode_supported = TRUE;
+		break;
+	case PRV_S:
+		if (misa_extension('S'))
+			next_mode_supported = TRUE;
+		break;
+	case PRV_U:
+		if (misa_extension('U'))
+			next_mode_supported = TRUE;
+		break;
+	default:
+		sbi_hart_hang();
+	}
+
+	/*
+	 * Only the HART supporting privilege mode specified in the
+	 * scratch->next_mode should be allowed to become the coldboot
+	 * HART because the coldboot HART will be directly jumping to
+	 * the next booting stage.
+	 *
+	 * We use a lottery mechanism to select coldboot HART among
+	 * HARTs which satisfy above condition.
+	 */
+
+	if (next_mode_supported && atomic_xchg(&coldboot_lottery, 1) == 0)
+		coldboot = TRUE;
+
+	if (coldboot)
+		init_coldboot(scratch, hartid);
+	else
+		init_warmboot(scratch, hartid);
+}
+
+unsigned long sbi_init_count(u32 hartid)
+{
+	struct sbi_scratch *scratch;
+	unsigned long *init_count;
+
+	if (!init_count_offset)
+		return 0;
+
+	scratch = sbi_hartid_to_scratch(hartid);
+	if (!scratch)
+		return 0;
+
+	init_count = sbi_scratch_offset_ptr(scratch, init_count_offset);
+
+	return *init_count;
+}
+
+/**
+ * Exit OpenSBI library for current HART and stop HART
+ *
+ * The function expects following:
+ * 1. The 'mscratch' CSR is pointing to sbi_scratch of current HART
+ * 2. Stack pointer (SP) is setup for current HART
+ *
+ * @param scratch pointer to sbi_scratch of current HART
+ */
+void __noreturn sbi_exit(struct sbi_scratch *scratch)
+{
+	u32 hartid			= current_hartid();
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+
+	if (sbi_platform_hart_invalid(plat, hartid))
+		sbi_hart_hang();
+
+	sbi_platform_early_exit(plat);
+
+	sbi_timer_exit(scratch);
+
+	sbi_ipi_exit(scratch);
+
+	sbi_platform_irqchip_exit(plat);
+
+	sbi_platform_final_exit(plat);
+
+	sbi_hsm_exit(scratch);
+}

lib/sbi/sbi_ipi.c

@@ -0,0 +1,277 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ *   Nick Kossifidis <mick@ics.forth.gr>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_atomic.h>
+#include <sbi/riscv_barrier.h>
+#include <sbi/sbi_bitops.h>
+#include <sbi/sbi_domain.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_hsm.h>
+#include <sbi/sbi_init.h>
+#include <sbi/sbi_ipi.h>
+#include <sbi/sbi_platform.h>
+
+struct sbi_ipi_data {
+	unsigned long ipi_type;
+};
+
+static unsigned long ipi_data_off;
+static const struct sbi_ipi_device *ipi_dev = NULL;
+static const struct sbi_ipi_event_ops *ipi_ops_array[SBI_IPI_EVENT_MAX];
+
+static int sbi_ipi_send(struct sbi_scratch *scratch, u32 remote_hartid,
+			u32 event, void *data)
+{
+	int ret;
+	struct sbi_scratch *remote_scratch = NULL;
+	struct sbi_ipi_data *ipi_data;
+	const struct sbi_ipi_event_ops *ipi_ops;
+
+	if ((SBI_IPI_EVENT_MAX <= event) ||
+	    !ipi_ops_array[event])
+		return SBI_EINVAL;
+	ipi_ops = ipi_ops_array[event];
+
+	remote_scratch = sbi_hartid_to_scratch(remote_hartid);
+	if (!remote_scratch)
+		return SBI_EINVAL;
+
+	ipi_data = sbi_scratch_offset_ptr(remote_scratch, ipi_data_off);
+
+	if (ipi_ops->update) {
+		ret = ipi_ops->update(scratch, remote_scratch,
+				      remote_hartid, data);
+		if (ret < 0)
+			return ret;
+	}
+
+	/*
+	 * Set IPI type on remote hart's scratch area and
+	 * trigger the interrupt
+	 */
+	atomic_raw_set_bit(event, &ipi_data->ipi_type);
+	smp_wmb();
+
+	if (ipi_dev && ipi_dev->ipi_send)
+		ipi_dev->ipi_send(remote_hartid);
+
+	if (ipi_ops->sync)
+		ipi_ops->sync(scratch);
+
+	return 0;
+}
+
+/**
+ * As this this function only handlers scalar values of hart mask, it must be
+ * set to all online harts if the intention is to send IPIs to all the harts.
+ * If hmask is zero, no IPIs will be sent.
+ */
+int sbi_ipi_send_many(ulong hmask, ulong hbase, u32 event, void *data)
+{
+	int rc;
+	ulong i, m;
+	struct sbi_domain *dom = sbi_domain_thishart_ptr();
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+
+	if (hbase != -1UL) {
+		rc = sbi_hsm_hart_interruptible_mask(dom, hbase, &m);
+		if (rc)
+			return rc;
+		m &= hmask;
+
+		/* Send IPIs */
+		for (i = hbase; m; i++, m >>= 1) {
+			if (m & 1UL)
+				sbi_ipi_send(scratch, i, event, data);
+		}
+	} else {
+		hbase = 0;
+		while (!sbi_hsm_hart_interruptible_mask(dom, hbase, &m)) {
+			/* Send IPIs */
+			for (i = hbase; m; i++, m >>= 1) {
+				if (m & 1UL)
+					sbi_ipi_send(scratch, i, event, data);
+			}
+			hbase += BITS_PER_LONG;
+		}
+	}
+
+	return 0;
+}
+
+int sbi_ipi_event_create(const struct sbi_ipi_event_ops *ops)
+{
+	int i, ret = SBI_ENOSPC;
+
+	if (!ops || !ops->process)
+		return SBI_EINVAL;
+
+	for (i = 0; i < SBI_IPI_EVENT_MAX; i++) {
+		if (!ipi_ops_array[i]) {
+			ret = i;
+			ipi_ops_array[i] = ops;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+void sbi_ipi_event_destroy(u32 event)
+{
+	if (SBI_IPI_EVENT_MAX <= event)
+		return;
+
+	ipi_ops_array[event] = NULL;
+}
+
+static void sbi_ipi_process_smode(struct sbi_scratch *scratch)
+{
+	csr_set(CSR_MIP, MIP_SSIP);
+}
+
+static struct sbi_ipi_event_ops ipi_smode_ops = {
+	.name = "IPI_SMODE",
+	.process = sbi_ipi_process_smode,
+};
+
+static u32 ipi_smode_event = SBI_IPI_EVENT_MAX;
+
+int sbi_ipi_send_smode(ulong hmask, ulong hbase)
+{
+	return sbi_ipi_send_many(hmask, hbase, ipi_smode_event, NULL);
+}
+
+void sbi_ipi_clear_smode(void)
+{
+	csr_clear(CSR_MIP, MIP_SSIP);
+}
+
+static void sbi_ipi_process_halt(struct sbi_scratch *scratch)
+{
+	sbi_hsm_hart_stop(scratch, TRUE);
+}
+
+static struct sbi_ipi_event_ops ipi_halt_ops = {
+	.name = "IPI_HALT",
+	.process = sbi_ipi_process_halt,
+};
+
+static u32 ipi_halt_event = SBI_IPI_EVENT_MAX;
+
+int sbi_ipi_send_halt(ulong hmask, ulong hbase)
+{
+	return sbi_ipi_send_many(hmask, hbase, ipi_halt_event, NULL);
+}
+
+void sbi_ipi_process(void)
+{
+	unsigned long ipi_type;
+	unsigned int ipi_event;
+	const struct sbi_ipi_event_ops *ipi_ops;
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+	struct sbi_ipi_data *ipi_data =
+			sbi_scratch_offset_ptr(scratch, ipi_data_off);
+	u32 hartid = current_hartid();
+
+	if (ipi_dev && ipi_dev->ipi_clear)
+		ipi_dev->ipi_clear(hartid);
+
+	ipi_type = atomic_raw_xchg_ulong(&ipi_data->ipi_type, 0);
+	ipi_event = 0;
+	while (ipi_type) {
+		if (!(ipi_type & 1UL))
+			goto skip;
+
+		ipi_ops = ipi_ops_array[ipi_event];
+		if (ipi_ops && ipi_ops->process)
+			ipi_ops->process(scratch);
+
+skip:
+		ipi_type = ipi_type >> 1;
+		ipi_event++;
+	};
+}
+
+void sbi_ipi_raw_send(u32 target_hart)
+{
+	if (ipi_dev && ipi_dev->ipi_send)
+		ipi_dev->ipi_send(target_hart);
+}
+
+const struct sbi_ipi_device *sbi_ipi_get_device(void)
+{
+	return ipi_dev;
+}
+
+void sbi_ipi_set_device(const struct sbi_ipi_device *dev)
+{
+	if (!dev || ipi_dev)
+		return;
+
+	ipi_dev = dev;
+}
+
+int sbi_ipi_init(struct sbi_scratch *scratch, bool cold_boot)
+{
+	int ret;
+	struct sbi_ipi_data *ipi_data;
+
+	if (cold_boot) {
+		ipi_data_off = sbi_scratch_alloc_offset(sizeof(*ipi_data),
+							"IPI_DATA");
+		if (!ipi_data_off)
+			return SBI_ENOMEM;
+		ret = sbi_ipi_event_create(&ipi_smode_ops);
+		if (ret < 0)
+			return ret;
+		ipi_smode_event = ret;
+		ret = sbi_ipi_event_create(&ipi_halt_ops);
+		if (ret < 0)
+			return ret;
+		ipi_halt_event = ret;
+	} else {
+		if (!ipi_data_off)
+			return SBI_ENOMEM;
+		if (SBI_IPI_EVENT_MAX <= ipi_smode_event ||
+		    SBI_IPI_EVENT_MAX <= ipi_halt_event)
+			return SBI_ENOSPC;
+	}
+
+	ipi_data = sbi_scratch_offset_ptr(scratch, ipi_data_off);
+	ipi_data->ipi_type = 0x00;
+
+	/*
+	 * Initialize platform IPI support. This will also clear any
+	 * pending IPIs for current/calling HART.
+	 */
+	ret = sbi_platform_ipi_init(sbi_platform_ptr(scratch), cold_boot);
+	if (ret)
+		return ret;
+
+	/* Enable software interrupts */
+	csr_set(CSR_MIE, MIP_MSIP);
+
+	return 0;
+}
+
+void sbi_ipi_exit(struct sbi_scratch *scratch)
+{
+	/* Disable software interrupts */
+	csr_clear(CSR_MIE, MIP_MSIP);
+
+	/* Process pending IPIs */
+	sbi_ipi_process();
+
+	/* Platform exit */
+	sbi_platform_ipi_exit(sbi_platform_ptr(scratch));
+}

lib/sbi/sbi_math.c

@@ -0,0 +1,23 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Common helper functions used across OpenSBI project.
+ *
+ * Authors:
+ *   Atish Patra <atish.patra@wdc.com>
+ */
+
+unsigned long log2roundup(unsigned long x)
+{
+	unsigned long ret = 0;
+
+	while (ret < __riscv_xlen) {
+		if (x <= (1UL << ret))
+			break;
+		ret++;
+	}
+
+	return ret;
+}

lib/sbi/sbi_misaligned_ldst.c

@@ -0,0 +1,243 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/riscv_fp.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_misaligned_ldst.h>
+#include <sbi/sbi_trap.h>
+#include <sbi/sbi_unpriv.h>
+
+union reg_data {
+	u8 data_bytes[8];
+	ulong data_ulong;
+	u64 data_u64;
+};
+
+int sbi_misaligned_load_handler(ulong addr, ulong tval2, ulong tinst,
+				struct sbi_trap_regs *regs)
+{
+	ulong insn, insn_len;
+	union reg_data val;
+	struct sbi_trap_info uptrap;
+	int i, fp = 0, shift = 0, len = 0;
+
+	if (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;
+	} else {
+		/*
+		 * Bit[0] == 0 implies trapped instruction value is
+		 * zero or special value.
+		 */
+		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) {
+		len   = 4;
+		shift = 8 * (sizeof(ulong) - len);
+#if __riscv_xlen == 64
+	} else if ((insn & INSN_MASK_LD) == INSN_MATCH_LD) {
+		len   = 8;
+		shift = 8 * (sizeof(ulong) - len);
+	} else if ((insn & INSN_MASK_LWU) == INSN_MATCH_LWU) {
+		len = 4;
+#endif
+#ifdef __riscv_flen
+	} else if ((insn & INSN_MASK_FLD) == INSN_MATCH_FLD) {
+		fp  = 1;
+		len = 8;
+	} else if ((insn & INSN_MASK_FLW) == INSN_MATCH_FLW) {
+		fp  = 1;
+		len = 4;
+#endif
+	} else if ((insn & INSN_MASK_LH) == INSN_MATCH_LH) {
+		len   = 2;
+		shift = 8 * (sizeof(ulong) - len);
+	} else if ((insn & INSN_MASK_LHU) == INSN_MATCH_LHU) {
+		len = 2;
+#if __riscv_xlen >= 64
+	} else if ((insn & INSN_MASK_C_LD) == INSN_MATCH_C_LD) {
+		len   = 8;
+		shift = 8 * (sizeof(ulong) - len);
+		insn  = RVC_RS2S(insn) << SH_RD;
+	} else if ((insn & INSN_MASK_C_LDSP) == INSN_MATCH_C_LDSP &&
+		   ((insn >> SH_RD) & 0x1f)) {
+		len   = 8;
+		shift = 8 * (sizeof(ulong) - len);
+#endif
+	} else if ((insn & INSN_MASK_C_LW) == INSN_MATCH_C_LW) {
+		len   = 4;
+		shift = 8 * (sizeof(ulong) - len);
+		insn  = RVC_RS2S(insn) << SH_RD;
+	} else if ((insn & INSN_MASK_C_LWSP) == INSN_MATCH_C_LWSP &&
+		   ((insn >> SH_RD) & 0x1f)) {
+		len   = 4;
+		shift = 8 * (sizeof(ulong) - len);
+#ifdef __riscv_flen
+	} else if ((insn & INSN_MASK_C_FLD) == INSN_MATCH_C_FLD) {
+		fp   = 1;
+		len  = 8;
+		insn = RVC_RS2S(insn) << SH_RD;
+	} else if ((insn & INSN_MASK_C_FLDSP) == INSN_MATCH_C_FLDSP) {
+		fp  = 1;
+		len = 8;
+#if __riscv_xlen == 32
+	} else if ((insn & INSN_MASK_C_FLW) == INSN_MATCH_C_FLW) {
+		fp   = 1;
+		len  = 4;
+		insn = RVC_RS2S(insn) << SH_RD;
+	} else if ((insn & INSN_MASK_C_FLWSP) == INSN_MATCH_C_FLWSP) {
+		fp  = 1;
+		len = 4;
+#endif
+#endif
+	} else {
+		uptrap.epc = regs->mepc;
+		uptrap.cause = CAUSE_MISALIGNED_LOAD;
+		uptrap.tval = addr;
+		uptrap.tval2 = tval2;
+		uptrap.tinst = tinst;
+		return sbi_trap_redirect(regs, &uptrap);
+	}
+
+	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;
+			return sbi_trap_redirect(regs, &uptrap);
+		}
+	}
+
+	if (!fp)
+		SET_RD(insn, regs, ((long)(val.data_ulong << shift)) >> shift);
+#ifdef __riscv_flen
+	else if (len == 8)
+		SET_F64_RD(insn, regs, val.data_u64);
+	else
+		SET_F32_RD(insn, regs, val.data_ulong);
+#endif
+
+	regs->mepc += insn_len;
+
+	return 0;
+}
+
+int sbi_misaligned_store_handler(ulong addr, ulong tval2, ulong tinst,
+				 struct sbi_trap_regs *regs)
+{
+	ulong insn, insn_len;
+	union reg_data val;
+	struct sbi_trap_info uptrap;
+	int i, len = 0;
+
+	if (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;
+	} else {
+		/*
+		 * Bit[0] == 0 implies trapped instruction value is
+		 * zero or special value.
+		 */
+		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);
+	}
+
+	val.data_ulong = GET_RS2(insn, regs);
+
+	if ((insn & INSN_MASK_SW) == INSN_MATCH_SW) {
+		len = 4;
+#if __riscv_xlen == 64
+	} else if ((insn & INSN_MASK_SD) == INSN_MATCH_SD) {
+		len = 8;
+#endif
+#ifdef __riscv_flen
+	} else if ((insn & INSN_MASK_FSD) == INSN_MATCH_FSD) {
+		len	     = 8;
+		val.data_u64 = GET_F64_RS2(insn, regs);
+	} else if ((insn & INSN_MASK_FSW) == INSN_MATCH_FSW) {
+		len	       = 4;
+		val.data_ulong = GET_F32_RS2(insn, regs);
+#endif
+	} else if ((insn & INSN_MASK_SH) == INSN_MATCH_SH) {
+		len = 2;
+#if __riscv_xlen >= 64
+	} else if ((insn & INSN_MASK_C_SD) == INSN_MATCH_C_SD) {
+		len	       = 8;
+		val.data_ulong = GET_RS2S(insn, regs);
+	} else if ((insn & INSN_MASK_C_SDSP) == INSN_MATCH_C_SDSP &&
+		   ((insn >> SH_RD) & 0x1f)) {
+		len	       = 8;
+		val.data_ulong = GET_RS2C(insn, regs);
+#endif
+	} else if ((insn & INSN_MASK_C_SW) == INSN_MATCH_C_SW) {
+		len	       = 4;
+		val.data_ulong = GET_RS2S(insn, regs);
+	} else if ((insn & INSN_MASK_C_SWSP) == INSN_MATCH_C_SWSP &&
+		   ((insn >> SH_RD) & 0x1f)) {
+		len	       = 4;
+		val.data_ulong = GET_RS2C(insn, regs);
+#ifdef __riscv_flen
+	} else if ((insn & INSN_MASK_C_FSD) == INSN_MATCH_C_FSD) {
+		len	     = 8;
+		val.data_u64 = GET_F64_RS2S(insn, regs);
+	} else if ((insn & INSN_MASK_C_FSDSP) == INSN_MATCH_C_FSDSP) {
+		len	     = 8;
+		val.data_u64 = GET_F64_RS2C(insn, regs);
+#if __riscv_xlen == 32
+	} else if ((insn & INSN_MASK_C_FSW) == INSN_MATCH_C_FSW) {
+		len	       = 4;
+		val.data_ulong = GET_F32_RS2S(insn, regs);
+	} else if ((insn & INSN_MASK_C_FSWSP) == INSN_MATCH_C_FSWSP) {
+		len	       = 4;
+		val.data_ulong = GET_F32_RS2C(insn, regs);
+#endif
+#endif
+	} else {
+		uptrap.epc = regs->mepc;
+		uptrap.cause = CAUSE_MISALIGNED_STORE;
+		uptrap.tval = addr;
+		uptrap.tval2 = tval2;
+		uptrap.tinst = tinst;
+		return sbi_trap_redirect(regs, &uptrap);
+	}
+
+	for (i = 0; i < len; i++) {
+		sbi_store_u8((void *)(addr + i), val.data_bytes[i],
+			     &uptrap);
+		if (uptrap.cause) {
+			uptrap.epc = regs->mepc;
+			return sbi_trap_redirect(regs, &uptrap);
+		}
+	}
+
+	regs->mepc += insn_len;
+
+	return 0;
+}

lib/sbi/sbi_platform.c

@@ -0,0 +1,81 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Atish Patra <atish.patra@wdc.com>
+ */
+
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_platform.h>
+#include <sbi/sbi_string.h>
+
+static inline char *sbi_platform_feature_id2string(unsigned long feature)
+{
+	char *fstr = NULL;
+
+	if (!feature)
+		return NULL;
+
+	switch (feature) {
+	case SBI_PLATFORM_HAS_MFAULTS_DELEGATION:
+		fstr = "mfdeleg";
+		break;
+	default:
+		break;
+	}
+
+	return fstr;
+}
+
+void sbi_platform_get_features_str(const struct sbi_platform *plat,
+				   char *features_str, int nfstr)
+{
+	unsigned long features, feat = 1UL;
+	char *temp;
+	int offset = 0;
+
+	if (!plat || !features_str || !nfstr)
+		return;
+	sbi_memset(features_str, 0, nfstr);
+
+	features = sbi_platform_get_features(plat);
+	if (!features)
+		goto done;
+
+	do {
+		if (features & feat) {
+			temp = sbi_platform_feature_id2string(feat);
+			if (temp) {
+				sbi_snprintf(features_str + offset, nfstr,
+					     "%s,", temp);
+				offset = offset + sbi_strlen(temp) + 1;
+			}
+		}
+		feat = feat << 1;
+	} while (feat <= SBI_PLATFORM_HAS_LAST_FEATURE);
+
+done:
+	if (offset)
+		features_str[offset - 1] = '\0';
+	else
+		sbi_strncpy(features_str, "none", nfstr);
+}
+
+u32 sbi_platform_hart_index(const struct sbi_platform *plat, u32 hartid)
+{
+	u32 i;
+
+	if (!plat)
+		return -1U;
+	if (plat->hart_index2id) {
+		for (i = 0; i < plat->hart_count; i++) {
+			if (plat->hart_index2id[i] == hartid)
+				return i;
+		}
+		return -1U;
+	}
+
+	return hartid;
+}

lib/sbi/sbi_scratch.c

@@ -0,0 +1,99 @@
+ /*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_locks.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_hartmask.h>
+#include <sbi/sbi_platform.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi/sbi_string.h>
+
+u32 last_hartid_having_scratch = SBI_HARTMASK_MAX_BITS;
+struct sbi_scratch *hartid_to_scratch_table[SBI_HARTMASK_MAX_BITS] = { 0 };
+
+static spinlock_t extra_lock = SPIN_LOCK_INITIALIZER;
+static unsigned long extra_offset = SBI_SCRATCH_EXTRA_SPACE_OFFSET;
+
+typedef struct sbi_scratch *(*hartid2scratch)(ulong hartid, ulong hartindex);
+
+int sbi_scratch_init(struct sbi_scratch *scratch)
+{
+	u32 i;
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+
+	for (i = 0; i < SBI_HARTMASK_MAX_BITS; i++) {
+		if (sbi_platform_hart_invalid(plat, i))
+			continue;
+		hartid_to_scratch_table[i] =
+			((hartid2scratch)scratch->hartid_to_scratch)(i,
+					sbi_platform_hart_index(plat, i));
+		if (hartid_to_scratch_table[i])
+			last_hartid_having_scratch = i;
+	}
+
+	return 0;
+}
+
+unsigned long sbi_scratch_alloc_offset(unsigned long size, const char *owner)
+{
+	u32 i;
+	void *ptr;
+	unsigned long ret = 0;
+	struct sbi_scratch *rscratch;
+
+	/*
+	 * We have a simple brain-dead allocator which never expects
+	 * anything to be free-ed hence it keeps incrementing the
+	 * next allocation offset until it runs-out of space.
+	 *
+	 * In future, we will have more sophisticated allocator which
+	 * will allow us to re-claim free-ed space.
+	 */
+
+	if (!size)
+		return 0;
+
+	if (size & (__SIZEOF_POINTER__ - 1))
+		size = (size & ~(__SIZEOF_POINTER__ - 1)) + __SIZEOF_POINTER__;
+
+	spin_lock(&extra_lock);
+
+	if (SBI_SCRATCH_SIZE < (extra_offset + size))
+		goto done;
+
+	ret = extra_offset;
+	extra_offset += size;
+
+done:
+	spin_unlock(&extra_lock);
+
+	if (ret) {
+		for (i = 0; i < sbi_scratch_last_hartid(); i++) {
+			rscratch = sbi_hartid_to_scratch(i);
+			if (!rscratch)
+				continue;
+			ptr = sbi_scratch_offset_ptr(rscratch, ret);
+			sbi_memset(ptr, 0, size);
+		}
+	}
+
+	return ret;
+}
+
+void sbi_scratch_free_offset(unsigned long offset)
+{
+	if ((offset < SBI_SCRATCH_EXTRA_SPACE_OFFSET) ||
+	    (SBI_SCRATCH_SIZE <= offset))
+		return;
+
+	/*
+	 * We don't actually free-up because it's a simple
+	 * brain-dead allocator.
+	 */
+}

lib/sbi/sbi_string.c

@@ -0,0 +1,188 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Atish Patra <atish.patra@wdc.com>
+ */
+
+/*
+ * Simple libc functions. These are not optimized at all and might have some
+ * bugs as well. Use any optimized routines from newlib or glibc if required.
+ */
+
+#include <sbi/sbi_string.h>
+
+/*
+  Provides sbi_strcmp for the completeness of supporting string functions.
+  it is not recommended to use sbi_strcmp() but use sbi_strncmp instead.
+*/
+int sbi_strcmp(const char *a, const char *b)
+{
+	/* search first diff or end of string */
+	for (; *a == *b && *a != '\0'; a++, b++)
+		;
+
+	return *a - *b;
+}
+
+int sbi_strncmp(const char *a, const char *b, size_t count)
+{
+	/* search first diff or end of string */
+	for (; count > 0 && *a == *b && *a != '\0'; a++, b++, count--)
+		;
+
+	return *a - *b;
+}
+
+size_t sbi_strlen(const char *str)
+{
+	unsigned long ret = 0;
+
+	while (*str != '\0') {
+		ret++;
+		str++;
+	}
+
+	return ret;
+}
+
+size_t sbi_strnlen(const char *str, size_t count)
+{
+	unsigned long ret = 0;
+
+	while (*str != '\0' && ret < count) {
+		ret++;
+		str++;
+		count--;
+	}
+
+	return ret;
+}
+
+char *sbi_strcpy(char *dest, const char *src)
+{
+	char *ret = dest;
+
+	while (*src != '\0') {
+		*dest++ = *src++;
+	}
+
+	return ret;
+}
+
+char *sbi_strncpy(char *dest, const char *src, size_t count)
+{
+	char *ret = dest;
+
+	while (count-- && *src != '\0') {
+		*dest++ = *src++;
+	}
+
+	return ret;
+}
+
+char *sbi_strchr(const char *s, int c)
+{
+	while (*s != '\0' && *s != (char)c)
+		s++;
+
+	if (*s == '\0')
+		return NULL;
+	else
+		return (char *)s;
+}
+
+char *sbi_strrchr(const char *s, int c)
+{
+	const char *last = s + sbi_strlen(s);
+
+	while (last > s && *last != (char)c)
+		last--;
+
+	if (*last != (char)c)
+		return NULL;
+	else
+		return (char *)last;
+}
+void *sbi_memset(void *s, int c, size_t count)
+{
+	char *temp = s;
+
+	while (count > 0) {
+		count--;
+		*temp++ = c;
+	}
+
+	return s;
+}
+
+void *sbi_memcpy(void *dest, const void *src, size_t count)
+{
+	char *temp1	  = dest;
+	const char *temp2 = src;
+
+	while (count > 0) {
+		*temp1++ = *temp2++;
+		count--;
+	}
+
+	return dest;
+}
+
+void *sbi_memmove(void *dest, const void *src, size_t count)
+{
+	char *temp1	  = (char *)dest;
+	const char *temp2 = (char *)src;
+
+	if (src == dest)
+		return dest;
+
+	if (dest < src) {
+		while (count > 0) {
+			*temp1++ = *temp2++;
+			count--;
+		}
+	} else {
+		temp1 = dest + count - 1;
+		temp2 = src + count - 1;
+
+		while (count > 0) {
+			*temp1-- = *temp2--;
+			count--;
+		}
+	}
+
+	return dest;
+}
+
+int sbi_memcmp(const void *s1, const void *s2, size_t count)
+{
+	const char *temp1 = s1;
+	const char *temp2 = s2;
+
+	for (; count > 0 && (*temp1 == *temp2); count--) {
+		temp1++;
+		temp2++;
+	}
+
+	if (count > 0)
+		return *(unsigned char *)temp1 - *(unsigned char *)temp2;
+	else
+		return 0;
+}
+
+void *sbi_memchr(const void *s, int c, size_t count)
+{
+	const unsigned char *temp = s;
+
+	while (count > 0) {
+		if ((unsigned char)c == *temp++) {
+			return (void *)(temp - 1);
+		}
+		count--;
+	}
+
+	return NULL;
+}

lib/sbi/sbi_system.c

@@ -0,0 +1,74 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ *   Nick Kossifidis <mick@ics.forth.gr>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/sbi_bitops.h>
+#include <sbi/sbi_domain.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_hsm.h>
+#include <sbi/sbi_platform.h>
+#include <sbi/sbi_system.h>
+#include <sbi/sbi_ipi.h>
+#include <sbi/sbi_init.h>
+
+static const struct sbi_system_reset_device *reset_dev = NULL;
+
+const struct sbi_system_reset_device *sbi_system_reset_get_device(void)
+{
+	return reset_dev;
+}
+
+void sbi_system_reset_set_device(const struct sbi_system_reset_device *dev)
+{
+	if (!dev || reset_dev)
+		return;
+
+	reset_dev = dev;
+}
+
+bool sbi_system_reset_supported(u32 reset_type, u32 reset_reason)
+{
+	if (reset_dev && reset_dev->system_reset_check &&
+	    reset_dev->system_reset_check(reset_type, reset_reason))
+		return TRUE;
+
+	return FALSE;
+}
+
+#include <sbi/riscv_io.h>
+void __noreturn sbi_system_reset(u32 reset_type, u32 reset_reason)
+{
+	ulong hbase = 0, hmask;
+	u32 cur_hartid = current_hartid();
+	struct sbi_domain *dom = sbi_domain_thishart_ptr();
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+
+	/* Send HALT IPI to every hart other than the current hart */
+	while (!sbi_hsm_hart_interruptible_mask(dom, hbase, &hmask)) {
+		if (hbase <= cur_hartid)
+			hmask &= ~(1UL << (cur_hartid - hbase));
+		if (hmask)
+			sbi_ipi_send_halt(hmask, hbase);
+		hbase += BITS_PER_LONG;
+	}
+
+	/* Stop current HART */
+	sbi_hsm_hart_stop(scratch, FALSE);
+
+	/* Platform specific reset if domain allowed system reset */
+	if (dom->system_reset_allowed &&
+	    reset_dev && reset_dev->system_reset)
+		reset_dev->system_reset(reset_type, reset_reason);
+
+	writew(0x5555, (void *)0x100000); // qemu poweroff
+
+	/* If platform specific reset did not work then do sbi_exit() */
+	sbi_exit(scratch);
+}

lib/sbi/sbi_timer.c

@@ -0,0 +1,151 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_platform.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi/sbi_timer.h>
+
+static unsigned long time_delta_off;
+static u64 (*get_time_val)(void);
+static const struct sbi_timer_device *timer_dev = NULL;
+
+#if __riscv_xlen == 32
+static u64 get_ticks(void)
+{
+	u32 lo, hi, tmp;
+	__asm__ __volatile__("1:\n"
+			     "rdtimeh %0\n"
+			     "rdtime %1\n"
+			     "rdtimeh %2\n"
+			     "bne %0, %2, 1b"
+			     : "=&r"(hi), "=&r"(lo), "=&r"(tmp));
+	return ((u64)hi << 32) | lo;
+}
+#else
+static u64 get_ticks(void)
+{
+	unsigned long n;
+
+	__asm__ __volatile__("rdtime %0" : "=r"(n));
+	return n;
+}
+#endif
+
+static u64 get_platform_ticks(void)
+{
+	return timer_dev->timer_value();
+}
+
+u64 sbi_timer_value(void)
+{
+	if (get_time_val)
+		return get_time_val();
+	return 0;
+}
+
+u64 sbi_timer_virt_value(void)
+{
+	u64 *time_delta = sbi_scratch_offset_ptr(sbi_scratch_thishart_ptr(),
+						 time_delta_off);
+
+	return sbi_timer_value() + *time_delta;
+}
+
+u64 sbi_timer_get_delta(void)
+{
+	u64 *time_delta = sbi_scratch_offset_ptr(sbi_scratch_thishart_ptr(),
+						 time_delta_off);
+
+	return *time_delta;
+}
+
+void sbi_timer_set_delta(ulong delta)
+{
+	u64 *time_delta = sbi_scratch_offset_ptr(sbi_scratch_thishart_ptr(),
+						 time_delta_off);
+
+	*time_delta = (u64)delta;
+}
+
+void sbi_timer_set_delta_upper(ulong delta_upper)
+{
+	u64 *time_delta = sbi_scratch_offset_ptr(sbi_scratch_thishart_ptr(),
+						 time_delta_off);
+
+	*time_delta &= 0xffffffffULL;
+	*time_delta |= ((u64)delta_upper << 32);
+}
+
+void sbi_timer_event_start(u64 next_event)
+{
+	if (timer_dev && timer_dev->timer_event_start)
+		timer_dev->timer_event_start(next_event);
+	csr_clear(CSR_MIP, MIP_STIP);
+	csr_set(CSR_MIE, MIP_MTIP);
+}
+
+void sbi_timer_process(void)
+{
+	csr_clear(CSR_MIE, MIP_MTIP);
+	csr_set(CSR_MIP, MIP_STIP);
+}
+
+const struct sbi_timer_device *sbi_timer_get_device(void)
+{
+	return timer_dev;
+}
+
+void sbi_timer_set_device(const struct sbi_timer_device *dev)
+{
+	if (!dev || timer_dev)
+		return;
+
+	timer_dev = dev;
+	if (!get_time_val && timer_dev->timer_value)
+		get_time_val = get_platform_ticks;
+}
+
+int sbi_timer_init(struct sbi_scratch *scratch, bool cold_boot)
+{
+	u64 *time_delta;
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+
+	if (cold_boot) {
+		time_delta_off = sbi_scratch_alloc_offset(sizeof(*time_delta),
+							  "TIME_DELTA");
+		if (!time_delta_off)
+			return SBI_ENOMEM;
+
+		if (sbi_hart_has_feature(scratch, SBI_HART_HAS_TIME))
+			get_time_val = get_ticks;
+	} else {
+		if (!time_delta_off)
+			return SBI_ENOMEM;
+	}
+
+	time_delta = sbi_scratch_offset_ptr(scratch, time_delta_off);
+	*time_delta = 0;
+
+	return sbi_platform_timer_init(plat, cold_boot);
+}
+
+void sbi_timer_exit(struct sbi_scratch *scratch)
+{
+	if (timer_dev && timer_dev->timer_event_stop)
+		timer_dev->timer_event_stop();
+
+	csr_clear(CSR_MIP, MIP_STIP);
+	csr_clear(CSR_MIE, MIP_MTIP);
+
+	sbi_platform_timer_exit(sbi_platform_ptr(scratch));
+}

lib/sbi/sbi_tlb.c

@@ -0,0 +1,429 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Atish Patra <atish.patra@wdc.com>
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_atomic.h>
+#include <sbi/riscv_barrier.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_fifo.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_ipi.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi/sbi_tlb.h>
+#include <sbi/sbi_hfence.h>
+#include <sbi/sbi_string.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_platform.h>
+
+static unsigned long tlb_sync_off;
+static unsigned long tlb_fifo_off;
+static unsigned long tlb_fifo_mem_off;
+static unsigned long tlb_range_flush_limit;
+
+static void sbi_tlb_flush_all(void)
+{
+	__asm__ __volatile("sfence.vma");
+}
+
+void sbi_tlb_local_hfence_vvma(struct sbi_tlb_info *tinfo)
+{
+	unsigned long start = tinfo->start;
+	unsigned long size  = tinfo->size;
+	unsigned long vmid  = tinfo->vmid;
+	unsigned long i, hgatp;
+
+	hgatp = csr_swap(CSR_HGATP,
+			 (vmid << HGATP_VMID_SHIFT) & HGATP_VMID_MASK);
+
+	if ((start == 0 && size == 0) || (size == SBI_TLB_FLUSH_ALL)) {
+		__sbi_hfence_vvma_all();
+		goto done;
+	}
+
+	for (i = 0; i < size; i += PAGE_SIZE) {
+		__sbi_hfence_vvma_va(start+i);
+	}
+
+done:
+	csr_write(CSR_HGATP, hgatp);
+}
+
+void sbi_tlb_local_hfence_gvma(struct sbi_tlb_info *tinfo)
+{
+	unsigned long start = tinfo->start;
+	unsigned long size  = tinfo->size;
+	unsigned long i;
+
+	if ((start == 0 && size == 0) || (size == SBI_TLB_FLUSH_ALL)) {
+		__sbi_hfence_gvma_all();
+		return;
+	}
+
+	for (i = 0; i < size; i += PAGE_SIZE) {
+		__sbi_hfence_gvma_gpa(start+i);
+	}
+}
+
+void sbi_tlb_local_sfence_vma(struct sbi_tlb_info *tinfo)
+{
+	unsigned long start = tinfo->start;
+	unsigned long size  = tinfo->size;
+	unsigned long i;
+
+	if ((start == 0 && size == 0) || (size == SBI_TLB_FLUSH_ALL)) {
+		sbi_tlb_flush_all();
+		return;
+	}
+
+	for (i = 0; i < size; i += PAGE_SIZE) {
+		__asm__ __volatile__("sfence.vma %0"
+				     :
+				     : "r"(start + i)
+				     : "memory");
+	}
+}
+
+void sbi_tlb_local_hfence_vvma_asid(struct sbi_tlb_info *tinfo)
+{
+	unsigned long start = tinfo->start;
+	unsigned long size  = tinfo->size;
+	unsigned long asid  = tinfo->asid;
+	unsigned long vmid  = tinfo->vmid;
+	unsigned long i, hgatp;
+
+	hgatp = csr_swap(CSR_HGATP,
+			 (vmid << HGATP_VMID_SHIFT) & HGATP_VMID_MASK);
+
+	if (start == 0 && size == 0) {
+		__sbi_hfence_vvma_all();
+		goto done;
+	}
+
+	if (size == SBI_TLB_FLUSH_ALL) {
+		__sbi_hfence_vvma_asid(asid);
+		goto done;
+	}
+
+	for (i = 0; i < size; i += PAGE_SIZE) {
+		__sbi_hfence_vvma_asid_va(start + i, asid);
+	}
+
+done:
+	csr_write(CSR_HGATP, hgatp);
+}
+
+void sbi_tlb_local_hfence_gvma_vmid(struct sbi_tlb_info *tinfo)
+{
+	unsigned long start = tinfo->start;
+	unsigned long size  = tinfo->size;
+	unsigned long vmid  = tinfo->vmid;
+	unsigned long i;
+
+	if (start == 0 && size == 0) {
+		__sbi_hfence_gvma_all();
+		return;
+	}
+
+	if (size == SBI_TLB_FLUSH_ALL) {
+		__sbi_hfence_gvma_vmid(vmid);
+		return;
+	}
+
+	for (i = 0; i < size; i += PAGE_SIZE) {
+		__sbi_hfence_gvma_vmid_gpa(start + i, vmid);
+	}
+}
+
+void sbi_tlb_local_sfence_vma_asid(struct sbi_tlb_info *tinfo)
+{
+	unsigned long start = tinfo->start;
+	unsigned long size  = tinfo->size;
+	unsigned long asid  = tinfo->asid;
+	unsigned long i;
+
+	if (start == 0 && size == 0) {
+		sbi_tlb_flush_all();
+		return;
+	}
+
+	/* Flush entire MM context for a given ASID */
+	if (size == SBI_TLB_FLUSH_ALL) {
+		__asm__ __volatile__("sfence.vma x0, %0"
+				     :
+				     : "r"(asid)
+				     : "memory");
+		return;
+	}
+
+	for (i = 0; i < size; i += PAGE_SIZE) {
+		__asm__ __volatile__("sfence.vma %0, %1"
+				     :
+				     : "r"(start + i), "r"(asid)
+				     : "memory");
+	}
+}
+
+void sbi_tlb_local_fence_i(struct sbi_tlb_info *tinfo)
+{
+	__asm__ __volatile("fence.i");
+}
+
+static void sbi_tlb_entry_process(struct sbi_tlb_info *tinfo)
+{
+	u32 rhartid;
+	struct sbi_scratch *rscratch = NULL;
+	unsigned long *rtlb_sync = NULL;
+
+	tinfo->local_fn(tinfo);
+
+	sbi_hartmask_for_each_hart(rhartid, &tinfo->smask) {
+		rscratch = sbi_hartid_to_scratch(rhartid);
+		if (!rscratch)
+			continue;
+
+		rtlb_sync = sbi_scratch_offset_ptr(rscratch, tlb_sync_off);
+		while (atomic_raw_xchg_ulong(rtlb_sync, 1)) ;
+	}
+}
+
+static void sbi_tlb_process_count(struct sbi_scratch *scratch, int count)
+{
+	struct sbi_tlb_info tinfo;
+	u32 deq_count = 0;
+	struct sbi_fifo *tlb_fifo =
+			sbi_scratch_offset_ptr(scratch, tlb_fifo_off);
+
+	while (!sbi_fifo_dequeue(tlb_fifo, &tinfo)) {
+		sbi_tlb_entry_process(&tinfo);
+		deq_count++;
+		if (deq_count > count)
+			break;
+
+	}
+}
+
+static void sbi_tlb_process(struct sbi_scratch *scratch)
+{
+	struct sbi_tlb_info tinfo;
+	struct sbi_fifo *tlb_fifo =
+			sbi_scratch_offset_ptr(scratch, tlb_fifo_off);
+
+	while (!sbi_fifo_dequeue(tlb_fifo, &tinfo))
+		sbi_tlb_entry_process(&tinfo);
+}
+
+static void sbi_tlb_sync(struct sbi_scratch *scratch)
+{
+	unsigned long *tlb_sync =
+			sbi_scratch_offset_ptr(scratch, tlb_sync_off);
+
+	while (!atomic_raw_xchg_ulong(tlb_sync, 0)) {
+		/*
+		 * While we are waiting for remote hart to set the sync,
+		 * consume fifo requests to avoid deadlock.
+		 */
+		sbi_tlb_process_count(scratch, 1);
+	}
+
+	return;
+}
+
+static inline int __sbi_tlb_range_check(struct sbi_tlb_info *curr,
+					struct sbi_tlb_info *next)
+{
+	unsigned long curr_end;
+	unsigned long next_end;
+	int ret = SBI_FIFO_UNCHANGED;
+
+	if (!curr || !next)
+		return ret;
+
+	next_end = next->start + next->size;
+	curr_end = curr->start + curr->size;
+	if (next->start <= curr->start && next_end > curr_end) {
+		curr->start = next->start;
+		curr->size  = next->size;
+		sbi_hartmask_or(&curr->smask, &curr->smask, &next->smask);
+		ret = SBI_FIFO_UPDATED;
+	} else if (next->start >= curr->start && next_end <= curr_end) {
+		sbi_hartmask_or(&curr->smask, &curr->smask, &next->smask);
+		ret = SBI_FIFO_SKIP;
+	}
+
+	return ret;
+}
+
+/**
+ * Call back to decide if an inplace fifo update is required or next entry can
+ * can be skipped. Here are the different cases that are being handled.
+ *
+ * Case1:
+ *	if next flush request range lies within one of the existing entry, skip
+ *	the next entry.
+ * Case2:
+ *	if flush request range in current fifo entry lies within next flush
+ *	request, update the current entry.
+ *
+ * Note:
+ *	We can not issue a fifo reset anymore if a complete vma flush is requested.
+ *	This is because we are queueing FENCE.I requests as well now.
+ *	To ease up the pressure in enqueue/fifo sync path, try to dequeue 1 element
+ *	before continuing the while loop. This method is preferred over wfi/ipi because
+ *	of MMIO cost involved in later method.
+ */
+static int sbi_tlb_update_cb(void *in, void *data)
+{
+	struct sbi_tlb_info *curr;
+	struct sbi_tlb_info *next;
+	int ret = SBI_FIFO_UNCHANGED;
+
+	if (!in || !data)
+		return ret;
+
+	curr = (struct sbi_tlb_info *)data;
+	next = (struct sbi_tlb_info *)in;
+
+	if (next->local_fn == sbi_tlb_local_sfence_vma_asid &&
+	    curr->local_fn == sbi_tlb_local_sfence_vma_asid) {
+		if (next->asid == curr->asid)
+			ret = __sbi_tlb_range_check(curr, next);
+	} else if (next->local_fn == sbi_tlb_local_sfence_vma &&
+		   curr->local_fn == sbi_tlb_local_sfence_vma) {
+		ret = __sbi_tlb_range_check(curr, next);
+	}
+
+	return ret;
+}
+
+static int sbi_tlb_update(struct sbi_scratch *scratch,
+			  struct sbi_scratch *remote_scratch,
+			  u32 remote_hartid, void *data)
+{
+	int ret;
+	struct sbi_fifo *tlb_fifo_r;
+	struct sbi_tlb_info *tinfo = data;
+	u32 curr_hartid = current_hartid();
+
+	/*
+	 * If address range to flush is too big then simply
+	 * upgrade it to flush all because we can only flush
+	 * 4KB at a time.
+	 */
+	if (tinfo->size > tlb_range_flush_limit) {
+		tinfo->start = 0;
+		tinfo->size = SBI_TLB_FLUSH_ALL;
+	}
+
+	/*
+	 * If the request is to queue a tlb flush entry for itself
+	 * then just do a local flush and return;
+	 */
+	if (remote_hartid == curr_hartid) {
+		tinfo->local_fn(tinfo);
+		return -1;
+	}
+
+	tlb_fifo_r = sbi_scratch_offset_ptr(remote_scratch, tlb_fifo_off);
+
+	ret = sbi_fifo_inplace_update(tlb_fifo_r, data, sbi_tlb_update_cb);
+	if (ret != SBI_FIFO_UNCHANGED) {
+		return 1;
+	}
+
+	while (sbi_fifo_enqueue(tlb_fifo_r, data) < 0) {
+		/**
+		 * For now, Busy loop until there is space in the fifo.
+		 * There may be case where target hart is also
+		 * enqueue in source hart's fifo. Both hart may busy
+		 * loop leading to a deadlock.
+		 * TODO: Introduce a wait/wakeup event mechanism to handle
+		 * this properly.
+		 */
+		sbi_tlb_process_count(scratch, 1);
+		sbi_dprintf("hart%d: hart%d tlb fifo full\n",
+			    curr_hartid, remote_hartid);
+	}
+
+	return 0;
+}
+
+static struct sbi_ipi_event_ops tlb_ops = {
+	.name = "IPI_TLB",
+	.update = sbi_tlb_update,
+	.sync = sbi_tlb_sync,
+	.process = sbi_tlb_process,
+};
+
+static u32 tlb_event = SBI_IPI_EVENT_MAX;
+
+int sbi_tlb_request(ulong hmask, ulong hbase, struct sbi_tlb_info *tinfo)
+{
+	if (!tinfo->local_fn)
+		return SBI_EINVAL;
+
+	return sbi_ipi_send_many(hmask, hbase, tlb_event, tinfo);
+}
+
+int sbi_tlb_init(struct sbi_scratch *scratch, bool cold_boot)
+{
+	int ret;
+	void *tlb_mem;
+	unsigned long *tlb_sync;
+	struct sbi_fifo *tlb_q;
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+
+	if (cold_boot) {
+		tlb_sync_off = sbi_scratch_alloc_offset(sizeof(*tlb_sync),
+							"IPI_TLB_SYNC");
+		if (!tlb_sync_off)
+			return SBI_ENOMEM;
+		tlb_fifo_off = sbi_scratch_alloc_offset(sizeof(*tlb_q),
+							"IPI_TLB_FIFO");
+		if (!tlb_fifo_off) {
+			sbi_scratch_free_offset(tlb_sync_off);
+			return SBI_ENOMEM;
+		}
+		tlb_fifo_mem_off = sbi_scratch_alloc_offset(
+				SBI_TLB_FIFO_NUM_ENTRIES * SBI_TLB_INFO_SIZE,
+				"IPI_TLB_FIFO_MEM");
+		if (!tlb_fifo_mem_off) {
+			sbi_scratch_free_offset(tlb_fifo_off);
+			sbi_scratch_free_offset(tlb_sync_off);
+			return SBI_ENOMEM;
+		}
+		ret = sbi_ipi_event_create(&tlb_ops);
+		if (ret < 0) {
+			sbi_scratch_free_offset(tlb_fifo_mem_off);
+			sbi_scratch_free_offset(tlb_fifo_off);
+			sbi_scratch_free_offset(tlb_sync_off);
+			return ret;
+		}
+		tlb_event = ret;
+		tlb_range_flush_limit = sbi_platform_tlbr_flush_limit(plat);
+	} else {
+		if (!tlb_sync_off ||
+		    !tlb_fifo_off ||
+		    !tlb_fifo_mem_off)
+			return SBI_ENOMEM;
+		if (SBI_IPI_EVENT_MAX <= tlb_event)
+			return SBI_ENOSPC;
+	}
+
+	tlb_sync = sbi_scratch_offset_ptr(scratch, tlb_sync_off);
+	tlb_q = sbi_scratch_offset_ptr(scratch, tlb_fifo_off);
+	tlb_mem = sbi_scratch_offset_ptr(scratch, tlb_fifo_mem_off);
+
+	*tlb_sync = 0;
+
+	sbi_fifo_init(tlb_q, tlb_mem,
+		      SBI_TLB_FIFO_NUM_ENTRIES, SBI_TLB_INFO_SIZE);
+
+	return 0;
+}

lib/sbi/sbi_trap.c

@@ -0,0 +1,294 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_ecall.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_illegal_insn.h>
+#include <sbi/sbi_ipi.h>
+#include <sbi/sbi_misaligned_ldst.h>
+#include <sbi/sbi_scratch.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)
+{
+	u32 hartid = current_hartid();
+
+	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);
+	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: 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_hart_hang();
+}
+
+/**
+ * Redirect trap to lower privledge mode (S-mode or U-mode)
+ *
+ * @param regs pointer to register state
+ * @param trap pointer to trap details
+ *
+ * @return 0 on success and negative error code on failure
+ */
+int sbi_trap_redirect(struct sbi_trap_regs *regs,
+		      struct sbi_trap_info *trap)
+{
+	ulong hstatus, vsstatus, prev_mode;
+#if __riscv_xlen == 32
+	bool prev_virt = (regs->mstatusH & MSTATUSH_MPV) ? TRUE : FALSE;
+#else
+	bool prev_virt = (regs->mstatus & MSTATUS_MPV) ? TRUE : FALSE;
+#endif
+	/* By default, we redirect to HS-mode */
+	bool next_virt = FALSE;
+
+	/* Sanity check on previous mode */
+	prev_mode = (regs->mstatus & MSTATUS_MPP) >> MSTATUS_MPP_SHIFT;
+	if (prev_mode != PRV_S && prev_mode != PRV_U)
+		return SBI_ENOTSUPP;
+
+	/* For certain exceptions from VS/VU-mode we redirect to VS-mode */
+	if (misa_extension('H') && prev_virt) {
+		switch (trap->cause) {
+		case CAUSE_FETCH_PAGE_FAULT:
+		case CAUSE_LOAD_PAGE_FAULT:
+		case CAUSE_STORE_PAGE_FAULT:
+			next_virt = TRUE;
+			break;
+		default:
+			break;
+		};
+	}
+
+	/* Update MSTATUS MPV bits */
+#if __riscv_xlen == 32
+	regs->mstatusH &= ~MSTATUSH_MPV;
+	regs->mstatusH |= (next_virt) ? MSTATUSH_MPV : 0UL;
+#else
+	regs->mstatus &= ~MSTATUS_MPV;
+	regs->mstatus |= (next_virt) ? MSTATUS_MPV : 0UL;
+#endif
+
+	/* Update HSTATUS for VS/VU-mode to HS-mode transition */
+	if (misa_extension('H') && prev_virt && !next_virt) {
+		/* Update HSTATUS SPVP and SPV bits */
+		hstatus = csr_read(CSR_HSTATUS);
+		hstatus &= ~HSTATUS_SPVP;
+		hstatus |= (prev_mode == PRV_S) ? HSTATUS_SPVP : 0;
+		hstatus &= ~HSTATUS_SPV;
+		hstatus |= (prev_virt) ? HSTATUS_SPV : 0;
+		csr_write(CSR_HSTATUS, hstatus);
+		csr_write(CSR_HTVAL, trap->tval2);
+		csr_write(CSR_HTINST, trap->tinst);
+	}
+
+	/* Update exception related CSRs */
+	if (next_virt) {
+		/* Update VS-mode exception info */
+		csr_write(CSR_VSTVAL, trap->tval);
+		csr_write(CSR_VSEPC, trap->epc);
+		csr_write(CSR_VSCAUSE, trap->cause);
+
+		/* Set MEPC to VS-mode exception vector base */
+		regs->mepc = csr_read(CSR_VSTVEC);
+
+		/* Set MPP to VS-mode */
+		regs->mstatus &= ~MSTATUS_MPP;
+		regs->mstatus |= (PRV_S << MSTATUS_MPP_SHIFT);
+
+		/* Get VS-mode SSTATUS CSR */
+		vsstatus = csr_read(CSR_VSSTATUS);
+
+		/* Set SPP for VS-mode */
+		vsstatus &= ~SSTATUS_SPP;
+		if (prev_mode == PRV_S)
+			vsstatus |= (1UL << SSTATUS_SPP_SHIFT);
+
+		/* Set SPIE for VS-mode */
+		vsstatus &= ~SSTATUS_SPIE;
+		if (vsstatus & SSTATUS_SIE)
+			vsstatus |= (1UL << SSTATUS_SPIE_SHIFT);
+
+		/* Clear SIE for VS-mode */
+		vsstatus &= ~SSTATUS_SIE;
+
+		/* Update VS-mode SSTATUS CSR */
+		csr_write(CSR_VSSTATUS, vsstatus);
+	} else {
+		/* Update S-mode exception info */
+		csr_write(CSR_STVAL, trap->tval);
+		csr_write(CSR_SEPC, trap->epc);
+		csr_write(CSR_SCAUSE, trap->cause);
+
+		/* Set MEPC to S-mode exception vector base */
+		regs->mepc = csr_read(CSR_STVEC);
+
+		/* Set MPP to S-mode */
+		regs->mstatus &= ~MSTATUS_MPP;
+		regs->mstatus |= (PRV_S << MSTATUS_MPP_SHIFT);
+
+		/* Set SPP for S-mode */
+		regs->mstatus &= ~MSTATUS_SPP;
+		if (prev_mode == PRV_S)
+			regs->mstatus |= (1UL << MSTATUS_SPP_SHIFT);
+
+		/* Set SPIE for S-mode */
+		regs->mstatus &= ~MSTATUS_SPIE;
+		if (regs->mstatus & MSTATUS_SIE)
+			regs->mstatus |= (1UL << MSTATUS_SPIE_SHIFT);
+
+		/* Clear SIE for S-mode */
+		regs->mstatus &= ~MSTATUS_SIE;
+	}
+
+	return 0;
+}
+
+/**
+ * Handle trap/interrupt
+ *
+ * This function is called by firmware linked to OpenSBI
+ * library for handling trap/interrupt. It expects the
+ * following:
+ * 1. The 'mscratch' CSR is pointing to sbi_scratch of current HART
+ * 2. The 'mcause' CSR is having exception/interrupt cause
+ * 3. The 'mtval' CSR is having additional trap information
+ * 4. The 'mtval2' CSR is having additional trap information
+ * 5. The 'mtinst' CSR is having decoded trap instruction
+ * 6. Stack pointer (SP) is setup for current HART
+ * 7. Interrupts are disabled in MSTATUS CSR
+ *
+ * @param regs pointer to register state
+ */
+struct sbi_trap_regs *sbi_trap_handler(struct sbi_trap_regs *regs)
+{
+	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;
+
+	if (misa_extension('H')) {
+		mtval2 = csr_read(CSR_MTVAL2);
+		mtinst = csr_read(CSR_MTINST);
+	}
+
+	if (mcause & (1UL << (__riscv_xlen - 1))) {
+		mcause &= ~(1UL << (__riscv_xlen - 1));
+		switch (mcause) {
+		case IRQ_M_TIMER:
+			sbi_timer_process();
+			break;
+		case IRQ_M_SOFT:
+			sbi_ipi_process();
+			break;
+		default:
+			msg = "unhandled external interrupt";
+			goto trap_error;
+		};
+		return regs;
+	}
+
+	switch (mcause) {
+	case CAUSE_ILLEGAL_INSTRUCTION:
+		rc  = sbi_illegal_insn_handler(mtval, regs);
+		msg = "illegal instruction handler failed";
+		break;
+	case CAUSE_MISALIGNED_LOAD:
+		rc = sbi_misaligned_load_handler(mtval, mtval2, mtinst, regs);
+		msg = "misaligned load handler failed";
+		break;
+	case CAUSE_MISALIGNED_STORE:
+		rc  = sbi_misaligned_store_handler(mtval, mtval2, mtinst, regs);
+		msg = "misaligned store handler failed";
+		break;
+	case CAUSE_SUPERVISOR_ECALL:
+	case CAUSE_MACHINE_ECALL:
+		rc  = sbi_ecall_handler(regs);
+		msg = "ecall 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;
+		rc = sbi_trap_redirect(regs, &trap);
+		break;
+	};
+
+trap_error:
+	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();
+}

lib/sbi/sbi_unpriv.c

@@ -0,0 +1,165 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_encoding.h>
+#include <sbi/sbi_bitops.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi/sbi_trap.h>
+#include <sbi/sbi_unpriv.h>
+
+/**
+ * a3 must a pointer to the sbi_trap_info and a4 is used as a temporary
+ * register in the trap handler. Make sure that compiler doesn't use a3 & a4.
+ */
+#define DEFINE_UNPRIVILEGED_LOAD_FUNCTION(type, insn)                         \
+	type sbi_load_##type(const type *addr,                                \
+			     struct sbi_trap_info *trap)                      \
+	{                                                                     \
+		register ulong tinfo asm("a3");                               \
+		register ulong mstatus = 0;                                   \
+		register ulong mtvec = sbi_hart_expected_trap_addr();         \
+		type ret = 0;                                                 \
+		trap->cause = 0;                                              \
+		asm volatile(                                                 \
+			"add %[tinfo], %[taddr], zero\n"                      \
+			"csrrw %[mtvec], " STR(CSR_MTVEC) ", %[mtvec]\n"      \
+			"csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n"   \
+			".option push\n"                                      \
+			".option norvc\n"                                     \
+			#insn " %[ret], %[addr]\n"                            \
+			".option pop\n"                                       \
+			"csrw " STR(CSR_MSTATUS) ", %[mstatus]\n"             \
+			"csrw " STR(CSR_MTVEC) ", %[mtvec]"                   \
+		    : [mstatus] "+&r"(mstatus), [mtvec] "+&r"(mtvec),         \
+		      [tinfo] "+&r"(tinfo), [ret] "=&r"(ret)                  \
+		    : [addr] "m"(*addr), [mprv] "r"(MSTATUS_MPRV),            \
+		      [taddr] "r"((ulong)trap)                                \
+		    : "a4", "memory");                                        \
+		return ret;                                                   \
+	}
+
+#define DEFINE_UNPRIVILEGED_STORE_FUNCTION(type, insn)                        \
+	void sbi_store_##type(type *addr, type val,                           \
+			      struct sbi_trap_info *trap)                     \
+	{                                                                     \
+		register ulong tinfo asm("a3") = (ulong)trap;                 \
+		register ulong mstatus = 0;                                   \
+		register ulong mtvec = sbi_hart_expected_trap_addr();         \
+		trap->cause = 0;                                              \
+		asm volatile(                                                 \
+			"add %[tinfo], %[taddr], zero\n"                      \
+			"csrrw %[mtvec], " STR(CSR_MTVEC) ", %[mtvec]\n"      \
+			"csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n"   \
+			".option push\n"                                      \
+			".option norvc\n"                                     \
+			#insn " %[val], %[addr]\n"                            \
+			".option pop\n"                                       \
+			"csrw " STR(CSR_MSTATUS) ", %[mstatus]\n"             \
+			"csrw " STR(CSR_MTVEC) ", %[mtvec]"                   \
+		    : [mstatus] "+&r"(mstatus), [mtvec] "+&r"(mtvec),         \
+		      [tinfo] "+&r"(tinfo)                                    \
+		    : [addr] "m"(*addr), [mprv] "r"(MSTATUS_MPRV),            \
+		      [val] "r"(val), [taddr] "r"((ulong)trap)                \
+		    : "a4", "memory");                                        \
+	}
+
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u8, lbu)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u16, lhu)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(s8, lb)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(s16, lh)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(s32, lw)
+DEFINE_UNPRIVILEGED_STORE_FUNCTION(u8, sb)
+DEFINE_UNPRIVILEGED_STORE_FUNCTION(u16, sh)
+DEFINE_UNPRIVILEGED_STORE_FUNCTION(u32, sw)
+#if __riscv_xlen == 64
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u32, lwu)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u64, ld)
+DEFINE_UNPRIVILEGED_STORE_FUNCTION(u64, sd)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(ulong, ld)
+#else
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(u32, lw)
+DEFINE_UNPRIVILEGED_LOAD_FUNCTION(ulong, lw)
+
+u64 sbi_load_u64(const u64 *addr,
+		 struct sbi_trap_info *trap)
+{
+	u64 ret = sbi_load_u32((u32 *)addr, trap);
+
+	if (trap->cause)
+		return 0;
+	ret |= ((u64)sbi_load_u32((u32 *)addr + 1, trap) << 32);
+	if (trap->cause)
+		return 0;
+
+	return ret;
+}
+
+void sbi_store_u64(u64 *addr, u64 val,
+		   struct sbi_trap_info *trap)
+{
+	sbi_store_u32((u32 *)addr, val, trap);
+	if (trap->cause)
+		return;
+
+	sbi_store_u32((u32 *)addr + 1, val >> 32, trap);
+	if (trap->cause)
+		return;
+}
+#endif
+
+ulong sbi_get_insn(ulong mepc, struct sbi_trap_info *trap)
+{
+	register ulong tinfo asm("a3");
+	register ulong ttmp asm("a4");
+	register ulong mstatus = 0;
+	register ulong mtvec = sbi_hart_expected_trap_addr();
+	ulong insn = 0;
+
+	trap->cause = 0;
+
+	asm volatile(
+	    "add %[tinfo], %[taddr], zero\n"
+	    "csrrw %[mtvec], " STR(CSR_MTVEC) ", %[mtvec]\n"
+	    "csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n"
+	    "lhu %[insn], (%[addr])\n"
+	    "andi %[ttmp], %[insn], 3\n"
+	    "addi %[ttmp], %[ttmp], -3\n"
+	    "bne %[ttmp], zero, 2f\n"
+	    "lhu %[ttmp], 2(%[addr])\n"
+	    "sll %[ttmp], %[ttmp], 16\n"
+	    "add %[insn], %[insn], %[ttmp]\n"
+	    "2: csrw " STR(CSR_MSTATUS) ", %[mstatus]\n"
+	    "csrw " STR(CSR_MTVEC) ", %[mtvec]"
+	    : [mstatus] "+&r"(mstatus), [mtvec] "+&r"(mtvec),
+	      [tinfo] "+&r"(tinfo), [ttmp] "+&r"(ttmp),
+	      [insn] "=&r"(insn)
+	    : [mprv] "r"(MSTATUS_MPRV | MSTATUS_MXR),
+	      [taddr] "r"((ulong)trap), [addr] "r"(mepc)
+	    : "memory");
+
+	switch (trap->cause) {
+	case CAUSE_LOAD_ACCESS:
+		trap->cause = CAUSE_FETCH_ACCESS;
+		trap->tval = mepc;
+		break;
+	case CAUSE_LOAD_PAGE_FAULT:
+		trap->cause = CAUSE_FETCH_PAGE_FAULT;
+		trap->tval = mepc;
+		break;
+	case CAUSE_LOAD_GUEST_PAGE_FAULT:
+		trap->cause = CAUSE_FETCH_GUEST_PAGE_FAULT;
+		trap->tval = mepc;
+		break;
+	default:
+		break;
+	};
+
+	return insn;
+}

lib/utils/fdt/fdt_domain.c

@@ -0,0 +1,473 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * fdt_domain.c - Flat Device Tree Domain helper routines
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <libfdt.h>
+#include <sbi/sbi_domain.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_hartmask.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi_utils/fdt/fdt_domain.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+
+int fdt_iterate_each_domain(void *fdt, void *opaque,
+			    int (*fn)(void *fdt, int domain_offset,
+				       void *opaque))
+{
+	int rc, doffset, poffset;
+
+	if (!fdt || !fn)
+		return SBI_EINVAL;
+
+	poffset = fdt_path_offset(fdt, "/chosen");
+	if (poffset < 0)
+		return 0;
+	poffset = fdt_node_offset_by_compatible(fdt, poffset,
+						"opensbi,domain,config");
+	if (poffset < 0)
+		return 0;
+
+	fdt_for_each_subnode(doffset, fdt, poffset) {
+		if (fdt_node_check_compatible(fdt, doffset,
+					      "opensbi,domain,instance"))
+			continue;
+
+		rc = fn(fdt, doffset, opaque);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
+}
+
+int fdt_iterate_each_memregion(void *fdt, int domain_offset, void *opaque,
+			       int (*fn)(void *fdt, int domain_offset,
+					 int region_offset, u32 region_access,
+					 void *opaque))
+{
+	u32 i, rcount;
+	int rc, len, region_offset;
+	const u32 *regions;
+
+	if (!fdt || (domain_offset < 0) || !fn)
+		return SBI_EINVAL;
+
+	if (fdt_node_check_compatible(fdt, domain_offset,
+				      "opensbi,domain,instance"))
+		return SBI_EINVAL;
+
+	regions = fdt_getprop(fdt, domain_offset, "regions", &len);
+	if (!regions)
+		return 0;
+
+	rcount = (u32)len / (sizeof(u32) * 2);
+	for (i = 0; i < rcount; i++) {
+		region_offset = fdt_node_offset_by_phandle(fdt,
+						fdt32_to_cpu(regions[2 * i]));
+		if (region_offset < 0)
+			return region_offset;
+
+		if (fdt_node_check_compatible(fdt, region_offset,
+					      "opensbi,domain,memregion"))
+			return SBI_EINVAL;
+
+		rc = fn(fdt, domain_offset, region_offset,
+			fdt32_to_cpu(regions[(2 * i) + 1]), opaque);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
+}
+
+struct __fixup_find_domain_offset_info {
+	const char *name;
+	int *doffset;
+};
+
+static int __fixup_find_domain_offset(void *fdt, int doff, void *p)
+{
+	struct __fixup_find_domain_offset_info *fdo = p;
+
+	if (!sbi_strcmp(fdo->name, fdt_get_name(fdt, doff, NULL)))
+		*fdo->doffset = doff;
+
+	return 0;
+}
+
+#define DISABLE_DEVICES_MASK	(SBI_DOMAIN_MEMREGION_READABLE | \
+				 SBI_DOMAIN_MEMREGION_WRITEABLE | \
+				 SBI_DOMAIN_MEMREGION_EXECUTABLE)
+
+static int __fixup_count_disable_devices(void *fdt, int doff, int roff,
+					 u32 perm, void *p)
+{
+	int len;
+	u32 *dcount = p;
+
+	if (perm & DISABLE_DEVICES_MASK)
+		return 0;
+
+	len = 0;
+	if (fdt_getprop(fdt, roff, "devices", &len))
+		*dcount += len / sizeof(u32);
+
+	return 0;
+}
+
+static int __fixup_disable_devices(void *fdt, int doff, int roff,
+				   u32 raccess, void *p)
+{
+	int i, len, coff;
+	const u32 *devices;
+
+	if (raccess & DISABLE_DEVICES_MASK)
+		return 0;
+
+	len = 0;
+	devices = fdt_getprop(fdt, roff, "devices", &len);
+	if (!devices)
+		return 0;
+	len = len / sizeof(u32);
+
+	for (i = 0; i < len; i++) {
+		coff = fdt_node_offset_by_phandle(fdt,
+					fdt32_to_cpu(devices[i]));
+		if (coff < 0)
+			return coff;
+
+		fdt_setprop_string(fdt, coff, "status", "disabled");
+	}
+
+	return 0;
+}
+
+void fdt_domain_fixup(void *fdt)
+{
+	u32 i, dcount;
+	int err, poffset, doffset;
+	struct sbi_domain *dom = sbi_domain_thishart_ptr();
+	struct __fixup_find_domain_offset_info fdo;
+
+	/* Remove the domain assignment DT property from CPU DT nodes */
+	poffset = fdt_path_offset(fdt, "/cpus");
+	if (poffset < 0)
+		return;
+	fdt_for_each_subnode(doffset, fdt, poffset) {
+		err = fdt_parse_hart_id(fdt, doffset, &i);
+		if (err)
+			continue;
+
+		fdt_nop_property(fdt, doffset, "opensbi-domain");
+	}
+
+	/* Skip device disable for root domain */
+	if (!dom->index)
+		goto skip_device_disable;
+
+	/* Find current domain DT node */
+	doffset = -1;
+	fdo.name = dom->name;
+	fdo.doffset = &doffset;
+	fdt_iterate_each_domain(fdt, &fdo, __fixup_find_domain_offset);
+	if (doffset < 0)
+		goto skip_device_disable;
+
+	/* Count current domain device DT nodes to be disabled */
+	dcount = 0;
+	fdt_iterate_each_memregion(fdt, doffset, &dcount,
+				   __fixup_count_disable_devices);
+	if (!dcount)
+		goto skip_device_disable;
+
+	/* Expand FDT based on device DT nodes to be disabled */
+	err = fdt_open_into(fdt, fdt, fdt_totalsize(fdt) + dcount * 32);
+	if (err < 0)
+		return;
+
+	/* Again find current domain DT node */
+	doffset = -1;
+	fdo.name = dom->name;
+	fdo.doffset = &doffset;
+	fdt_iterate_each_domain(fdt, &fdo, __fixup_find_domain_offset);
+	if (doffset < 0)
+		goto skip_device_disable;
+
+	/* Disable device DT nodes for current domain */
+	fdt_iterate_each_memregion(fdt, doffset, NULL,
+				   __fixup_disable_devices);
+skip_device_disable:
+
+	/* Remove the OpenSBI domain config DT node */
+	poffset = fdt_path_offset(fdt, "/chosen");
+	if (poffset < 0)
+		return;
+	poffset = fdt_node_offset_by_compatible(fdt, poffset,
+						"opensbi,domain,config");
+	if (poffset < 0)
+		return;
+	fdt_nop_node(fdt, poffset);
+}
+
+#define FDT_DOMAIN_MAX_COUNT		8
+#define FDT_DOMAIN_REGION_MAX_COUNT	16
+
+static u32 fdt_domains_count;
+static struct sbi_domain fdt_domains[FDT_DOMAIN_MAX_COUNT];
+static struct sbi_hartmask fdt_masks[FDT_DOMAIN_MAX_COUNT];
+static struct sbi_domain_memregion
+	fdt_regions[FDT_DOMAIN_MAX_COUNT][FDT_DOMAIN_REGION_MAX_COUNT + 1];
+
+static int __fdt_parse_region(void *fdt, int domain_offset,
+			      int region_offset, u32 region_access,
+			      void *opaque)
+{
+	int len;
+	u32 val32;
+	u64 val64;
+	const u32 *val;
+	u32 *region_count = opaque;
+	struct sbi_domain_memregion *region;
+
+	/* Find next region of the domain */
+	if (FDT_DOMAIN_REGION_MAX_COUNT <= *region_count)
+		return SBI_EINVAL;
+	region = &fdt_regions[fdt_domains_count][*region_count];
+
+	/* Read "base" DT property */
+	val = fdt_getprop(fdt, region_offset, "base", &len);
+	if (!val && len >= 8)
+		return SBI_EINVAL;
+	val64 = fdt32_to_cpu(val[0]);
+	val64 = (val64 << 32) | fdt32_to_cpu(val[1]);
+	region->base = val64;
+
+	/* Read "order" DT property */
+	val = fdt_getprop(fdt, region_offset, "order", &len);
+	if (!val && len >= 4)
+		return SBI_EINVAL;
+	val32 = fdt32_to_cpu(*val);
+	if (val32 < 3 || __riscv_xlen < val32)
+		return SBI_EINVAL;
+	region->order = val32;
+
+	/* Read "mmio" DT property */
+	region->flags = region_access & SBI_DOMAIN_MEMREGION_ACCESS_MASK;
+	if (fdt_get_property(fdt, region_offset, "mmio", NULL))
+		region->flags |= SBI_DOMAIN_MEMREGION_MMIO;
+
+	(*region_count)++;
+
+	return 0;
+}
+
+static int __fdt_parse_domain(void *fdt, int domain_offset, void *opaque)
+{
+	u32 val32;
+	u64 val64;
+	const u32 *val;
+	struct sbi_domain *dom;
+	struct sbi_hartmask *mask;
+	struct sbi_hartmask assign_mask;
+	int *cold_domain_offset = opaque;
+	struct sbi_domain_memregion *reg, *regions;
+	int i, err, len, cpus_offset, cpu_offset, doffset;
+
+	/* Sanity check on maximum domains we can handle */
+	if (FDT_DOMAIN_MAX_COUNT <= fdt_domains_count)
+		return SBI_EINVAL;
+	dom = &fdt_domains[fdt_domains_count];
+	mask = &fdt_masks[fdt_domains_count];
+	regions = &fdt_regions[fdt_domains_count][0];
+
+	/* Read DT node name */
+	sbi_strncpy(dom->name, fdt_get_name(fdt, domain_offset, NULL),
+		    sizeof(dom->name));
+	dom->name[sizeof(dom->name) - 1] = '\0';
+
+	/* Setup possible HARTs mask */
+	SBI_HARTMASK_INIT(mask);
+	dom->possible_harts = mask;
+	val = fdt_getprop(fdt, domain_offset, "possible-harts", &len);
+	len = len / sizeof(u32);
+	if (val && len) {
+		for (i = 0; i < len; i++) {
+			cpu_offset = fdt_node_offset_by_phandle(fdt,
+							fdt32_to_cpu(val[i]));
+			if (cpu_offset < 0)
+				return cpu_offset;
+
+			err = fdt_parse_hart_id(fdt, cpu_offset, &val32);
+			if (err)
+				return err;
+
+			sbi_hartmask_set_hart(val32, mask);
+		}
+	}
+
+	/* Setup memregions from DT */
+	val32 = 0;
+	sbi_memset(regions, 0,
+		   sizeof(*regions) * (FDT_DOMAIN_REGION_MAX_COUNT + 1));
+	dom->regions = regions;
+	err = fdt_iterate_each_memregion(fdt, domain_offset, &val32,
+					 __fdt_parse_region);
+	if (err)
+		return err;
+
+	/*
+	 * Copy over root domain memregions which don't allow
+	 * read, write and execute from lower privilege modes.
+	 *
+	 * These root domain memregions without read, write,
+	 * and execute permissions include:
+	 * 1) firmware region protecting the firmware memory
+	 * 2) mmio regions protecting M-mode only mmio devices
+	 */
+	sbi_domain_for_each_memregion(&root, reg) {
+		if ((reg->flags & SBI_DOMAIN_MEMREGION_READABLE) ||
+		    (reg->flags & SBI_DOMAIN_MEMREGION_WRITEABLE) ||
+		    (reg->flags & SBI_DOMAIN_MEMREGION_EXECUTABLE))
+			continue;
+		if (FDT_DOMAIN_REGION_MAX_COUNT <= val32)
+			return SBI_EINVAL;
+		sbi_memcpy(&regions[val32++], reg, sizeof(*reg));
+	}
+
+	/* Read "boot-hart" DT property */
+	val32 = -1U;
+	val = fdt_getprop(fdt, domain_offset, "boot-hart", &len);
+	if (val && len >= 4) {
+		cpu_offset = fdt_node_offset_by_phandle(fdt,
+							 fdt32_to_cpu(*val));
+		if (cpu_offset >= 0)
+			fdt_parse_hart_id(fdt, cpu_offset, &val32);
+	} else {
+		if (domain_offset == *cold_domain_offset)
+			val32 = current_hartid();
+	}
+	dom->boot_hartid = val32;
+
+	/* Read "next-arg1" DT property */
+	val64 = 0;
+	val = fdt_getprop(fdt, domain_offset, "next-arg1", &len);
+	if (val && len >= 8) {
+		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;
+	}
+	dom->next_arg1 = val64;
+
+	/* Read "next-addr" DT property */
+	val64 = 0;
+	val = fdt_getprop(fdt, domain_offset, "next-addr", &len);
+	if (val && len >= 8) {
+		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_addr;
+	}
+	dom->next_addr = val64;
+
+	/* Read "next-mode" DT property */
+	val32 = 0x1;
+	val = fdt_getprop(fdt, domain_offset, "next-mode", &len);
+	if (val && len >= 4) {
+		val32 = fdt32_to_cpu(val[0]);
+		if (val32 != 0x0 && val32 != 0x1)
+			val32 = 0x1;
+	} else {
+		if (domain_offset == *cold_domain_offset)
+			val32 = sbi_scratch_thishart_ptr()->next_mode;
+	}
+	dom->next_mode = val32;
+
+	/* Read "system-reset-allowed" DT property */
+	if (fdt_get_property(fdt, domain_offset,
+			     "system-reset-allowed", NULL))
+		dom->system_reset_allowed = TRUE;
+	else
+		dom->system_reset_allowed = FALSE;
+
+	/* Find /cpus DT node */
+	cpus_offset = fdt_path_offset(fdt, "/cpus");
+	if (cpus_offset < 0)
+		return cpus_offset;
+
+	/* HART to domain assignment mask based on CPU DT nodes */
+	sbi_hartmask_clear_all(&assign_mask);
+	fdt_for_each_subnode(cpu_offset, fdt, cpus_offset) {
+		err = fdt_parse_hart_id(fdt, cpu_offset, &val32);
+		if (err)
+			continue;
+
+		if (SBI_HARTMASK_MAX_BITS <= val32)
+			continue;
+
+		val = fdt_getprop(fdt, cpu_offset, "opensbi-domain", &len);
+		if (!val || len < 4)
+			return SBI_EINVAL;
+
+		doffset = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(*val));
+		if (doffset < 0)
+			return doffset;
+
+		if (doffset == domain_offset)
+			sbi_hartmask_set_hart(val32, &assign_mask);
+	}
+
+	/* Increment domains count */
+	fdt_domains_count++;
+
+	/* Register the domain */
+	return sbi_domain_register(dom, &assign_mask);
+}
+
+int fdt_domains_populate(void *fdt)
+{
+	const u32 *val;
+	int cold_domain_offset;
+	u32 hartid, cold_hartid;
+	int err, len, cpus_offset, cpu_offset;
+
+	/* Sanity checks */
+	if (!fdt)
+		return SBI_EINVAL;
+
+	/* Find /cpus DT node */
+	cpus_offset = fdt_path_offset(fdt, "/cpus");
+	if (cpus_offset < 0)
+		return cpus_offset;
+
+	/* Find coldboot HART domain DT node offset */
+	cold_domain_offset = -1;
+	cold_hartid = current_hartid();
+	fdt_for_each_subnode(cpu_offset, fdt, cpus_offset) {
+		err = fdt_parse_hart_id(fdt, cpu_offset, &hartid);
+		if (err)
+			continue;
+
+		if (hartid != cold_hartid)
+			continue;
+
+		val = fdt_getprop(fdt, cpu_offset, "opensbi-domain", &len);
+		if (val && len >= 4)
+			cold_domain_offset = fdt_node_offset_by_phandle(fdt,
+							   fdt32_to_cpu(*val));
+
+		break;
+	}
+
+	/* Iterate over each domain in FDT and populate details */
+	return fdt_iterate_each_domain(fdt, &cold_domain_offset,
+				       __fdt_parse_domain);
+}

lib/utils/fdt/fdt_fixup.c

@@ -0,0 +1,268 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * fdt_fixup.c - Flat Device Tree parsing helper routines
+ * Implement helper routines to parse FDT nodes on top of
+ * libfdt for OpenSBI usage
+ *
+ * Copyright (C) 2020 Bin Meng <bmeng.cn@gmail.com>
+ */
+
+#include <libfdt.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_domain.h>
+#include <sbi/sbi_math.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi/sbi_string.h>
+#include <sbi_utils/fdt/fdt_fixup.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+
+void fdt_cpu_fixup(void *fdt)
+{
+	struct sbi_domain *dom = sbi_domain_thishart_ptr();
+	int err, cpu_offset, cpus_offset, len;
+	const char *mmu_type;
+	u32 hartid;
+
+	err = fdt_open_into(fdt, fdt, fdt_totalsize(fdt) + 32);
+	if (err < 0)
+		return;
+
+	cpus_offset = fdt_path_offset(fdt, "/cpus");
+	if (cpus_offset < 0)
+		return;
+
+	fdt_for_each_subnode(cpu_offset, fdt, cpus_offset) {
+		err = fdt_parse_hart_id(fdt, cpu_offset, &hartid);
+		if (err)
+			continue;
+
+		/*
+		 * Disable a HART DT node if one of the following is true:
+		 * 1. The HART is not assigned to the current domain
+		 * 2. MMU is not available for the HART
+		 */
+
+		mmu_type = fdt_getprop(fdt, cpu_offset, "mmu-type", &len);
+		if (!sbi_domain_is_assigned_hart(dom, hartid) ||
+		    !mmu_type || !len)
+			fdt_setprop_string(fdt, cpu_offset, "status",
+					   "disabled");
+	}
+}
+
+void fdt_plic_fixup(void *fdt)
+{
+	u32 *cells;
+	int i, cells_count;
+	int plic_off;
+
+	plic_off = fdt_node_offset_by_compatible(fdt, 0, "sifive,plic-1.0.0");
+	if (plic_off < 0) {
+		plic_off = fdt_node_offset_by_compatible(fdt, 0, "riscv,plic0");
+		if (plic_off < 0)
+			return;
+	}
+
+	cells = (u32 *)fdt_getprop(fdt, plic_off,
+				   "interrupts-extended", &cells_count);
+	if (!cells)
+		return;
+
+	cells_count = cells_count / sizeof(u32);
+	if (!cells_count)
+		return;
+
+	for (i = 0; i < (cells_count / 2); i++) {
+		if (fdt32_to_cpu(cells[2 * i + 1]) == IRQ_M_EXT)
+			cells[2 * i + 1] = cpu_to_fdt32(0xffffffff);
+	}
+}
+
+static int fdt_resv_memory_update_node(void *fdt, unsigned long addr,
+				       unsigned long size, int index,
+				       int parent, bool no_map)
+{
+	int na = fdt_address_cells(fdt, 0);
+	int ns = fdt_size_cells(fdt, 0);
+	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)addr >> 32;
+	addr_low = addr;
+	size_high = (u64)size >> 32;
+	size_low = size;
+
+	if (na > 1 && addr_high)
+		sbi_snprintf(name, sizeof(name),
+			     "mmode_resv%d@%x,%x", index,
+			     addr_high, addr_low);
+	else
+		sbi_snprintf(name, sizeof(name),
+			     "mmode_resv%d@%x", index,
+			     addr_low);
+
+	subnode = fdt_add_subnode(fdt, parent, name);
+	if (subnode < 0)
+		return subnode;
+
+	if (no_map) {
+		/*
+		 * Tell operating system not to create a virtual
+		 * mapping of the region as part of its standard
+		 * mapping of system memory.
+		 */
+		err = fdt_setprop_empty(fdt, subnode, "no-map");
+		if (err < 0)
+			return err;
+	}
+
+	/* 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;
+}
+
+/**
+ * We use PMP to protect OpenSBI firmware to safe-guard it from buggy S-mode
+ * software, see pmp_init() in lib/sbi/sbi_hart.c. The protected memory region
+ * information needs to be conveyed to S-mode software (e.g.: operating system)
+ * via some well-known method.
+ *
+ * With device tree, this can be done by inserting a child node of the reserved
+ * memory node which is used to specify one or more regions of reserved memory.
+ *
+ * For the reserved memory node bindings, see Linux kernel documentation at
+ * Documentation/devicetree/bindings/reserved-memory/reserved-memory.txt
+ *
+ * Some additional memory spaces may be protected by platform codes via PMP as
+ * well, and corresponding child nodes will be inserted.
+ */
+int fdt_reserved_memory_fixup(void *fdt)
+{
+	struct sbi_domain_memregion *reg;
+	struct sbi_domain *dom = sbi_domain_thishart_ptr();
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+	unsigned long addr, size;
+	int err, parent, i;
+	int na = fdt_address_cells(fdt, 0);
+	int ns = fdt_size_cells(fdt, 0);
+
+	/*
+	 * Expand the device tree to accommodate new node
+	 * by the following estimated size:
+	 *
+	 * Each PMP memory region entry occupies 64 bytes.
+	 * With 16 PMP memory regions we need 64 * 16 = 1024 bytes.
+	 */
+	err = fdt_open_into(fdt, fdt, fdt_totalsize(fdt) + 1024);
+	if (err < 0)
+		return err;
+
+	/* try to locate the reserved memory node */
+	parent = fdt_path_offset(fdt, "/reserved-memory");
+	if (parent < 0) {
+		/* if such node does not exist, create one */
+		parent = fdt_add_subnode(fdt, 0, "reserved-memory");
+		if (parent < 0)
+			return parent;
+
+		/*
+		 * reserved-memory node has 3 required properties:
+		 * - #address-cells: the same value as the root node
+		 * - #size-cells: the same value as the root node
+		 * - ranges: should be empty
+		 */
+
+		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;
+	}
+
+	/*
+	 * We assume the given device tree does not contain any memory region
+	 * child node protected by PMP. Normally PMP programming happens at
+	 * M-mode firmware. The memory space used by OpenSBI is protected.
+	 * Some additional memory spaces may be protected by domain memory
+	 * regions.
+	 *
+	 * With above assumption, we create child nodes directly.
+	 */
+
+	i = 0;
+	sbi_domain_for_each_memregion(dom, reg) {
+		/* Ignore MMIO or READABLE or WRITABLE or EXECUTABLE regions */
+		if (reg->flags & SBI_DOMAIN_MEMREGION_MMIO)
+			continue;
+		if (reg->flags & SBI_DOMAIN_MEMREGION_READABLE)
+			continue;
+		if (reg->flags & SBI_DOMAIN_MEMREGION_WRITEABLE)
+			continue;
+		if (reg->flags & SBI_DOMAIN_MEMREGION_EXECUTABLE)
+			continue;
+
+		addr = reg->base;
+		size = 1UL << reg->order;
+		fdt_resv_memory_update_node(fdt, addr, size, i, parent,
+			(sbi_hart_pmp_count(scratch)) ? false : true);
+		i++;
+	}
+
+	return 0;
+}
+
+int fdt_reserved_memory_nomap_fixup(void *fdt)
+{
+	int parent, subnode;
+	int err;
+
+	/* Locate the reserved memory node */
+	parent = fdt_path_offset(fdt, "/reserved-memory");
+	if (parent < 0)
+		return parent;
+
+	fdt_for_each_subnode(subnode, fdt, parent) {
+		/*
+		 * Tell operating system not to create a virtual
+		 * mapping of the region as part of its standard
+		 * mapping of system memory.
+		 */
+		err = fdt_setprop_empty(fdt, subnode, "no-map");
+		if (err < 0)
+			return err;
+	}
+
+	return 0;
+}
+
+void fdt_fixups(void *fdt)
+{
+	fdt_plic_fixup(fdt);
+
+	fdt_reserved_memory_fixup(fdt);
+}
+
+

lib/utils/fdt/fdt_helper.c

@@ -0,0 +1,466 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * fdt_helper.c - Flat Device Tree manipulation helper routines
+ * Implement helper routines on top of libfdt for OpenSBI usage
+ *
+ * Copyright (C) 2020 Bin Meng <bmeng.cn@gmail.com>
+ */
+
+#include <libfdt.h>
+#include <sbi/riscv_asm.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_hartmask.h>
+#include <sbi/sbi_platform.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/irqchip/plic.h>
+#include <sbi_utils/sys/clint.h>
+
+#define DEFAULT_UART_FREQ		0
+#define DEFAULT_UART_BAUD		115200
+#define DEFAULT_UART_REG_SHIFT		0
+#define DEFAULT_UART_REG_IO_WIDTH	1
+
+#define DEFAULT_SIFIVE_UART_FREQ		0
+#define DEFAULT_SIFIVE_UART_BAUD		115200
+#define DEFAULT_SIFIVE_UART_REG_SHIFT		0
+#define DEFAULT_SIFIVE_UART_REG_IO_WIDTH	4
+
+#define DEFAULT_SHAKTI_UART_FREQ		50000000
+#define DEFAULT_SHAKTI_UART_BAUD		115200
+
+const struct fdt_match *fdt_match_node(void *fdt, int nodeoff,
+				       const struct fdt_match *match_table)
+{
+	int ret;
+
+	if (!fdt || nodeoff < 0 || !match_table)
+		return NULL;
+
+	while (match_table->compatible) {
+		ret = fdt_node_check_compatible(fdt, nodeoff,
+						match_table->compatible);
+		if (!ret)
+			return match_table;
+		match_table++;
+	}
+
+	return NULL;
+}
+
+int fdt_find_match(void *fdt, int startoff,
+		   const struct fdt_match *match_table,
+		   const struct fdt_match **out_match)
+{
+	int nodeoff;
+
+	if (!fdt || !match_table)
+		return SBI_ENODEV;
+
+	while (match_table->compatible) {
+		nodeoff = fdt_node_offset_by_compatible(fdt, startoff,
+						match_table->compatible);
+		if (nodeoff >= 0) {
+			if (out_match)
+				*out_match = match_table;
+			return nodeoff;
+		}
+		match_table++;
+	}
+
+	return SBI_ENODEV;
+}
+
+static int fdt_translate_address(void *fdt, uint64_t reg, int parent,
+				 unsigned long *addr)
+{
+	int i, rlen;
+	int cell_addr, cell_size;
+	const fdt32_t *ranges;
+	uint64_t offset = 0, caddr = 0, paddr = 0, rsize = 0;
+
+	cell_addr = fdt_address_cells(fdt, parent);
+	if (cell_addr < 1)
+		return SBI_ENODEV;
+
+	cell_size = fdt_size_cells(fdt, parent);
+	if (cell_size < 0)
+		return SBI_ENODEV;
+
+	ranges = fdt_getprop(fdt, parent, "ranges", &rlen);
+	if (ranges && rlen > 0) {
+		for (i = 0; i < cell_addr; i++)
+			caddr = (caddr << 32) | fdt32_to_cpu(*ranges++);
+		for (i = 0; i < cell_addr; i++)
+			paddr = (paddr << 32) | fdt32_to_cpu(*ranges++);
+		for (i = 0; i < cell_size; i++)
+			rsize = (rsize << 32) | fdt32_to_cpu(*ranges++);
+		if (reg < caddr || caddr >= (reg + rsize )) {
+			sbi_printf("invalid address translation\n");
+			return SBI_ENODEV;
+		}
+		offset = reg - caddr;
+		*addr = paddr + offset;
+	} else {
+		/* No translation required */
+		*addr = reg;
+	}
+
+	return 0;
+}
+
+int fdt_get_node_addr_size(void *fdt, int node, unsigned long *addr,
+			   unsigned long *size)
+{
+	int parent, len, i, rc;
+	int cell_addr, cell_size;
+	const fdt32_t *prop_addr, *prop_size;
+	uint64_t temp = 0;
+
+	parent = fdt_parent_offset(fdt, node);
+	if (parent < 0)
+		return parent;
+	cell_addr = fdt_address_cells(fdt, parent);
+	if (cell_addr < 1)
+		return SBI_ENODEV;
+
+	cell_size = fdt_size_cells(fdt, parent);
+	if (cell_size < 0)
+		return SBI_ENODEV;
+
+	prop_addr = fdt_getprop(fdt, node, "reg", &len);
+	if (!prop_addr)
+		return SBI_ENODEV;
+	prop_size = prop_addr + cell_addr;
+
+	if (addr) {
+		for (i = 0; i < cell_addr; i++)
+			temp = (temp << 32) | fdt32_to_cpu(*prop_addr++);
+		do {
+			if (parent < 0)
+				break;
+			rc  = fdt_translate_address(fdt, temp, parent, addr);
+			if (rc)
+				break;
+			parent = fdt_parent_offset(fdt, parent);
+			temp = *addr;
+		} while (1);
+	}
+	temp = 0;
+
+	if (size) {
+		for (i = 0; i < cell_size; i++)
+			temp = (temp << 32) | fdt32_to_cpu(*prop_size++);
+		*size = temp;
+	}
+
+	return 0;
+}
+
+int fdt_parse_hart_id(void *fdt, int cpu_offset, u32 *hartid)
+{
+	int len;
+	const void *prop;
+	const fdt32_t *val;
+
+	if (!fdt || cpu_offset < 0)
+		return SBI_EINVAL;
+
+	prop = fdt_getprop(fdt, cpu_offset, "device_type", &len);
+	if (!prop || !len)
+		return SBI_EINVAL;
+	if (strncmp (prop, "cpu", strlen ("cpu")))
+		return SBI_EINVAL;
+
+	val = fdt_getprop(fdt, cpu_offset, "reg", &len);
+	if (!val || len < sizeof(fdt32_t))
+		return SBI_EINVAL;
+
+	if (len > sizeof(fdt32_t))
+		val++;
+
+	if (hartid)
+		*hartid = fdt32_to_cpu(*val);
+
+	return 0;
+}
+
+int fdt_parse_max_hart_id(void *fdt, u32 *max_hartid)
+{
+	u32 hartid;
+	int err, cpu_offset, cpus_offset;
+
+	if (!fdt)
+		return SBI_EINVAL;
+	if (!max_hartid)
+		return 0;
+
+	*max_hartid = 0;
+
+	cpus_offset = fdt_path_offset(fdt, "/cpus");
+	if (cpus_offset < 0)
+		return cpus_offset;
+
+	fdt_for_each_subnode(cpu_offset, fdt, cpus_offset) {
+		err = fdt_parse_hart_id(fdt, cpu_offset, &hartid);
+		if (err)
+			continue;
+
+		if (hartid > *max_hartid)
+			*max_hartid = hartid;
+	}
+
+	return 0;
+}
+
+int fdt_parse_shakti_uart_node(void *fdt, int nodeoffset,
+			       struct platform_uart_data *uart)
+{
+	int len, rc;
+	const fdt32_t *val;
+	unsigned long reg_addr, reg_size;
+
+	if (nodeoffset < 0 || !uart || !fdt)
+		return SBI_ENODEV;
+
+	rc = fdt_get_node_addr_size(fdt, nodeoffset, &reg_addr, &reg_size);
+	if (rc < 0 || !reg_addr || !reg_size)
+		return SBI_ENODEV;
+	uart->addr = reg_addr;
+
+	/**
+	 * UART address is mandaotry. clock-frequency and current-speed
+	 * may not be present. Don't return error.
+	 */
+	val = (fdt32_t *)fdt_getprop(fdt, nodeoffset, "clock-frequency", &len);
+	if (len > 0 && val)
+		uart->freq = fdt32_to_cpu(*val);
+	else
+		uart->freq = DEFAULT_SHAKTI_UART_FREQ;
+
+	val = (fdt32_t *)fdt_getprop(fdt, nodeoffset, "current-speed", &len);
+	if (len > 0 && val)
+		uart->baud = fdt32_to_cpu(*val);
+	else
+		uart->baud = DEFAULT_SHAKTI_UART_BAUD;
+
+	return 0;
+}
+
+int fdt_parse_sifive_uart_node(void *fdt, int nodeoffset,
+			       struct platform_uart_data *uart)
+{
+	int len, rc;
+	const fdt32_t *val;
+	unsigned long reg_addr, reg_size;
+
+	if (nodeoffset < 0 || !uart || !fdt)
+		return SBI_ENODEV;
+
+	rc = fdt_get_node_addr_size(fdt, nodeoffset, &reg_addr, &reg_size);
+	if (rc < 0 || !reg_addr || !reg_size)
+		return SBI_ENODEV;
+	uart->addr = reg_addr;
+
+	/**
+	 * UART address is mandaotry. clock-frequency and current-speed
+	 * may not be present. Don't return error.
+	 */
+	val = (fdt32_t *)fdt_getprop(fdt, nodeoffset, "clock-frequency", &len);
+	if (len > 0 && val)
+		uart->freq = fdt32_to_cpu(*val);
+	else
+		uart->freq = DEFAULT_SIFIVE_UART_FREQ;
+
+	val = (fdt32_t *)fdt_getprop(fdt, nodeoffset, "current-speed", &len);
+	if (len > 0 && val)
+		uart->baud = fdt32_to_cpu(*val);
+	else
+		uart->baud = DEFAULT_SIFIVE_UART_BAUD;
+
+	/* For SiFive UART, the reg-shift and reg-io-width are fixed .*/
+	uart->reg_shift = DEFAULT_SIFIVE_UART_REG_SHIFT;
+	uart->reg_io_width = DEFAULT_SIFIVE_UART_REG_IO_WIDTH;
+
+	return 0;
+}
+
+int fdt_parse_uart8250_node(void *fdt, int nodeoffset,
+			    struct platform_uart_data *uart)
+{
+	int len, rc;
+	const fdt32_t *val;
+	unsigned long reg_addr, reg_size;
+
+	if (nodeoffset < 0 || !uart || !fdt)
+		return SBI_ENODEV;
+
+	rc = fdt_get_node_addr_size(fdt, nodeoffset, &reg_addr, &reg_size);
+	if (rc < 0 || !reg_addr || !reg_size)
+		return SBI_ENODEV;
+	uart->addr = reg_addr;
+
+	/**
+	 * UART address is mandaotry. clock-frequency and current-speed
+	 * may not be present. Don't return error.
+	 */
+	val = (fdt32_t *)fdt_getprop(fdt, nodeoffset, "clock-frequency", &len);
+	if (len > 0 && val)
+		uart->freq = fdt32_to_cpu(*val);
+	else
+		uart->freq = DEFAULT_UART_FREQ;
+
+	val = (fdt32_t *)fdt_getprop(fdt, nodeoffset, "current-speed", &len);
+	if (len > 0 && val)
+		uart->baud = fdt32_to_cpu(*val);
+	else
+		uart->baud = DEFAULT_UART_BAUD;
+
+	val = (fdt32_t *)fdt_getprop(fdt, nodeoffset, "reg-shift", &len);
+	if (len > 0 && val)
+		uart->reg_shift = fdt32_to_cpu(*val);
+	else
+		uart->reg_shift = DEFAULT_UART_REG_SHIFT;
+
+	val = (fdt32_t *)fdt_getprop(fdt, nodeoffset, "reg-io-width", &len);
+	if (len > 0 && val)
+		uart->reg_io_width = fdt32_to_cpu(*val);
+	else
+		uart->reg_io_width = DEFAULT_UART_REG_IO_WIDTH;
+
+	return 0;
+}
+
+int fdt_parse_uart8250(void *fdt, struct platform_uart_data *uart,
+		   const char *compatible)
+{
+	int nodeoffset;
+
+	if (!compatible || !uart || !fdt)
+		return SBI_ENODEV;
+
+	nodeoffset = fdt_node_offset_by_compatible(fdt, -1, compatible);
+	if (nodeoffset < 0)
+		return nodeoffset;
+
+	return fdt_parse_uart8250_node(fdt, nodeoffset, uart);
+}
+
+int fdt_parse_plic_node(void *fdt, int nodeoffset, struct plic_data *plic)
+{
+	int len, rc;
+	const fdt32_t *val;
+	unsigned long reg_addr, reg_size;
+
+	if (nodeoffset < 0 || !plic || !fdt)
+		return SBI_ENODEV;
+
+	rc = fdt_get_node_addr_size(fdt, nodeoffset, &reg_addr, &reg_size);
+	if (rc < 0 || !reg_addr || !reg_size)
+		return SBI_ENODEV;
+	plic->addr = reg_addr;
+
+	val = fdt_getprop(fdt, nodeoffset, "riscv,ndev", &len);
+	if (len > 0)
+		plic->num_src = fdt32_to_cpu(*val);
+
+	return 0;
+}
+
+int fdt_parse_plic(void *fdt, struct plic_data *plic, const char *compat)
+{
+	int nodeoffset;
+
+	if (!compat || !plic || !fdt)
+		return SBI_ENODEV;
+
+	nodeoffset = fdt_node_offset_by_compatible(fdt, -1, compat);
+	if (nodeoffset < 0)
+		return nodeoffset;
+
+	return fdt_parse_plic_node(fdt, nodeoffset, plic);
+}
+
+int fdt_parse_clint_node(void *fdt, int nodeoffset, bool for_timer,
+			 struct clint_data *clint)
+{
+	const fdt32_t *val;
+	unsigned long reg_addr, reg_size;
+	int i, rc, count, cpu_offset, cpu_intc_offset;
+	u32 phandle, hwirq, hartid, first_hartid, last_hartid;
+	u32 match_hwirq = (for_timer) ? IRQ_M_TIMER : IRQ_M_SOFT;
+
+	if (nodeoffset < 0 || !clint || !fdt)
+		return SBI_ENODEV;
+
+	rc = fdt_get_node_addr_size(fdt, nodeoffset, &reg_addr, &reg_size);
+	if (rc < 0 || !reg_addr || !reg_size)
+		return SBI_ENODEV;
+	clint->addr = reg_addr;
+
+	val = fdt_getprop(fdt, nodeoffset, "interrupts-extended", &count);
+	if (!val || count < sizeof(fdt32_t))
+		return SBI_EINVAL;
+	count = count / sizeof(fdt32_t);
+
+	first_hartid = -1U;
+	last_hartid = 0;
+	clint->hart_count = 0;
+	for (i = 0; i < count; i += 2) {
+		phandle = fdt32_to_cpu(val[i]);
+		hwirq = fdt32_to_cpu(val[i + 1]);
+
+		cpu_intc_offset = fdt_node_offset_by_phandle(fdt, phandle);
+		if (cpu_intc_offset < 0)
+			continue;
+
+		cpu_offset = fdt_parent_offset(fdt, cpu_intc_offset);
+		if (cpu_intc_offset < 0)
+			continue;
+
+		rc = fdt_parse_hart_id(fdt, cpu_offset, &hartid);
+		if (rc)
+			continue;
+
+		if (SBI_HARTMASK_MAX_BITS <= hartid)
+			continue;
+
+		if (match_hwirq == hwirq) {
+			if (hartid < first_hartid)
+				first_hartid = hartid;
+			if (hartid > last_hartid)
+				last_hartid = hartid;
+			clint->hart_count++;
+		}
+	}
+
+	if ((last_hartid < first_hartid) || first_hartid == -1U)
+		return SBI_ENODEV;
+
+	clint->first_hartid = first_hartid;
+	count = last_hartid - first_hartid + 1;
+	if (clint->hart_count < count)
+		clint->hart_count = count;
+
+	clint->has_64bit_mmio = TRUE;
+	if (fdt_getprop(fdt, nodeoffset, "clint,has-no-64bit-mmio", &count))
+		clint->has_64bit_mmio = FALSE;
+
+	return 0;
+}
+
+int fdt_parse_compat_addr(void *fdt, unsigned long *addr,
+			  const char *compatible)
+{
+	int nodeoffset, rc;
+
+	nodeoffset = fdt_node_offset_by_compatible(fdt, -1, compatible);
+	if (nodeoffset < 0)
+		return nodeoffset;
+
+	rc = fdt_get_node_addr_size(fdt, nodeoffset, addr, NULL);
+	if (rc < 0 || !addr)
+		return SBI_ENODEV;
+
+	return 0;
+}

lib/utils/fdt/objects.mk

@@ -0,0 +1,9 @@
+#
+# SPDX-License-Identifier: BSD-2-Clause
+#
+# Copyright (C) 2020 Bin Meng <bmeng.cn@gmail.com>
+#
+
+libsbiutils-objs-y += fdt/fdt_domain.o
+libsbiutils-objs-y += fdt/fdt_helper.o
+libsbiutils-objs-y += fdt/fdt_fixup.o

lib/utils/ipi/fdt_ipi.c

@@ -0,0 +1,81 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/sbi_scratch.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/ipi/fdt_ipi.h>
+
+extern struct fdt_ipi fdt_ipi_clint;
+
+static struct fdt_ipi *ipi_drivers[] = {
+	&fdt_ipi_clint
+};
+
+static struct fdt_ipi dummy = {
+	.match_table = NULL,
+	.cold_init = NULL,
+	.warm_init = NULL,
+	.exit = NULL,
+};
+
+static struct fdt_ipi *current_driver = &dummy;
+
+void fdt_ipi_exit(void)
+{
+	if (current_driver->exit)
+		current_driver->exit();
+}
+
+static int fdt_ipi_warm_init(void)
+{
+	if (current_driver->warm_init)
+		return current_driver->warm_init();
+	return 0;
+}
+
+static int fdt_ipi_cold_init(void)
+{
+	int pos, noff, rc;
+	struct fdt_ipi *drv;
+	const struct fdt_match *match;
+	void *fdt = sbi_scratch_thishart_arg1_ptr();
+
+	for (pos = 0; pos < array_size(ipi_drivers); pos++) {
+		drv = ipi_drivers[pos];
+
+		noff = -1;
+		while ((noff = fdt_find_match(fdt, noff,
+					drv->match_table, &match)) >= 0) {
+			if (drv->cold_init) {
+				rc = drv->cold_init(fdt, noff, match);
+				if (rc)
+					return rc;
+			}
+			current_driver = drv;
+		}
+
+		if (current_driver != &dummy)
+			break;
+	}
+
+	return 0;
+}
+
+int fdt_ipi_init(bool cold_boot)
+{
+	int rc;
+
+	if (cold_boot) {
+		rc = fdt_ipi_cold_init();
+		if (rc)
+			return rc;
+	}
+
+	return fdt_ipi_warm_init();
+}

lib/utils/ipi/fdt_ipi_clint.c

@@ -0,0 +1,48 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/sbi_error.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/ipi/fdt_ipi.h>
+#include <sbi_utils/sys/clint.h>
+
+#define CLINT_IPI_MAX_NR			16
+
+static unsigned long clint_ipi_count = 0;
+static struct clint_data clint_ipi[CLINT_IPI_MAX_NR];
+
+static int ipi_clint_cold_init(void *fdt, int nodeoff,
+			       const struct fdt_match *match)
+{
+	int rc;
+	struct clint_data *ci;
+
+	if (CLINT_IPI_MAX_NR <= clint_ipi_count)
+		return SBI_ENOSPC;
+	ci = &clint_ipi[clint_ipi_count++];
+
+	rc = fdt_parse_clint_node(fdt, nodeoff, FALSE, ci);
+	if (rc)
+		return rc;
+
+	return clint_cold_ipi_init(ci);
+}
+
+static const struct fdt_match ipi_clint_match[] = {
+	{ .compatible = "riscv,clint0" },
+	{ .compatible = "sifive,clint0" },
+	{ },
+};
+
+struct fdt_ipi fdt_ipi_clint = {
+	.match_table = ipi_clint_match,
+	.cold_init = ipi_clint_cold_init,
+	.warm_init = clint_warm_ipi_init,
+	.exit = NULL,
+};

lib/utils/ipi/objects.mk

@@ -0,0 +1,11 @@
+#
+# SPDX-License-Identifier: BSD-2-Clause
+#
+# Copyright (c) 2020 Western Digital Corporation or its affiliates.
+#
+# Authors:
+#   Anup Patel <anup.patel@wdc.com>
+#
+
+libsbiutils-objs-y += ipi/fdt_ipi.o
+libsbiutils-objs-y += ipi/fdt_ipi_clint.o

lib/utils/irqchip/fdt_irqchip.c

@@ -0,0 +1,74 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/sbi_scratch.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/irqchip/fdt_irqchip.h>
+
+extern struct fdt_irqchip fdt_irqchip_plic;
+
+static struct fdt_irqchip *irqchip_drivers[] = {
+	&fdt_irqchip_plic
+};
+
+static struct fdt_irqchip *current_driver = NULL;
+
+void fdt_irqchip_exit(void)
+{
+	if (current_driver && current_driver->exit)
+		current_driver->exit();
+}
+
+static int fdt_irqchip_warm_init(void)
+{
+	if (current_driver && current_driver->warm_init)
+		return current_driver->warm_init();
+	return 0;
+}
+
+static int fdt_irqchip_cold_init(void)
+{
+	int pos, noff, rc;
+	struct fdt_irqchip *drv;
+	const struct fdt_match *match;
+	void *fdt = sbi_scratch_thishart_arg1_ptr();
+
+	for (pos = 0; pos < array_size(irqchip_drivers); pos++) {
+		drv = irqchip_drivers[pos];
+
+		noff = -1;
+		while ((noff = fdt_find_match(fdt, noff,
+					drv->match_table, &match)) >= 0) {
+			if (drv->cold_init) {
+				rc = drv->cold_init(fdt, noff, match);
+				if (rc)
+					return rc;
+			}
+			current_driver = drv;
+		}
+
+		if (current_driver)
+			break;
+	}
+
+	return 0;
+}
+
+int fdt_irqchip_init(bool cold_boot)
+{
+	int rc;
+
+	if (cold_boot) {
+		rc = fdt_irqchip_cold_init();
+		if (rc)
+			return rc;
+	}
+
+	return fdt_irqchip_warm_init();
+}

lib/utils/irqchip/fdt_irqchip_plic.c

@@ -0,0 +1,120 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <libfdt.h>
+#include <sbi/riscv_asm.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_hartmask.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/irqchip/fdt_irqchip.h>
+#include <sbi_utils/irqchip/plic.h>
+
+#define PLIC_MAX_NR			16
+
+static unsigned long plic_count = 0;
+static struct plic_data plic[PLIC_MAX_NR];
+
+static struct plic_data *plic_hartid2data[SBI_HARTMASK_MAX_BITS];
+static int plic_hartid2context[SBI_HARTMASK_MAX_BITS][2];
+
+static int irqchip_plic_warm_init(void)
+{
+	u32 hartid = current_hartid();
+
+	return plic_warm_irqchip_init(plic_hartid2data[hartid],
+				      plic_hartid2context[hartid][0],
+				      plic_hartid2context[hartid][1]);
+}
+
+static int irqchip_plic_update_hartid_table(void *fdt, int nodeoff,
+					    struct plic_data *pd)
+{
+	const fdt32_t *val;
+	u32 phandle, hwirq, hartid;
+	int i, err, count, cpu_offset, cpu_intc_offset;
+
+	val = fdt_getprop(fdt, nodeoff, "interrupts-extended", &count);
+	if (!val || count < sizeof(fdt32_t))
+		return SBI_EINVAL;
+	count = count / sizeof(fdt32_t);
+
+	for (i = 0; i < count; i += 2) {
+		phandle = fdt32_to_cpu(val[i]);
+		hwirq = fdt32_to_cpu(val[i + 1]);
+
+		cpu_intc_offset = fdt_node_offset_by_phandle(fdt, phandle);
+		if (cpu_intc_offset < 0)
+			continue;
+
+		cpu_offset = fdt_parent_offset(fdt, cpu_intc_offset);
+		if (cpu_intc_offset < 0)
+			continue;
+
+		err = fdt_parse_hart_id(fdt, cpu_offset, &hartid);
+		if (err)
+			continue;
+
+		if (SBI_HARTMASK_MAX_BITS <= hartid)
+			continue;
+
+		plic_hartid2data[hartid] = pd;
+		switch (hwirq) {
+		case IRQ_M_EXT:
+			plic_hartid2context[hartid][0] = i / 2;
+			break;
+		case IRQ_S_EXT:
+			plic_hartid2context[hartid][1] = i / 2;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static int irqchip_plic_cold_init(void *fdt, int nodeoff,
+				  const struct fdt_match *match)
+{
+	int i, rc;
+	struct plic_data *pd;
+
+	if (PLIC_MAX_NR <= plic_count)
+		return SBI_ENOSPC;
+	pd = &plic[plic_count++];
+
+	rc = fdt_parse_plic_node(fdt, nodeoff, pd);
+	if (rc)
+		return rc;
+
+	rc = plic_cold_irqchip_init(pd);
+	if (rc)
+		return rc;
+
+	if (plic_count == 1) {
+		for (i = 0; i < SBI_HARTMASK_MAX_BITS; i++) {
+			plic_hartid2data[i] = NULL;
+			plic_hartid2context[i][0] = -1;
+			plic_hartid2context[i][1] = -1;
+		}
+	}
+
+	return irqchip_plic_update_hartid_table(fdt, nodeoff, pd);
+}
+
+static const struct fdt_match irqchip_plic_match[] = {
+	{ .compatible = "riscv,plic0" },
+	{ .compatible = "sifive,plic-1.0.0" },
+	{ },
+};
+
+struct fdt_irqchip fdt_irqchip_plic = {
+	.match_table = irqchip_plic_match,
+	.cold_init = irqchip_plic_cold_init,
+	.warm_init = irqchip_plic_warm_init,
+	.exit = NULL,
+};

lib/utils/irqchip/objects.mk

@@ -0,0 +1,12 @@
+#
+# SPDX-License-Identifier: BSD-2-Clause
+#
+# Copyright (c) 2019 Western Digital Corporation or its affiliates.
+#
+# Authors:
+#   Anup Patel <anup.patel@wdc.com>
+#
+
+libsbiutils-objs-y += irqchip/fdt_irqchip.o
+libsbiutils-objs-y += irqchip/fdt_irqchip_plic.o
+libsbiutils-objs-y += irqchip/plic.o

lib/utils/irqchip/plic.c

@@ -0,0 +1,100 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_io.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_string.h>
+#include <sbi_utils/irqchip/plic.h>
+
+#define PLIC_PRIORITY_BASE 0x0
+#define PLIC_PENDING_BASE 0x1000
+#define PLIC_ENABLE_BASE 0x2000
+#define PLIC_ENABLE_STRIDE 0x80
+#define PLIC_CONTEXT_BASE 0x200000
+#define PLIC_CONTEXT_STRIDE 0x1000
+
+static void plic_set_priority(struct plic_data *plic, u32 source, u32 val)
+{
+	volatile void *plic_priority = (void *)plic->addr +
+			PLIC_PRIORITY_BASE + 4 * source;
+	writel(val, plic_priority);
+}
+
+void plic_set_thresh(struct plic_data *plic, u32 cntxid, u32 val)
+{
+	volatile void *plic_thresh;
+
+	if (!plic)
+		return;
+
+	plic_thresh = (void *)plic->addr +
+		      PLIC_CONTEXT_BASE + PLIC_CONTEXT_STRIDE * cntxid;
+	writel(val, plic_thresh);
+}
+
+void plic_set_ie(struct plic_data *plic, u32 cntxid, u32 word_index, u32 val)
+{
+	volatile void *plic_ie;
+
+	if (!plic)
+		return;
+
+	plic_ie = (void *)plic->addr +
+		   PLIC_ENABLE_BASE + PLIC_ENABLE_STRIDE * cntxid;
+	writel(val, plic_ie + word_index * 4);
+}
+
+int plic_warm_irqchip_init(struct plic_data *plic,
+			   int m_cntx_id, int s_cntx_id)
+{
+	size_t i, ie_words;
+
+	if (!plic)
+		return SBI_EINVAL;
+
+	ie_words = plic->num_src / 32 + 1;
+
+	/* By default, disable all IRQs for M-mode of target HART */
+	if (m_cntx_id > -1) {
+		for (i = 0; i < ie_words; i++)
+			plic_set_ie(plic, m_cntx_id, i, 0);
+	}
+
+	/* By default, disable all IRQs for S-mode of target HART */
+	if (s_cntx_id > -1) {
+		for (i = 0; i < ie_words; i++)
+			plic_set_ie(plic, s_cntx_id, i, 0);
+	}
+
+	/* By default, disable M-mode threshold */
+	if (m_cntx_id > -1)
+		plic_set_thresh(plic, m_cntx_id, 0x7);
+
+	/* By default, disable S-mode threshold */
+	if (s_cntx_id > -1)
+		plic_set_thresh(plic, s_cntx_id, 0x7);
+
+	return 0;
+}
+
+int plic_cold_irqchip_init(struct plic_data *plic)
+{
+	int i;
+
+	if (!plic)
+		return SBI_EINVAL;
+
+	/* Configure default priorities of all IRQs */
+	for (i = 1; i <= plic->num_src; i++)
+		plic_set_priority(plic, i, 1);
+
+	return 0;
+}

lib/utils/libfdt/.clang-format

@@ -0,0 +1 @@
+DisableFormat: true

lib/utils/libfdt/Makefile.libfdt

@@ -0,0 +1,18 @@
+# SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
+# Makefile.libfdt
+#
+# This is not a complete Makefile of itself.  Instead, it is designed to
+# be easily embeddable into other systems of Makefiles.
+#
+LIBFDT_soname = libfdt.$(SHAREDLIB_EXT).1
+LIBFDT_INCLUDES = fdt.h libfdt.h libfdt_env.h
+LIBFDT_VERSION = version.lds
+LIBFDT_SRCS = fdt.c fdt_ro.c fdt_wip.c fdt_sw.c fdt_rw.c fdt_strerror.c fdt_empty_tree.c \
+	fdt_addresses.c fdt_overlay.c fdt_check.c
+LIBFDT_OBJS = $(LIBFDT_SRCS:%.c=%.o)
+LIBFDT_LIB = libfdt-$(DTC_VERSION).$(SHAREDLIB_EXT)
+
+libfdt_clean:
+	@$(VECHO) CLEAN "(libfdt)"
+	rm -f $(STD_CLEANFILES:%=$(LIBFDT_dir)/%)
+	rm -f $(LIBFDT_dir)/$(LIBFDT_soname)

lib/utils/libfdt/TODO

@@ -0,0 +1,3 @@
+- Tree traversal functions
+- Graft function
+- Complete libfdt.h documenting comments

lib/utils/libfdt/fdt.c

@@ -0,0 +1,316 @@
+// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ */
+#include "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+/*
+ * Minimal sanity check for a read-only tree. fdt_ro_probe_() checks
+ * that the given buffer contains what appears to be a flattened
+ * device tree with sane information in its header.
+ */
+int32_t fdt_ro_probe_(const void *fdt)
+{
+	uint32_t totalsize = fdt_totalsize(fdt);
+
+	if (can_assume(VALID_DTB))
+		return totalsize;
+
+	if (fdt_magic(fdt) == FDT_MAGIC) {
+		/* Complete tree */
+		if (!can_assume(LATEST)) {
+			if (fdt_version(fdt) < FDT_FIRST_SUPPORTED_VERSION)
+				return -FDT_ERR_BADVERSION;
+			if (fdt_last_comp_version(fdt) >
+					FDT_LAST_SUPPORTED_VERSION)
+				return -FDT_ERR_BADVERSION;
+		}
+	} else if (fdt_magic(fdt) == FDT_SW_MAGIC) {
+		/* Unfinished sequential-write blob */
+		if (!can_assume(VALID_INPUT) && fdt_size_dt_struct(fdt) == 0)
+			return -FDT_ERR_BADSTATE;
+	} else {
+		return -FDT_ERR_BADMAGIC;
+	}
+
+	if (totalsize < INT32_MAX)
+		return totalsize;
+	else
+		return -FDT_ERR_TRUNCATED;
+}
+
+static int check_off_(uint32_t hdrsize, uint32_t totalsize, uint32_t off)
+{
+	return (off >= hdrsize) && (off <= totalsize);
+}
+
+static int check_block_(uint32_t hdrsize, uint32_t totalsize,
+			uint32_t base, uint32_t size)
+{
+	if (!check_off_(hdrsize, totalsize, base))
+		return 0; /* block start out of bounds */
+	if ((base + size) < base)
+		return 0; /* overflow */
+	if (!check_off_(hdrsize, totalsize, base + size))
+		return 0; /* block end out of bounds */
+	return 1;
+}
+
+size_t fdt_header_size_(uint32_t version)
+{
+	if (version <= 1)
+		return FDT_V1_SIZE;
+	else if (version <= 2)
+		return FDT_V2_SIZE;
+	else if (version <= 3)
+		return FDT_V3_SIZE;
+	else if (version <= 16)
+		return FDT_V16_SIZE;
+	else
+		return FDT_V17_SIZE;
+}
+
+size_t fdt_header_size(const void *fdt)
+{
+	return can_assume(LATEST) ? FDT_V17_SIZE :
+		fdt_header_size_(fdt_version(fdt));
+}
+
+int fdt_check_header(const void *fdt)
+{
+	size_t hdrsize;
+
+	if (fdt_magic(fdt) != FDT_MAGIC)
+		return -FDT_ERR_BADMAGIC;
+	if (!can_assume(LATEST)) {
+		if ((fdt_version(fdt) < FDT_FIRST_SUPPORTED_VERSION)
+		    || (fdt_last_comp_version(fdt) >
+			FDT_LAST_SUPPORTED_VERSION))
+			return -FDT_ERR_BADVERSION;
+		if (fdt_version(fdt) < fdt_last_comp_version(fdt))
+			return -FDT_ERR_BADVERSION;
+	}
+	hdrsize = fdt_header_size(fdt);
+	if (!can_assume(VALID_DTB)) {
+
+		if ((fdt_totalsize(fdt) < hdrsize)
+		    || (fdt_totalsize(fdt) > INT_MAX))
+			return -FDT_ERR_TRUNCATED;
+
+		/* Bounds check memrsv block */
+		if (!check_off_(hdrsize, fdt_totalsize(fdt),
+				fdt_off_mem_rsvmap(fdt)))
+			return -FDT_ERR_TRUNCATED;
+	}
+
+	if (!can_assume(VALID_DTB)) {
+		/* Bounds check structure block */
+		if (!can_assume(LATEST) && fdt_version(fdt) < 17) {
+			if (!check_off_(hdrsize, fdt_totalsize(fdt),
+					fdt_off_dt_struct(fdt)))
+				return -FDT_ERR_TRUNCATED;
+		} else {
+			if (!check_block_(hdrsize, fdt_totalsize(fdt),
+					  fdt_off_dt_struct(fdt),
+					  fdt_size_dt_struct(fdt)))
+				return -FDT_ERR_TRUNCATED;
+		}
+
+		/* Bounds check strings block */
+		if (!check_block_(hdrsize, fdt_totalsize(fdt),
+				  fdt_off_dt_strings(fdt),
+				  fdt_size_dt_strings(fdt)))
+			return -FDT_ERR_TRUNCATED;
+	}
+
+	return 0;
+}
+
+const void *fdt_offset_ptr(const void *fdt, int offset, unsigned int len)
+{
+	unsigned absoffset = offset + fdt_off_dt_struct(fdt);
+
+	if (!can_assume(VALID_INPUT))
+		if ((absoffset < offset)
+		    || ((absoffset + len) < absoffset)
+		    || (absoffset + len) > fdt_totalsize(fdt))
+			return NULL;
+
+	if (can_assume(LATEST) || fdt_version(fdt) >= 0x11)
+		if (((offset + len) < offset)
+		    || ((offset + len) > fdt_size_dt_struct(fdt)))
+			return NULL;
+
+	return fdt_offset_ptr_(fdt, offset);
+}
+
+uint32_t fdt_next_tag(const void *fdt, int startoffset, int *nextoffset)
+{
+	const fdt32_t *tagp, *lenp;
+	uint32_t tag;
+	int offset = startoffset;
+	const char *p;
+
+	*nextoffset = -FDT_ERR_TRUNCATED;
+	tagp = fdt_offset_ptr(fdt, offset, FDT_TAGSIZE);
+	if (!can_assume(VALID_DTB) && !tagp)
+		return FDT_END; /* premature end */
+	tag = fdt32_to_cpu(*tagp);
+	offset += FDT_TAGSIZE;
+
+	*nextoffset = -FDT_ERR_BADSTRUCTURE;
+	switch (tag) {
+	case FDT_BEGIN_NODE:
+		/* skip name */
+		do {
+			p = fdt_offset_ptr(fdt, offset++, 1);
+		} while (p && (*p != '\0'));
+		if (!can_assume(VALID_DTB) && !p)
+			return FDT_END; /* premature end */
+		break;
+
+	case FDT_PROP:
+		lenp = fdt_offset_ptr(fdt, offset, sizeof(*lenp));
+		if (!can_assume(VALID_DTB) && !lenp)
+			return FDT_END; /* premature end */
+		/* skip-name offset, length and value */
+		offset += sizeof(struct fdt_property) - FDT_TAGSIZE
+			+ fdt32_to_cpu(*lenp);
+		if (!can_assume(LATEST) &&
+		    fdt_version(fdt) < 0x10 && fdt32_to_cpu(*lenp) >= 8 &&
+		    ((offset - fdt32_to_cpu(*lenp)) % 8) != 0)
+			offset += 4;
+		break;
+
+	case FDT_END:
+	case FDT_END_NODE:
+	case FDT_NOP:
+		break;
+
+	default:
+		return FDT_END;
+	}
+
+	if (!fdt_offset_ptr(fdt, startoffset, offset - startoffset))
+		return FDT_END; /* premature end */
+
+	*nextoffset = FDT_TAGALIGN(offset);
+	return tag;
+}
+
+int fdt_check_node_offset_(const void *fdt, int offset)
+{
+	if (can_assume(VALID_INPUT))
+		return offset;
+	if ((offset < 0) || (offset % FDT_TAGSIZE)
+	    || (fdt_next_tag(fdt, offset, &offset) != FDT_BEGIN_NODE))
+		return -FDT_ERR_BADOFFSET;
+
+	return offset;
+}
+
+int fdt_check_prop_offset_(const void *fdt, int offset)
+{
+	if ((offset < 0) || (offset % FDT_TAGSIZE)
+	    || (fdt_next_tag(fdt, offset, &offset) != FDT_PROP))
+		return -FDT_ERR_BADOFFSET;
+
+	return offset;
+}
+
+int fdt_next_node(const void *fdt, int offset, int *depth)
+{
+	int nextoffset = 0;
+	uint32_t tag;
+
+	if (offset >= 0)
+		if ((nextoffset = fdt_check_node_offset_(fdt, offset)) < 0)
+			return nextoffset;
+
+	do {
+		offset = nextoffset;
+		tag = fdt_next_tag(fdt, offset, &nextoffset);
+
+		switch (tag) {
+		case FDT_PROP:
+		case FDT_NOP:
+			break;
+
+		case FDT_BEGIN_NODE:
+			if (depth)
+				(*depth)++;
+			break;
+
+		case FDT_END_NODE:
+			if (depth && ((--(*depth)) < 0))
+				return nextoffset;
+			break;
+
+		case FDT_END:
+			if ((nextoffset >= 0)
+			    || ((nextoffset == -FDT_ERR_TRUNCATED) && !depth))
+				return -FDT_ERR_NOTFOUND;
+			else
+				return nextoffset;
+		}
+	} while (tag != FDT_BEGIN_NODE);
+
+	return offset;
+}
+
+int fdt_first_subnode(const void *fdt, int offset)
+{
+	int depth = 0;
+
+	offset = fdt_next_node(fdt, offset, &depth);
+	if (offset < 0 || depth != 1)
+		return -FDT_ERR_NOTFOUND;
+
+	return offset;
+}
+
+int fdt_next_subnode(const void *fdt, int offset)
+{
+	int depth = 1;
+
+	/*
+	 * With respect to the parent, the depth of the next subnode will be
+	 * the same as the last.
+	 */
+	do {
+		offset = fdt_next_node(fdt, offset, &depth);
+		if (offset < 0 || depth < 1)
+			return -FDT_ERR_NOTFOUND;
+	} while (depth > 1);
+
+	return offset;
+}
+
+const char *fdt_find_string_(const char *strtab, int tabsize, const char *s)
+{
+	int len = strlen(s) + 1;
+	const char *last = strtab + tabsize - len;
+	const char *p;
+
+	for (p = strtab; p <= last; p++)
+		if (memcmp(p, s, len) == 0)
+			return p;
+	return NULL;
+}
+
+int fdt_move(const void *fdt, void *buf, int bufsize)
+{
+	FDT_RO_PROBE(fdt);
+
+	if (fdt_totalsize(fdt) > bufsize)
+		return -FDT_ERR_NOSPACE;
+
+	memmove(buf, fdt, fdt_totalsize(fdt));
+	return 0;
+}

lib/utils/libfdt/fdt.h

@@ -0,0 +1,66 @@
+/* SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause) */
+#ifndef FDT_H
+#define FDT_H
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ * Copyright 2012 Kim Phillips, Freescale Semiconductor.
+ */
+
+#ifndef __ASSEMBLER__
+
+struct fdt_header {
+	fdt32_t magic;			 /* magic word FDT_MAGIC */
+	fdt32_t totalsize;		 /* total size of DT block */
+	fdt32_t off_dt_struct;		 /* offset to structure */
+	fdt32_t off_dt_strings;		 /* offset to strings */
+	fdt32_t off_mem_rsvmap;		 /* offset to memory reserve map */
+	fdt32_t version;		 /* format version */
+	fdt32_t last_comp_version;	 /* last compatible version */
+
+	/* version 2 fields below */
+	fdt32_t boot_cpuid_phys;	 /* Which physical CPU id we're
+					    booting on */
+	/* version 3 fields below */
+	fdt32_t size_dt_strings;	 /* size of the strings block */
+
+	/* version 17 fields below */
+	fdt32_t size_dt_struct;		 /* size of the structure block */
+};
+
+struct fdt_reserve_entry {
+	fdt64_t address;
+	fdt64_t size;
+};
+
+struct fdt_node_header {
+	fdt32_t tag;
+	char name[0];
+};
+
+struct fdt_property {
+	fdt32_t tag;
+	fdt32_t len;
+	fdt32_t nameoff;
+	char data[0];
+};
+
+#endif /* !__ASSEMBLY */
+
+#define FDT_MAGIC	0xd00dfeed	/* 4: version, 4: total size */
+#define FDT_TAGSIZE	sizeof(fdt32_t)
+
+#define FDT_BEGIN_NODE	0x1		/* Start node: full name */
+#define FDT_END_NODE	0x2		/* End node */
+#define FDT_PROP	0x3		/* Property: name off,
+					   size, content */
+#define FDT_NOP		0x4		/* nop */
+#define FDT_END		0x9
+
+#define FDT_V1_SIZE	(7*sizeof(fdt32_t))
+#define FDT_V2_SIZE	(FDT_V1_SIZE + sizeof(fdt32_t))
+#define FDT_V3_SIZE	(FDT_V2_SIZE + sizeof(fdt32_t))
+#define FDT_V16_SIZE	FDT_V3_SIZE
+#define FDT_V17_SIZE	(FDT_V16_SIZE + sizeof(fdt32_t))
+
+#endif /* FDT_H */

lib/utils/libfdt/fdt_addresses.c

@@ -0,0 +1,101 @@
+// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2014 David Gibson <david@gibson.dropbear.id.au>
+ * Copyright (C) 2018 embedded brains GmbH
+ */
+#include "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+static int fdt_cells(const void *fdt, int nodeoffset, const char *name)
+{
+	const fdt32_t *c;
+	uint32_t val;
+	int len;
+
+	c = fdt_getprop(fdt, nodeoffset, name, &len);
+	if (!c)
+		return len;
+
+	if (len != sizeof(*c))
+		return -FDT_ERR_BADNCELLS;
+
+	val = fdt32_to_cpu(*c);
+	if (val > FDT_MAX_NCELLS)
+		return -FDT_ERR_BADNCELLS;
+
+	return (int)val;
+}
+
+int fdt_address_cells(const void *fdt, int nodeoffset)
+{
+	int val;
+
+	val = fdt_cells(fdt, nodeoffset, "#address-cells");
+	if (val == 0)
+		return -FDT_ERR_BADNCELLS;
+	if (val == -FDT_ERR_NOTFOUND)
+		return 2;
+	return val;
+}
+
+int fdt_size_cells(const void *fdt, int nodeoffset)
+{
+	int val;
+
+	val = fdt_cells(fdt, nodeoffset, "#size-cells");
+	if (val == -FDT_ERR_NOTFOUND)
+		return 1;
+	return val;
+}
+
+/* This function assumes that [address|size]_cells is 1 or 2 */
+int fdt_appendprop_addrrange(void *fdt, int parent, int nodeoffset,
+			     const char *name, uint64_t addr, uint64_t size)
+{
+	int addr_cells, size_cells, ret;
+	uint8_t data[sizeof(fdt64_t) * 2], *prop;
+
+	ret = fdt_address_cells(fdt, parent);
+	if (ret < 0)
+		return ret;
+	addr_cells = ret;
+
+	ret = fdt_size_cells(fdt, parent);
+	if (ret < 0)
+		return ret;
+	size_cells = ret;
+
+	/* check validity of address */
+	prop = data;
+	if (addr_cells == 1) {
+		if ((addr > UINT32_MAX) || ((UINT32_MAX + 1 - addr) < size))
+			return -FDT_ERR_BADVALUE;
+
+		fdt32_st(prop, (uint32_t)addr);
+	} else if (addr_cells == 2) {
+		fdt64_st(prop, addr);
+	} else {
+		return -FDT_ERR_BADNCELLS;
+	}
+
+	/* check validity of size */
+	prop += addr_cells * sizeof(fdt32_t);
+	if (size_cells == 1) {
+		if (size > UINT32_MAX)
+			return -FDT_ERR_BADVALUE;
+
+		fdt32_st(prop, (uint32_t)size);
+	} else if (size_cells == 2) {
+		fdt64_st(prop, size);
+	} else {
+		return -FDT_ERR_BADNCELLS;
+	}
+
+	return fdt_appendprop(fdt, nodeoffset, name, data,
+			      (addr_cells + size_cells) * sizeof(fdt32_t));
+}

lib/utils/libfdt/fdt_check.c

@@ -0,0 +1,74 @@
+// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ */
+#include "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+int fdt_check_full(const void *fdt, size_t bufsize)
+{
+	int err;
+	int num_memrsv;
+	int offset, nextoffset = 0;
+	uint32_t tag;
+	unsigned int depth = 0;
+	const void *prop;
+	const char *propname;
+
+	if (bufsize < FDT_V1_SIZE)
+		return -FDT_ERR_TRUNCATED;
+	err = fdt_check_header(fdt);
+	if (err != 0)
+		return err;
+	if (bufsize < fdt_totalsize(fdt))
+		return -FDT_ERR_TRUNCATED;
+
+	num_memrsv = fdt_num_mem_rsv(fdt);
+	if (num_memrsv < 0)
+		return num_memrsv;
+
+	while (1) {
+		offset = nextoffset;
+		tag = fdt_next_tag(fdt, offset, &nextoffset);
+
+		if (nextoffset < 0)
+			return nextoffset;
+
+		switch (tag) {
+		case FDT_NOP:
+			break;
+
+		case FDT_END:
+			if (depth != 0)
+				return -FDT_ERR_BADSTRUCTURE;
+			return 0;
+
+		case FDT_BEGIN_NODE:
+			depth++;
+			if (depth > INT_MAX)
+				return -FDT_ERR_BADSTRUCTURE;
+			break;
+
+		case FDT_END_NODE:
+			if (depth == 0)
+				return -FDT_ERR_BADSTRUCTURE;
+			depth--;
+			break;
+
+		case FDT_PROP:
+			prop = fdt_getprop_by_offset(fdt, offset, &propname,
+						     &err);
+			if (!prop)
+				return err;
+			break;
+
+		default:
+			return -FDT_ERR_INTERNAL;
+		}
+	}
+}

lib/utils/libfdt/fdt_empty_tree.c

@@ -0,0 +1,38 @@
+// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2012 David Gibson, IBM Corporation.
+ */
+#include "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+int fdt_create_empty_tree(void *buf, int bufsize)
+{
+	int err;
+
+	err = fdt_create(buf, bufsize);
+	if (err)
+		return err;
+
+	err = fdt_finish_reservemap(buf);
+	if (err)
+		return err;
+
+	err = fdt_begin_node(buf, "");
+	if (err)
+		return err;
+
+	err =  fdt_end_node(buf);
+	if (err)
+		return err;
+
+	err = fdt_finish(buf);
+	if (err)
+		return err;
+
+	return fdt_open_into(buf, buf, bufsize);
+}

lib/utils/libfdt/fdt_overlay.c

@@ -0,0 +1,881 @@
+// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2016 Free Electrons
+ * Copyright (C) 2016 NextThing Co.
+ */
+#include "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+/**
+ * overlay_get_target_phandle - retrieves the target phandle of a fragment
+ * @fdto: pointer to the device tree overlay blob
+ * @fragment: node offset of the fragment in the overlay
+ *
+ * overlay_get_target_phandle() retrieves the target phandle of an
+ * overlay fragment when that fragment uses a phandle (target
+ * property) instead of a path (target-path property).
+ *
+ * returns:
+ *      the phandle pointed by the target property
+ *      0, if the phandle was not found
+ *	-1, if the phandle was malformed
+ */
+static uint32_t overlay_get_target_phandle(const void *fdto, int fragment)
+{
+	const fdt32_t *val;
+	int len;
+
+	val = fdt_getprop(fdto, fragment, "target", &len);
+	if (!val)
+		return 0;
+
+	if ((len != sizeof(*val)) || (fdt32_to_cpu(*val) == (uint32_t)-1))
+		return (uint32_t)-1;
+
+	return fdt32_to_cpu(*val);
+}
+
+/**
+ * overlay_get_target - retrieves the offset of a fragment's target
+ * @fdt: Base device tree blob
+ * @fdto: Device tree overlay blob
+ * @fragment: node offset of the fragment in the overlay
+ * @pathp: pointer which receives the path of the target (or NULL)
+ *
+ * overlay_get_target() retrieves the target offset in the base
+ * device tree of a fragment, no matter how the actual targeting is
+ * done (through a phandle or a path)
+ *
+ * returns:
+ *      the targeted node offset in the base device tree
+ *      Negative error code on error
+ */
+static int overlay_get_target(const void *fdt, const void *fdto,
+			      int fragment, char const **pathp)
+{
+	uint32_t phandle;
+	const char *path = NULL;
+	int path_len = 0, ret;
+
+	/* Try first to do a phandle based lookup */
+	phandle = overlay_get_target_phandle(fdto, fragment);
+	if (phandle == (uint32_t)-1)
+		return -FDT_ERR_BADPHANDLE;
+
+	/* no phandle, try path */
+	if (!phandle) {
+		/* And then a path based lookup */
+		path = fdt_getprop(fdto, fragment, "target-path", &path_len);
+		if (path)
+			ret = fdt_path_offset(fdt, path);
+		else
+			ret = path_len;
+	} else
+		ret = fdt_node_offset_by_phandle(fdt, phandle);
+
+	/*
+	* If we haven't found either a target or a
+	* target-path property in a node that contains a
+	* __overlay__ subnode (we wouldn't be called
+	* otherwise), consider it a improperly written
+	* overlay
+	*/
+	if (ret < 0 && path_len == -FDT_ERR_NOTFOUND)
+		ret = -FDT_ERR_BADOVERLAY;
+
+	/* return on error */
+	if (ret < 0)
+		return ret;
+
+	/* return pointer to path (if available) */
+	if (pathp)
+		*pathp = path ? path : NULL;
+
+	return ret;
+}
+
+/**
+ * overlay_phandle_add_offset - Increases a phandle by an offset
+ * @fdt: Base device tree blob
+ * @node: Device tree overlay blob
+ * @name: Name of the property to modify (phandle or linux,phandle)
+ * @delta: offset to apply
+ *
+ * overlay_phandle_add_offset() increments a node phandle by a given
+ * offset.
+ *
+ * returns:
+ *      0 on success.
+ *      Negative error code on error
+ */
+static int overlay_phandle_add_offset(void *fdt, int node,
+				      const char *name, uint32_t delta)
+{
+	const fdt32_t *val;
+	uint32_t adj_val;
+	int len;
+
+	val = fdt_getprop(fdt, node, name, &len);
+	if (!val)
+		return len;
+
+	if (len != sizeof(*val))
+		return -FDT_ERR_BADPHANDLE;
+
+	adj_val = fdt32_to_cpu(*val);
+	if ((adj_val + delta) < adj_val)
+		return -FDT_ERR_NOPHANDLES;
+
+	adj_val += delta;
+	if (adj_val == (uint32_t)-1)
+		return -FDT_ERR_NOPHANDLES;
+
+	return fdt_setprop_inplace_u32(fdt, node, name, adj_val);
+}
+
+/**
+ * overlay_adjust_node_phandles - Offsets the phandles of a node
+ * @fdto: Device tree overlay blob
+ * @node: Offset of the node we want to adjust
+ * @delta: Offset to shift the phandles of
+ *
+ * overlay_adjust_node_phandles() adds a constant to all the phandles
+ * of a given node. This is mainly use as part of the overlay
+ * application process, when we want to update all the overlay
+ * phandles to not conflict with the overlays of the base device tree.
+ *
+ * returns:
+ *      0 on success
+ *      Negative error code on failure
+ */
+static int overlay_adjust_node_phandles(void *fdto, int node,
+					uint32_t delta)
+{
+	int child;
+	int ret;
+
+	ret = overlay_phandle_add_offset(fdto, node, "phandle", delta);
+	if (ret && ret != -FDT_ERR_NOTFOUND)
+		return ret;
+
+	ret = overlay_phandle_add_offset(fdto, node, "linux,phandle", delta);
+	if (ret && ret != -FDT_ERR_NOTFOUND)
+		return ret;
+
+	fdt_for_each_subnode(child, fdto, node) {
+		ret = overlay_adjust_node_phandles(fdto, child, delta);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * overlay_adjust_local_phandles - Adjust the phandles of a whole overlay
+ * @fdto: Device tree overlay blob
+ * @delta: Offset to shift the phandles of
+ *
+ * overlay_adjust_local_phandles() adds a constant to all the
+ * phandles of an overlay. This is mainly use as part of the overlay
+ * application process, when we want to update all the overlay
+ * phandles to not conflict with the overlays of the base device tree.
+ *
+ * returns:
+ *      0 on success
+ *      Negative error code on failure
+ */
+static int overlay_adjust_local_phandles(void *fdto, uint32_t delta)
+{
+	/*
+	 * Start adjusting the phandles from the overlay root
+	 */
+	return overlay_adjust_node_phandles(fdto, 0, delta);
+}
+
+/**
+ * overlay_update_local_node_references - Adjust the overlay references
+ * @fdto: Device tree overlay blob
+ * @tree_node: Node offset of the node to operate on
+ * @fixup_node: Node offset of the matching local fixups node
+ * @delta: Offset to shift the phandles of
+ *
+ * overlay_update_local_nodes_references() update the phandles
+ * pointing to a node within the device tree overlay by adding a
+ * constant delta.
+ *
+ * This is mainly used as part of a device tree application process,
+ * where you want the device tree overlays phandles to not conflict
+ * with the ones from the base device tree before merging them.
+ *
+ * returns:
+ *      0 on success
+ *      Negative error code on failure
+ */
+static int overlay_update_local_node_references(void *fdto,
+						int tree_node,
+						int fixup_node,
+						uint32_t delta)
+{
+	int fixup_prop;
+	int fixup_child;
+	int ret;
+
+	fdt_for_each_property_offset(fixup_prop, fdto, fixup_node) {
+		const fdt32_t *fixup_val;
+		const char *tree_val;
+		const char *name;
+		int fixup_len;
+		int tree_len;
+		int i;
+
+		fixup_val = fdt_getprop_by_offset(fdto, fixup_prop,
+						  &name, &fixup_len);
+		if (!fixup_val)
+			return fixup_len;
+
+		if (fixup_len % sizeof(uint32_t))
+			return -FDT_ERR_BADOVERLAY;
+
+		tree_val = fdt_getprop(fdto, tree_node, name, &tree_len);
+		if (!tree_val) {
+			if (tree_len == -FDT_ERR_NOTFOUND)
+				return -FDT_ERR_BADOVERLAY;
+
+			return tree_len;
+		}
+
+		for (i = 0; i < (fixup_len / sizeof(uint32_t)); i++) {
+			fdt32_t adj_val;
+			uint32_t poffset;
+
+			poffset = fdt32_to_cpu(fixup_val[i]);
+
+			/*
+			 * phandles to fixup can be unaligned.
+			 *
+			 * Use a memcpy for the architectures that do
+			 * not support unaligned accesses.
+			 */
+			memcpy(&adj_val, tree_val + poffset, sizeof(adj_val));
+
+			adj_val = cpu_to_fdt32(fdt32_to_cpu(adj_val) + delta);
+
+			ret = fdt_setprop_inplace_namelen_partial(fdto,
+								  tree_node,
+								  name,
+								  strlen(name),
+								  poffset,
+								  &adj_val,
+								  sizeof(adj_val));
+			if (ret == -FDT_ERR_NOSPACE)
+				return -FDT_ERR_BADOVERLAY;
+
+			if (ret)
+				return ret;
+		}
+	}
+
+	fdt_for_each_subnode(fixup_child, fdto, fixup_node) {
+		const char *fixup_child_name = fdt_get_name(fdto, fixup_child,
+							    NULL);
+		int tree_child;
+
+		tree_child = fdt_subnode_offset(fdto, tree_node,
+						fixup_child_name);
+		if (tree_child == -FDT_ERR_NOTFOUND)
+			return -FDT_ERR_BADOVERLAY;
+		if (tree_child < 0)
+			return tree_child;
+
+		ret = overlay_update_local_node_references(fdto,
+							   tree_child,
+							   fixup_child,
+							   delta);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * overlay_update_local_references - Adjust the overlay references
+ * @fdto: Device tree overlay blob
+ * @delta: Offset to shift the phandles of
+ *
+ * overlay_update_local_references() update all the phandles pointing
+ * to a node within the device tree overlay by adding a constant
+ * delta to not conflict with the base overlay.
+ *
+ * This is mainly used as part of a device tree application process,
+ * where you want the device tree overlays phandles to not conflict
+ * with the ones from the base device tree before merging them.
+ *
+ * returns:
+ *      0 on success
+ *      Negative error code on failure
+ */
+static int overlay_update_local_references(void *fdto, uint32_t delta)
+{
+	int fixups;
+
+	fixups = fdt_path_offset(fdto, "/__local_fixups__");
+	if (fixups < 0) {
+		/* There's no local phandles to adjust, bail out */
+		if (fixups == -FDT_ERR_NOTFOUND)
+			return 0;
+
+		return fixups;
+	}
+
+	/*
+	 * Update our local references from the root of the tree
+	 */
+	return overlay_update_local_node_references(fdto, 0, fixups,
+						    delta);
+}
+
+/**
+ * overlay_fixup_one_phandle - Set an overlay phandle to the base one
+ * @fdt: Base Device Tree blob
+ * @fdto: Device tree overlay blob
+ * @symbols_off: Node offset of the symbols node in the base device tree
+ * @path: Path to a node holding a phandle in the overlay
+ * @path_len: number of path characters to consider
+ * @name: Name of the property holding the phandle reference in the overlay
+ * @name_len: number of name characters to consider
+ * @poffset: Offset within the overlay property where the phandle is stored
+ * @label: Label of the node referenced by the phandle
+ *
+ * overlay_fixup_one_phandle() resolves an overlay phandle pointing to
+ * a node in the base device tree.
+ *
+ * This is part of the device tree overlay application process, when
+ * you want all the phandles in the overlay to point to the actual
+ * base dt nodes.
+ *
+ * returns:
+ *      0 on success
+ *      Negative error code on failure
+ */
+static int overlay_fixup_one_phandle(void *fdt, void *fdto,
+				     int symbols_off,
+				     const char *path, uint32_t path_len,
+				     const char *name, uint32_t name_len,
+				     int poffset, const char *label)
+{
+	const char *symbol_path;
+	uint32_t phandle;
+	fdt32_t phandle_prop;
+	int symbol_off, fixup_off;
+	int prop_len;
+
+	if (symbols_off < 0)
+		return symbols_off;
+
+	symbol_path = fdt_getprop(fdt, symbols_off, label,
+				  &prop_len);
+	if (!symbol_path)
+		return prop_len;
+
+	symbol_off = fdt_path_offset(fdt, symbol_path);
+	if (symbol_off < 0)
+		return symbol_off;
+
+	phandle = fdt_get_phandle(fdt, symbol_off);
+	if (!phandle)
+		return -FDT_ERR_NOTFOUND;
+
+	fixup_off = fdt_path_offset_namelen(fdto, path, path_len);
+	if (fixup_off == -FDT_ERR_NOTFOUND)
+		return -FDT_ERR_BADOVERLAY;
+	if (fixup_off < 0)
+		return fixup_off;
+
+	phandle_prop = cpu_to_fdt32(phandle);
+	return fdt_setprop_inplace_namelen_partial(fdto, fixup_off,
+						   name, name_len, poffset,
+						   &phandle_prop,
+						   sizeof(phandle_prop));
+};
+
+/**
+ * overlay_fixup_phandle - Set an overlay phandle to the base one
+ * @fdt: Base Device Tree blob
+ * @fdto: Device tree overlay blob
+ * @symbols_off: Node offset of the symbols node in the base device tree
+ * @property: Property offset in the overlay holding the list of fixups
+ *
+ * overlay_fixup_phandle() resolves all the overlay phandles pointed
+ * to in a __fixups__ property, and updates them to match the phandles
+ * in use in the base device tree.
+ *
+ * This is part of the device tree overlay application process, when
+ * you want all the phandles in the overlay to point to the actual
+ * base dt nodes.
+ *
+ * returns:
+ *      0 on success
+ *      Negative error code on failure
+ */
+static int overlay_fixup_phandle(void *fdt, void *fdto, int symbols_off,
+				 int property)
+{
+	const char *value;
+	const char *label;
+	int len;
+
+	value = fdt_getprop_by_offset(fdto, property,
+				      &label, &len);
+	if (!value) {
+		if (len == -FDT_ERR_NOTFOUND)
+			return -FDT_ERR_INTERNAL;
+
+		return len;
+	}
+
+	do {
+		const char *path, *name, *fixup_end;
+		const char *fixup_str = value;
+		uint32_t path_len, name_len;
+		uint32_t fixup_len;
+		char *sep, *endptr;
+		int poffset, ret;
+
+		fixup_end = memchr(value, '\0', len);
+		if (!fixup_end)
+			return -FDT_ERR_BADOVERLAY;
+		fixup_len = fixup_end - fixup_str;
+
+		len -= fixup_len + 1;
+		value += fixup_len + 1;
+
+		path = fixup_str;
+		sep = memchr(fixup_str, ':', fixup_len);
+		if (!sep || *sep != ':')
+			return -FDT_ERR_BADOVERLAY;
+
+		path_len = sep - path;
+		if (path_len == (fixup_len - 1))
+			return -FDT_ERR_BADOVERLAY;
+
+		fixup_len -= path_len + 1;
+		name = sep + 1;
+		sep = memchr(name, ':', fixup_len);
+		if (!sep || *sep != ':')
+			return -FDT_ERR_BADOVERLAY;
+
+		name_len = sep - name;
+		if (!name_len)
+			return -FDT_ERR_BADOVERLAY;
+
+		poffset = strtoul(sep + 1, &endptr, 10);
+		if ((*endptr != '\0') || (endptr <= (sep + 1)))
+			return -FDT_ERR_BADOVERLAY;
+
+		ret = overlay_fixup_one_phandle(fdt, fdto, symbols_off,
+						path, path_len, name, name_len,
+						poffset, label);
+		if (ret)
+			return ret;
+	} while (len > 0);
+
+	return 0;
+}
+
+/**
+ * overlay_fixup_phandles - Resolve the overlay phandles to the base
+ *                          device tree
+ * @fdt: Base Device Tree blob
+ * @fdto: Device tree overlay blob
+ *
+ * overlay_fixup_phandles() resolves all the overlay phandles pointing
+ * to nodes in the base device tree.
+ *
+ * This is one of the steps of the device tree overlay application
+ * process, when you want all the phandles in the overlay to point to
+ * the actual base dt nodes.
+ *
+ * returns:
+ *      0 on success
+ *      Negative error code on failure
+ */
+static int overlay_fixup_phandles(void *fdt, void *fdto)
+{
+	int fixups_off, symbols_off;
+	int property;
+
+	/* We can have overlays without any fixups */
+	fixups_off = fdt_path_offset(fdto, "/__fixups__");
+	if (fixups_off == -FDT_ERR_NOTFOUND)
+		return 0; /* nothing to do */
+	if (fixups_off < 0)
+		return fixups_off;
+
+	/* And base DTs without symbols */
+	symbols_off = fdt_path_offset(fdt, "/__symbols__");
+	if ((symbols_off < 0 && (symbols_off != -FDT_ERR_NOTFOUND)))
+		return symbols_off;
+
+	fdt_for_each_property_offset(property, fdto, fixups_off) {
+		int ret;
+
+		ret = overlay_fixup_phandle(fdt, fdto, symbols_off, property);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * overlay_apply_node - Merges a node into the base device tree
+ * @fdt: Base Device Tree blob
+ * @target: Node offset in the base device tree to apply the fragment to
+ * @fdto: Device tree overlay blob
+ * @node: Node offset in the overlay holding the changes to merge
+ *
+ * overlay_apply_node() merges a node into a target base device tree
+ * node pointed.
+ *
+ * This is part of the final step in the device tree overlay
+ * application process, when all the phandles have been adjusted and
+ * resolved and you just have to merge overlay into the base device
+ * tree.
+ *
+ * returns:
+ *      0 on success
+ *      Negative error code on failure
+ */
+static int overlay_apply_node(void *fdt, int target,
+			      void *fdto, int node)
+{
+	int property;
+	int subnode;
+
+	fdt_for_each_property_offset(property, fdto, node) {
+		const char *name;
+		const void *prop;
+		int prop_len;
+		int ret;
+
+		prop = fdt_getprop_by_offset(fdto, property, &name,
+					     &prop_len);
+		if (prop_len == -FDT_ERR_NOTFOUND)
+			return -FDT_ERR_INTERNAL;
+		if (prop_len < 0)
+			return prop_len;
+
+		ret = fdt_setprop(fdt, target, name, prop, prop_len);
+		if (ret)
+			return ret;
+	}
+
+	fdt_for_each_subnode(subnode, fdto, node) {
+		const char *name = fdt_get_name(fdto, subnode, NULL);
+		int nnode;
+		int ret;
+
+		nnode = fdt_add_subnode(fdt, target, name);
+		if (nnode == -FDT_ERR_EXISTS) {
+			nnode = fdt_subnode_offset(fdt, target, name);
+			if (nnode == -FDT_ERR_NOTFOUND)
+				return -FDT_ERR_INTERNAL;
+		}
+
+		if (nnode < 0)
+			return nnode;
+
+		ret = overlay_apply_node(fdt, nnode, fdto, subnode);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * overlay_merge - Merge an overlay into its base device tree
+ * @fdt: Base Device Tree blob
+ * @fdto: Device tree overlay blob
+ *
+ * overlay_merge() merges an overlay into its base device tree.
+ *
+ * This is the next to last step in the device tree overlay application
+ * process, when all the phandles have been adjusted and resolved and
+ * you just have to merge overlay into the base device tree.
+ *
+ * returns:
+ *      0 on success
+ *      Negative error code on failure
+ */
+static int overlay_merge(void *fdt, void *fdto)
+{
+	int fragment;
+
+	fdt_for_each_subnode(fragment, fdto, 0) {
+		int overlay;
+		int target;
+		int ret;
+
+		/*
+		 * Each fragments will have an __overlay__ node. If
+		 * they don't, it's not supposed to be merged
+		 */
+		overlay = fdt_subnode_offset(fdto, fragment, "__overlay__");
+		if (overlay == -FDT_ERR_NOTFOUND)
+			continue;
+
+		if (overlay < 0)
+			return overlay;
+
+		target = overlay_get_target(fdt, fdto, fragment, NULL);
+		if (target < 0)
+			return target;
+
+		ret = overlay_apply_node(fdt, target, fdto, overlay);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int get_path_len(const void *fdt, int nodeoffset)
+{
+	int len = 0, namelen;
+	const char *name;
+
+	FDT_RO_PROBE(fdt);
+
+	for (;;) {
+		name = fdt_get_name(fdt, nodeoffset, &namelen);
+		if (!name)
+			return namelen;
+
+		/* root? we're done */
+		if (namelen == 0)
+			break;
+
+		nodeoffset = fdt_parent_offset(fdt, nodeoffset);
+		if (nodeoffset < 0)
+			return nodeoffset;
+		len += namelen + 1;
+	}
+
+	/* in case of root pretend it's "/" */
+	if (len == 0)
+		len++;
+	return len;
+}
+
+/**
+ * overlay_symbol_update - Update the symbols of base tree after a merge
+ * @fdt: Base Device Tree blob
+ * @fdto: Device tree overlay blob
+ *
+ * overlay_symbol_update() updates the symbols of the base tree with the
+ * symbols of the applied overlay
+ *
+ * This is the last step in the device tree overlay application
+ * process, allowing the reference of overlay symbols by subsequent
+ * overlay operations.
+ *
+ * returns:
+ *      0 on success
+ *      Negative error code on failure
+ */
+static int overlay_symbol_update(void *fdt, void *fdto)
+{
+	int root_sym, ov_sym, prop, path_len, fragment, target;
+	int len, frag_name_len, ret, rel_path_len;
+	const char *s, *e;
+	const char *path;
+	const char *name;
+	const char *frag_name;
+	const char *rel_path;
+	const char *target_path;
+	char *buf;
+	void *p;
+
+	ov_sym = fdt_subnode_offset(fdto, 0, "__symbols__");
+
+	/* if no overlay symbols exist no problem */
+	if (ov_sym < 0)
+		return 0;
+
+	root_sym = fdt_subnode_offset(fdt, 0, "__symbols__");
+
+	/* it no root symbols exist we should create them */
+	if (root_sym == -FDT_ERR_NOTFOUND)
+		root_sym = fdt_add_subnode(fdt, 0, "__symbols__");
+
+	/* any error is fatal now */
+	if (root_sym < 0)
+		return root_sym;
+
+	/* iterate over each overlay symbol */
+	fdt_for_each_property_offset(prop, fdto, ov_sym) {
+		path = fdt_getprop_by_offset(fdto, prop, &name, &path_len);
+		if (!path)
+			return path_len;
+
+		/* verify it's a string property (terminated by a single \0) */
+		if (path_len < 1 || memchr(path, '\0', path_len) != &path[path_len - 1])
+			return -FDT_ERR_BADVALUE;
+
+		/* keep end marker to avoid strlen() */
+		e = path + path_len;
+
+		if (*path != '/')
+			return -FDT_ERR_BADVALUE;
+
+		/* get fragment name first */
+		s = strchr(path + 1, '/');
+		if (!s) {
+			/* Symbol refers to something that won't end
+			 * up in the target tree */
+			continue;
+		}
+
+		frag_name = path + 1;
+		frag_name_len = s - path - 1;
+
+		/* verify format; safe since "s" lies in \0 terminated prop */
+		len = sizeof("/__overlay__/") - 1;
+		if ((e - s) > len && (memcmp(s, "/__overlay__/", len) == 0)) {
+			/* /<fragment-name>/__overlay__/<relative-subnode-path> */
+			rel_path = s + len;
+			rel_path_len = e - rel_path - 1;
+		} else if ((e - s) == len
+			   && (memcmp(s, "/__overlay__", len - 1) == 0)) {
+			/* /<fragment-name>/__overlay__ */
+			rel_path = "";
+			rel_path_len = 0;
+		} else {
+			/* Symbol refers to something that won't end
+			 * up in the target tree */
+			continue;
+		}
+
+		/* find the fragment index in which the symbol lies */
+		ret = fdt_subnode_offset_namelen(fdto, 0, frag_name,
+					       frag_name_len);
+		/* not found? */
+		if (ret < 0)
+			return -FDT_ERR_BADOVERLAY;
+		fragment = ret;
+
+		/* an __overlay__ subnode must exist */
+		ret = fdt_subnode_offset(fdto, fragment, "__overlay__");
+		if (ret < 0)
+			return -FDT_ERR_BADOVERLAY;
+
+		/* get the target of the fragment */
+		ret = overlay_get_target(fdt, fdto, fragment, &target_path);
+		if (ret < 0)
+			return ret;
+		target = ret;
+
+		/* if we have a target path use */
+		if (!target_path) {
+			ret = get_path_len(fdt, target);
+			if (ret < 0)
+				return ret;
+			len = ret;
+		} else {
+			len = strlen(target_path);
+		}
+
+		ret = fdt_setprop_placeholder(fdt, root_sym, name,
+				len + (len > 1) + rel_path_len + 1, &p);
+		if (ret < 0)
+			return ret;
+
+		if (!target_path) {
+			/* again in case setprop_placeholder changed it */
+			ret = overlay_get_target(fdt, fdto, fragment, &target_path);
+			if (ret < 0)
+				return ret;
+			target = ret;
+		}
+
+		buf = p;
+		if (len > 1) { /* target is not root */
+			if (!target_path) {
+				ret = fdt_get_path(fdt, target, buf, len + 1);
+				if (ret < 0)
+					return ret;
+			} else
+				memcpy(buf, target_path, len + 1);
+
+		} else
+			len--;
+
+		buf[len] = '/';
+		memcpy(buf + len + 1, rel_path, rel_path_len);
+		buf[len + 1 + rel_path_len] = '\0';
+	}
+
+	return 0;
+}
+
+int fdt_overlay_apply(void *fdt, void *fdto)
+{
+	uint32_t delta;
+	int ret;
+
+	FDT_RO_PROBE(fdt);
+	FDT_RO_PROBE(fdto);
+
+	ret = fdt_find_max_phandle(fdt, &delta);
+	if (ret)
+		goto err;
+
+	ret = overlay_adjust_local_phandles(fdto, delta);
+	if (ret)
+		goto err;
+
+	ret = overlay_update_local_references(fdto, delta);
+	if (ret)
+		goto err;
+
+	ret = overlay_fixup_phandles(fdt, fdto);
+	if (ret)
+		goto err;
+
+	ret = overlay_merge(fdt, fdto);
+	if (ret)
+		goto err;
+
+	ret = overlay_symbol_update(fdt, fdto);
+	if (ret)
+		goto err;
+
+	/*
+	 * The overlay has been damaged, erase its magic.
+	 */
+	fdt_set_magic(fdto, ~0);
+
+	return 0;
+
+err:
+	/*
+	 * The overlay might have been damaged, erase its magic.
+	 */
+	fdt_set_magic(fdto, ~0);
+
+	/*
+	 * The base device tree might have been damaged, erase its
+	 * magic.
+	 */
+	fdt_set_magic(fdt, ~0);
+
+	return ret;
+}

lib/utils/libfdt/fdt_ro.c

@@ -0,0 +1,857 @@
+// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ */
+#include "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+static int fdt_nodename_eq_(const void *fdt, int offset,
+			    const char *s, int len)
+{
+	int olen;
+	const char *p = fdt_get_name(fdt, offset, &olen);
+
+	if (!p || olen < len)
+		/* short match */
+		return 0;
+
+	if (memcmp(p, s, len) != 0)
+		return 0;
+
+	if (p[len] == '\0')
+		return 1;
+	else if (!memchr(s, '@', len) && (p[len] == '@'))
+		return 1;
+	else
+		return 0;
+}
+
+const char *fdt_get_string(const void *fdt, int stroffset, int *lenp)
+{
+	int32_t totalsize;
+	uint32_t absoffset;
+	size_t len;
+	int err;
+	const char *s, *n;
+
+	if (can_assume(VALID_INPUT)) {
+		s = (const char *)fdt + fdt_off_dt_strings(fdt) + stroffset;
+
+		if (lenp)
+			*lenp = strlen(s);
+		return s;
+	}
+	totalsize = fdt_ro_probe_(fdt);
+	err = totalsize;
+	if (totalsize < 0)
+		goto fail;
+
+	err = -FDT_ERR_BADOFFSET;
+	absoffset = stroffset + fdt_off_dt_strings(fdt);
+	if (absoffset >= totalsize)
+		goto fail;
+	len = totalsize - absoffset;
+
+	if (fdt_magic(fdt) == FDT_MAGIC) {
+		if (stroffset < 0)
+			goto fail;
+		if (can_assume(LATEST) || fdt_version(fdt) >= 17) {
+			if (stroffset >= fdt_size_dt_strings(fdt))
+				goto fail;
+			if ((fdt_size_dt_strings(fdt) - stroffset) < len)
+				len = fdt_size_dt_strings(fdt) - stroffset;
+		}
+	} else if (fdt_magic(fdt) == FDT_SW_MAGIC) {
+		if ((stroffset >= 0)
+		    || (stroffset < -fdt_size_dt_strings(fdt)))
+			goto fail;
+		if ((-stroffset) < len)
+			len = -stroffset;
+	} else {
+		err = -FDT_ERR_INTERNAL;
+		goto fail;
+	}
+
+	s = (const char *)fdt + absoffset;
+	n = memchr(s, '\0', len);
+	if (!n) {
+		/* missing terminating NULL */
+		err = -FDT_ERR_TRUNCATED;
+		goto fail;
+	}
+
+	if (lenp)
+		*lenp = n - s;
+	return s;
+
+fail:
+	if (lenp)
+		*lenp = err;
+	return NULL;
+}
+
+const char *fdt_string(const void *fdt, int stroffset)
+{
+	return fdt_get_string(fdt, stroffset, NULL);
+}
+
+static int fdt_string_eq_(const void *fdt, int stroffset,
+			  const char *s, int len)
+{
+	int slen;
+	const char *p = fdt_get_string(fdt, stroffset, &slen);
+
+	return p && (slen == len) && (memcmp(p, s, len) == 0);
+}
+
+int fdt_find_max_phandle(const void *fdt, uint32_t *phandle)
+{
+	uint32_t max = 0;
+	int offset = -1;
+
+	while (true) {
+		uint32_t value;
+
+		offset = fdt_next_node(fdt, offset, NULL);
+		if (offset < 0) {
+			if (offset == -FDT_ERR_NOTFOUND)
+				break;
+
+			return offset;
+		}
+
+		value = fdt_get_phandle(fdt, offset);
+
+		if (value > max)
+			max = value;
+	}
+
+	if (phandle)
+		*phandle = max;
+
+	return 0;
+}
+
+int fdt_generate_phandle(const void *fdt, uint32_t *phandle)
+{
+	uint32_t max;
+	int err;
+
+	err = fdt_find_max_phandle(fdt, &max);
+	if (err < 0)
+		return err;
+
+	if (max == FDT_MAX_PHANDLE)
+		return -FDT_ERR_NOPHANDLES;
+
+	if (phandle)
+		*phandle = max + 1;
+
+	return 0;
+}
+
+static const struct fdt_reserve_entry *fdt_mem_rsv(const void *fdt, int n)
+{
+	int offset = n * sizeof(struct fdt_reserve_entry);
+	int absoffset = fdt_off_mem_rsvmap(fdt) + offset;
+
+	if (!can_assume(VALID_INPUT)) {
+		if (absoffset < fdt_off_mem_rsvmap(fdt))
+			return NULL;
+		if (absoffset > fdt_totalsize(fdt) -
+		    sizeof(struct fdt_reserve_entry))
+			return NULL;
+	}
+	return fdt_mem_rsv_(fdt, n);
+}
+
+int fdt_get_mem_rsv(const void *fdt, int n, uint64_t *address, uint64_t *size)
+{
+	const struct fdt_reserve_entry *re;
+
+	FDT_RO_PROBE(fdt);
+	re = fdt_mem_rsv(fdt, n);
+	if (!can_assume(VALID_INPUT) && !re)
+		return -FDT_ERR_BADOFFSET;
+
+	*address = fdt64_ld(&re->address);
+	*size = fdt64_ld(&re->size);
+	return 0;
+}
+
+int fdt_num_mem_rsv(const void *fdt)
+{
+	int i;
+	const struct fdt_reserve_entry *re;
+
+	for (i = 0; (re = fdt_mem_rsv(fdt, i)) != NULL; i++) {
+		if (fdt64_ld(&re->size) == 0)
+			return i;
+	}
+	return -FDT_ERR_TRUNCATED;
+}
+
+static int nextprop_(const void *fdt, int offset)
+{
+	uint32_t tag;
+	int nextoffset;
+
+	do {
+		tag = fdt_next_tag(fdt, offset, &nextoffset);
+
+		switch (tag) {
+		case FDT_END:
+			if (nextoffset >= 0)
+				return -FDT_ERR_BADSTRUCTURE;
+			else
+				return nextoffset;
+
+		case FDT_PROP:
+			return offset;
+		}
+		offset = nextoffset;
+	} while (tag == FDT_NOP);
+
+	return -FDT_ERR_NOTFOUND;
+}
+
+int fdt_subnode_offset_namelen(const void *fdt, int offset,
+			       const char *name, int namelen)
+{
+	int depth;
+
+	FDT_RO_PROBE(fdt);
+
+	for (depth = 0;
+	     (offset >= 0) && (depth >= 0);
+	     offset = fdt_next_node(fdt, offset, &depth))
+		if ((depth == 1)
+		    && fdt_nodename_eq_(fdt, offset, name, namelen))
+			return offset;
+
+	if (depth < 0)
+		return -FDT_ERR_NOTFOUND;
+	return offset; /* error */
+}
+
+int fdt_subnode_offset(const void *fdt, int parentoffset,
+		       const char *name)
+{
+	return fdt_subnode_offset_namelen(fdt, parentoffset, name, strlen(name));
+}
+
+int fdt_path_offset_namelen(const void *fdt, const char *path, int namelen)
+{
+	const char *end = path + namelen;
+	const char *p = path;
+	int offset = 0;
+
+	FDT_RO_PROBE(fdt);
+
+	/* see if we have an alias */
+	if (*path != '/') {
+		const char *q = memchr(path, '/', end - p);
+
+		if (!q)
+			q = end;
+
+		p = fdt_get_alias_namelen(fdt, p, q - p);
+		if (!p)
+			return -FDT_ERR_BADPATH;
+		offset = fdt_path_offset(fdt, p);
+
+		p = q;
+	}
+
+	while (p < end) {
+		const char *q;
+
+		while (*p == '/') {
+			p++;
+			if (p == end)
+				return offset;
+		}
+		q = memchr(p, '/', end - p);
+		if (! q)
+			q = end;
+
+		offset = fdt_subnode_offset_namelen(fdt, offset, p, q-p);
+		if (offset < 0)
+			return offset;
+
+		p = q;
+	}
+
+	return offset;
+}
+
+int fdt_path_offset(const void *fdt, const char *path)
+{
+	return fdt_path_offset_namelen(fdt, path, strlen(path));
+}
+
+const char *fdt_get_name(const void *fdt, int nodeoffset, int *len)
+{
+	const struct fdt_node_header *nh = fdt_offset_ptr_(fdt, nodeoffset);
+	const char *nameptr;
+	int err;
+
+	if (((err = fdt_ro_probe_(fdt)) < 0)
+	    || ((err = fdt_check_node_offset_(fdt, nodeoffset)) < 0))
+			goto fail;
+
+	nameptr = nh->name;
+
+	if (!can_assume(LATEST) && fdt_version(fdt) < 0x10) {
+		/*
+		 * For old FDT versions, match the naming conventions of V16:
+		 * give only the leaf name (after all /). The actual tree
+		 * contents are loosely checked.
+		 */
+		const char *leaf;
+		leaf = strrchr(nameptr, '/');
+		if (leaf == NULL) {
+			err = -FDT_ERR_BADSTRUCTURE;
+			goto fail;
+		}
+		nameptr = leaf+1;
+	}
+
+	if (len)
+		*len = strlen(nameptr);
+
+	return nameptr;
+
+ fail:
+	if (len)
+		*len = err;
+	return NULL;
+}
+
+int fdt_first_property_offset(const void *fdt, int nodeoffset)
+{
+	int offset;
+
+	if ((offset = fdt_check_node_offset_(fdt, nodeoffset)) < 0)
+		return offset;
+
+	return nextprop_(fdt, offset);
+}
+
+int fdt_next_property_offset(const void *fdt, int offset)
+{
+	if ((offset = fdt_check_prop_offset_(fdt, offset)) < 0)
+		return offset;
+
+	return nextprop_(fdt, offset);
+}
+
+static const struct fdt_property *fdt_get_property_by_offset_(const void *fdt,
+						              int offset,
+						              int *lenp)
+{
+	int err;
+	const struct fdt_property *prop;
+
+	if (!can_assume(VALID_INPUT) &&
+	    (err = fdt_check_prop_offset_(fdt, offset)) < 0) {
+		if (lenp)
+			*lenp = err;
+		return NULL;
+	}
+
+	prop = fdt_offset_ptr_(fdt, offset);
+
+	if (lenp)
+		*lenp = fdt32_ld(&prop->len);
+
+	return prop;
+}
+
+const struct fdt_property *fdt_get_property_by_offset(const void *fdt,
+						      int offset,
+						      int *lenp)
+{
+	/* Prior to version 16, properties may need realignment
+	 * and this API does not work. fdt_getprop_*() will, however. */
+
+	if (!can_assume(LATEST) && fdt_version(fdt) < 0x10) {
+		if (lenp)
+			*lenp = -FDT_ERR_BADVERSION;
+		return NULL;
+	}
+
+	return fdt_get_property_by_offset_(fdt, offset, lenp);
+}
+
+static const struct fdt_property *fdt_get_property_namelen_(const void *fdt,
+						            int offset,
+						            const char *name,
+						            int namelen,
+							    int *lenp,
+							    int *poffset)
+{
+	for (offset = fdt_first_property_offset(fdt, offset);
+	     (offset >= 0);
+	     (offset = fdt_next_property_offset(fdt, offset))) {
+		const struct fdt_property *prop;
+
+		prop = fdt_get_property_by_offset_(fdt, offset, lenp);
+		if (!can_assume(LIBFDT_FLAWLESS) && !prop) {
+			offset = -FDT_ERR_INTERNAL;
+			break;
+		}
+		if (fdt_string_eq_(fdt, fdt32_ld(&prop->nameoff),
+				   name, namelen)) {
+			if (poffset)
+				*poffset = offset;
+			return prop;
+		}
+	}
+
+	if (lenp)
+		*lenp = offset;
+	return NULL;
+}
+
+
+const struct fdt_property *fdt_get_property_namelen(const void *fdt,
+						    int offset,
+						    const char *name,
+						    int namelen, int *lenp)
+{
+	/* Prior to version 16, properties may need realignment
+	 * and this API does not work. fdt_getprop_*() will, however. */
+	if (!can_assume(LATEST) && fdt_version(fdt) < 0x10) {
+		if (lenp)
+			*lenp = -FDT_ERR_BADVERSION;
+		return NULL;
+	}
+
+	return fdt_get_property_namelen_(fdt, offset, name, namelen, lenp,
+					 NULL);
+}
+
+
+const struct fdt_property *fdt_get_property(const void *fdt,
+					    int nodeoffset,
+					    const char *name, int *lenp)
+{
+	return fdt_get_property_namelen(fdt, nodeoffset, name,
+					strlen(name), lenp);
+}
+
+const void *fdt_getprop_namelen(const void *fdt, int nodeoffset,
+				const char *name, int namelen, int *lenp)
+{
+	int poffset;
+	const struct fdt_property *prop;
+
+	prop = fdt_get_property_namelen_(fdt, nodeoffset, name, namelen, lenp,
+					 &poffset);
+	if (!prop)
+		return NULL;
+
+	/* Handle realignment */
+	if (!can_assume(LATEST) && fdt_version(fdt) < 0x10 &&
+	    (poffset + sizeof(*prop)) % 8 && fdt32_ld(&prop->len) >= 8)
+		return prop->data + 4;
+	return prop->data;
+}
+
+const void *fdt_getprop_by_offset(const void *fdt, int offset,
+				  const char **namep, int *lenp)
+{
+	const struct fdt_property *prop;
+
+	prop = fdt_get_property_by_offset_(fdt, offset, lenp);
+	if (!prop)
+		return NULL;
+	if (namep) {
+		const char *name;
+		int namelen;
+
+		if (!can_assume(VALID_INPUT)) {
+			name = fdt_get_string(fdt, fdt32_ld(&prop->nameoff),
+					      &namelen);
+			if (!name) {
+				if (lenp)
+					*lenp = namelen;
+				return NULL;
+			}
+			*namep = name;
+		} else {
+			*namep = fdt_string(fdt, fdt32_ld(&prop->nameoff));
+		}
+	}
+
+	/* Handle realignment */
+	if (!can_assume(LATEST) && fdt_version(fdt) < 0x10 &&
+	    (offset + sizeof(*prop)) % 8 && fdt32_ld(&prop->len) >= 8)
+		return prop->data + 4;
+	return prop->data;
+}
+
+const void *fdt_getprop(const void *fdt, int nodeoffset,
+			const char *name, int *lenp)
+{
+	return fdt_getprop_namelen(fdt, nodeoffset, name, strlen(name), lenp);
+}
+
+uint32_t fdt_get_phandle(const void *fdt, int nodeoffset)
+{
+	const fdt32_t *php;
+	int len;
+
+	/* FIXME: This is a bit sub-optimal, since we potentially scan
+	 * over all the properties twice. */
+	php = fdt_getprop(fdt, nodeoffset, "phandle", &len);
+	if (!php || (len != sizeof(*php))) {
+		php = fdt_getprop(fdt, nodeoffset, "linux,phandle", &len);
+		if (!php || (len != sizeof(*php)))
+			return 0;
+	}
+
+	return fdt32_ld(php);
+}
+
+const char *fdt_get_alias_namelen(const void *fdt,
+				  const char *name, int namelen)
+{
+	int aliasoffset;
+
+	aliasoffset = fdt_path_offset(fdt, "/aliases");
+	if (aliasoffset < 0)
+		return NULL;
+
+	return fdt_getprop_namelen(fdt, aliasoffset, name, namelen, NULL);
+}
+
+const char *fdt_get_alias(const void *fdt, const char *name)
+{
+	return fdt_get_alias_namelen(fdt, name, strlen(name));
+}
+
+int fdt_get_path(const void *fdt, int nodeoffset, char *buf, int buflen)
+{
+	int pdepth = 0, p = 0;
+	int offset, depth, namelen;
+	const char *name;
+
+	FDT_RO_PROBE(fdt);
+
+	if (buflen < 2)
+		return -FDT_ERR_NOSPACE;
+
+	for (offset = 0, depth = 0;
+	     (offset >= 0) && (offset <= nodeoffset);
+	     offset = fdt_next_node(fdt, offset, &depth)) {
+		while (pdepth > depth) {
+			do {
+				p--;
+			} while (buf[p-1] != '/');
+			pdepth--;
+		}
+
+		if (pdepth >= depth) {
+			name = fdt_get_name(fdt, offset, &namelen);
+			if (!name)
+				return namelen;
+			if ((p + namelen + 1) <= buflen) {
+				memcpy(buf + p, name, namelen);
+				p += namelen;
+				buf[p++] = '/';
+				pdepth++;
+			}
+		}
+
+		if (offset == nodeoffset) {
+			if (pdepth < (depth + 1))
+				return -FDT_ERR_NOSPACE;
+
+			if (p > 1) /* special case so that root path is "/", not "" */
+				p--;
+			buf[p] = '\0';
+			return 0;
+		}
+	}
+
+	if ((offset == -FDT_ERR_NOTFOUND) || (offset >= 0))
+		return -FDT_ERR_BADOFFSET;
+	else if (offset == -FDT_ERR_BADOFFSET)
+		return -FDT_ERR_BADSTRUCTURE;
+
+	return offset; /* error from fdt_next_node() */
+}
+
+int fdt_supernode_atdepth_offset(const void *fdt, int nodeoffset,
+				 int supernodedepth, int *nodedepth)
+{
+	int offset, depth;
+	int supernodeoffset = -FDT_ERR_INTERNAL;
+
+	FDT_RO_PROBE(fdt);
+
+	if (supernodedepth < 0)
+		return -FDT_ERR_NOTFOUND;
+
+	for (offset = 0, depth = 0;
+	     (offset >= 0) && (offset <= nodeoffset);
+	     offset = fdt_next_node(fdt, offset, &depth)) {
+		if (depth == supernodedepth)
+			supernodeoffset = offset;
+
+		if (offset == nodeoffset) {
+			if (nodedepth)
+				*nodedepth = depth;
+
+			if (supernodedepth > depth)
+				return -FDT_ERR_NOTFOUND;
+			else
+				return supernodeoffset;
+		}
+	}
+
+	if (!can_assume(VALID_INPUT)) {
+		if ((offset == -FDT_ERR_NOTFOUND) || (offset >= 0))
+			return -FDT_ERR_BADOFFSET;
+		else if (offset == -FDT_ERR_BADOFFSET)
+			return -FDT_ERR_BADSTRUCTURE;
+	}
+
+	return offset; /* error from fdt_next_node() */
+}
+
+int fdt_node_depth(const void *fdt, int nodeoffset)
+{
+	int nodedepth;
+	int err;
+
+	err = fdt_supernode_atdepth_offset(fdt, nodeoffset, 0, &nodedepth);
+	if (err)
+		return (can_assume(LIBFDT_FLAWLESS) || err < 0) ? err :
+			-FDT_ERR_INTERNAL;
+	return nodedepth;
+}
+
+int fdt_parent_offset(const void *fdt, int nodeoffset)
+{
+	int nodedepth = fdt_node_depth(fdt, nodeoffset);
+
+	if (nodedepth < 0)
+		return nodedepth;
+	return fdt_supernode_atdepth_offset(fdt, nodeoffset,
+					    nodedepth - 1, NULL);
+}
+
+int fdt_node_offset_by_prop_value(const void *fdt, int startoffset,
+				  const char *propname,
+				  const void *propval, int proplen)
+{
+	int offset;
+	const void *val;
+	int len;
+
+	FDT_RO_PROBE(fdt);
+
+	/* FIXME: The algorithm here is pretty horrible: we scan each
+	 * property of a node in fdt_getprop(), then if that didn't
+	 * find what we want, we scan over them again making our way
+	 * to the next node.  Still it's the easiest to implement
+	 * approach; performance can come later. */
+	for (offset = fdt_next_node(fdt, startoffset, NULL);
+	     offset >= 0;
+	     offset = fdt_next_node(fdt, offset, NULL)) {
+		val = fdt_getprop(fdt, offset, propname, &len);
+		if (val && (len == proplen)
+		    && (memcmp(val, propval, len) == 0))
+			return offset;
+	}
+
+	return offset; /* error from fdt_next_node() */
+}
+
+int fdt_node_offset_by_phandle(const void *fdt, uint32_t phandle)
+{
+	int offset;
+
+	if ((phandle == 0) || (phandle == -1))
+		return -FDT_ERR_BADPHANDLE;
+
+	FDT_RO_PROBE(fdt);
+
+	/* FIXME: The algorithm here is pretty horrible: we
+	 * potentially scan each property of a node in
+	 * fdt_get_phandle(), then if that didn't find what
+	 * we want, we scan over them again making our way to the next
+	 * node.  Still it's the easiest to implement approach;
+	 * performance can come later. */
+	for (offset = fdt_next_node(fdt, -1, NULL);
+	     offset >= 0;
+	     offset = fdt_next_node(fdt, offset, NULL)) {
+		if (fdt_get_phandle(fdt, offset) == phandle)
+			return offset;
+	}
+
+	return offset; /* error from fdt_next_node() */
+}
+
+int fdt_stringlist_contains(const char *strlist, int listlen, const char *str)
+{
+	int len = strlen(str);
+	const char *p;
+
+	while (listlen >= len) {
+		if (memcmp(str, strlist, len+1) == 0)
+			return 1;
+		p = memchr(strlist, '\0', listlen);
+		if (!p)
+			return 0; /* malformed strlist.. */
+		listlen -= (p-strlist) + 1;
+		strlist = p + 1;
+	}
+	return 0;
+}
+
+int fdt_stringlist_count(const void *fdt, int nodeoffset, const char *property)
+{
+	const char *list, *end;
+	int length, count = 0;
+
+	list = fdt_getprop(fdt, nodeoffset, property, &length);
+	if (!list)
+		return length;
+
+	end = list + length;
+
+	while (list < end) {
+		length = strnlen(list, end - list) + 1;
+
+		/* Abort if the last string isn't properly NUL-terminated. */
+		if (list + length > end)
+			return -FDT_ERR_BADVALUE;
+
+		list += length;
+		count++;
+	}
+
+	return count;
+}
+
+int fdt_stringlist_search(const void *fdt, int nodeoffset, const char *property,
+			  const char *string)
+{
+	int length, len, idx = 0;
+	const char *list, *end;
+
+	list = fdt_getprop(fdt, nodeoffset, property, &length);
+	if (!list)
+		return length;
+
+	len = strlen(string) + 1;
+	end = list + length;
+
+	while (list < end) {
+		length = strnlen(list, end - list) + 1;
+
+		/* Abort if the last string isn't properly NUL-terminated. */
+		if (list + length > end)
+			return -FDT_ERR_BADVALUE;
+
+		if (length == len && memcmp(list, string, length) == 0)
+			return idx;
+
+		list += length;
+		idx++;
+	}
+
+	return -FDT_ERR_NOTFOUND;
+}
+
+const char *fdt_stringlist_get(const void *fdt, int nodeoffset,
+			       const char *property, int idx,
+			       int *lenp)
+{
+	const char *list, *end;
+	int length;
+
+	list = fdt_getprop(fdt, nodeoffset, property, &length);
+	if (!list) {
+		if (lenp)
+			*lenp = length;
+
+		return NULL;
+	}
+
+	end = list + length;
+
+	while (list < end) {
+		length = strnlen(list, end - list) + 1;
+
+		/* Abort if the last string isn't properly NUL-terminated. */
+		if (list + length > end) {
+			if (lenp)
+				*lenp = -FDT_ERR_BADVALUE;
+
+			return NULL;
+		}
+
+		if (idx == 0) {
+			if (lenp)
+				*lenp = length - 1;
+
+			return list;
+		}
+
+		list += length;
+		idx--;
+	}
+
+	if (lenp)
+		*lenp = -FDT_ERR_NOTFOUND;
+
+	return NULL;
+}
+
+int fdt_node_check_compatible(const void *fdt, int nodeoffset,
+			      const char *compatible)
+{
+	const void *prop;
+	int len;
+
+	prop = fdt_getprop(fdt, nodeoffset, "compatible", &len);
+	if (!prop)
+		return len;
+
+	return !fdt_stringlist_contains(prop, len, compatible);
+}
+
+int fdt_node_offset_by_compatible(const void *fdt, int startoffset,
+				  const char *compatible)
+{
+	int offset, err;
+
+	FDT_RO_PROBE(fdt);
+
+	/* FIXME: The algorithm here is pretty horrible: we scan each
+	 * property of a node in fdt_node_check_compatible(), then if
+	 * that didn't find what we want, we scan over them again
+	 * making our way to the next node.  Still it's the easiest to
+	 * implement approach; performance can come later. */
+	for (offset = fdt_next_node(fdt, startoffset, NULL);
+	     offset >= 0;
+	     offset = fdt_next_node(fdt, offset, NULL)) {
+		err = fdt_node_check_compatible(fdt, offset, compatible);
+		if ((err < 0) && (err != -FDT_ERR_NOTFOUND))
+			return err;
+		else if (err == 0)
+			return offset;
+	}
+
+	return offset; /* error from fdt_next_node() */
+}

lib/utils/libfdt/fdt_rw.c

@@ -0,0 +1,491 @@
+// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ */
+#include "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+static int fdt_blocks_misordered_(const void *fdt,
+				  int mem_rsv_size, int struct_size)
+{
+	return (fdt_off_mem_rsvmap(fdt) < FDT_ALIGN(sizeof(struct fdt_header), 8))
+		|| (fdt_off_dt_struct(fdt) <
+		    (fdt_off_mem_rsvmap(fdt) + mem_rsv_size))
+		|| (fdt_off_dt_strings(fdt) <
+		    (fdt_off_dt_struct(fdt) + struct_size))
+		|| (fdt_totalsize(fdt) <
+		    (fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt)));
+}
+
+static int fdt_rw_probe_(void *fdt)
+{
+	if (can_assume(VALID_DTB))
+		return 0;
+	FDT_RO_PROBE(fdt);
+
+	if (!can_assume(LATEST) && fdt_version(fdt) < 17)
+		return -FDT_ERR_BADVERSION;
+	if (fdt_blocks_misordered_(fdt, sizeof(struct fdt_reserve_entry),
+				   fdt_size_dt_struct(fdt)))
+		return -FDT_ERR_BADLAYOUT;
+	if (!can_assume(LATEST) && fdt_version(fdt) > 17)
+		fdt_set_version(fdt, 17);
+
+	return 0;
+}
+
+#define FDT_RW_PROBE(fdt) \
+	{ \
+		int err_; \
+		if ((err_ = fdt_rw_probe_(fdt)) != 0) \
+			return err_; \
+	}
+
+static inline int fdt_data_size_(void *fdt)
+{
+	return fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt);
+}
+
+static int fdt_splice_(void *fdt, void *splicepoint, int oldlen, int newlen)
+{
+	char *p = splicepoint;
+	char *end = (char *)fdt + fdt_data_size_(fdt);
+
+	if (((p + oldlen) < p) || ((p + oldlen) > end))
+		return -FDT_ERR_BADOFFSET;
+	if ((p < (char *)fdt) || ((end - oldlen + newlen) < (char *)fdt))
+		return -FDT_ERR_BADOFFSET;
+	if ((end - oldlen + newlen) > ((char *)fdt + fdt_totalsize(fdt)))
+		return -FDT_ERR_NOSPACE;
+	memmove(p + newlen, p + oldlen, end - p - oldlen);
+	return 0;
+}
+
+static int fdt_splice_mem_rsv_(void *fdt, struct fdt_reserve_entry *p,
+			       int oldn, int newn)
+{
+	int delta = (newn - oldn) * sizeof(*p);
+	int err;
+	err = fdt_splice_(fdt, p, oldn * sizeof(*p), newn * sizeof(*p));
+	if (err)
+		return err;
+	fdt_set_off_dt_struct(fdt, fdt_off_dt_struct(fdt) + delta);
+	fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta);
+	return 0;
+}
+
+static int fdt_splice_struct_(void *fdt, void *p,
+			      int oldlen, int newlen)
+{
+	int delta = newlen - oldlen;
+	int err;
+
+	if ((err = fdt_splice_(fdt, p, oldlen, newlen)))
+		return err;
+
+	fdt_set_size_dt_struct(fdt, fdt_size_dt_struct(fdt) + delta);
+	fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta);
+	return 0;
+}
+
+/* Must only be used to roll back in case of error */
+static void fdt_del_last_string_(void *fdt, const char *s)
+{
+	int newlen = strlen(s) + 1;
+
+	fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) - newlen);
+}
+
+static int fdt_splice_string_(void *fdt, int newlen)
+{
+	void *p = (char *)fdt
+		+ fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt);
+	int err;
+
+	if ((err = fdt_splice_(fdt, p, 0, newlen)))
+		return err;
+
+	fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) + newlen);
+	return 0;
+}
+
+/**
+ * fdt_find_add_string_() - Find or allocate a string
+ *
+ * @fdt: pointer to the device tree to check/adjust
+ * @s: string to find/add
+ * @allocated: Set to 0 if the string was found, 1 if not found and so
+ *	allocated. Ignored if can_assume(NO_ROLLBACK)
+ * @return offset of string in the string table (whether found or added)
+ */
+static int fdt_find_add_string_(void *fdt, const char *s, int *allocated)
+{
+	char *strtab = (char *)fdt + fdt_off_dt_strings(fdt);
+	const char *p;
+	char *new;
+	int len = strlen(s) + 1;
+	int err;
+
+	if (!can_assume(NO_ROLLBACK))
+		*allocated = 0;
+
+	p = fdt_find_string_(strtab, fdt_size_dt_strings(fdt), s);
+	if (p)
+		/* found it */
+		return (p - strtab);
+
+	new = strtab + fdt_size_dt_strings(fdt);
+	err = fdt_splice_string_(fdt, len);
+	if (err)
+		return err;
+
+	if (!can_assume(NO_ROLLBACK))
+		*allocated = 1;
+
+	memcpy(new, s, len);
+	return (new - strtab);
+}
+
+int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size)
+{
+	struct fdt_reserve_entry *re;
+	int err;
+
+	FDT_RW_PROBE(fdt);
+
+	re = fdt_mem_rsv_w_(fdt, fdt_num_mem_rsv(fdt));
+	err = fdt_splice_mem_rsv_(fdt, re, 0, 1);
+	if (err)
+		return err;
+
+	re->address = cpu_to_fdt64(address);
+	re->size = cpu_to_fdt64(size);
+	return 0;
+}
+
+int fdt_del_mem_rsv(void *fdt, int n)
+{
+	struct fdt_reserve_entry *re = fdt_mem_rsv_w_(fdt, n);
+
+	FDT_RW_PROBE(fdt);
+
+	if (n >= fdt_num_mem_rsv(fdt))
+		return -FDT_ERR_NOTFOUND;
+
+	return fdt_splice_mem_rsv_(fdt, re, 1, 0);
+}
+
+static int fdt_resize_property_(void *fdt, int nodeoffset, const char *name,
+				int len, struct fdt_property **prop)
+{
+	int oldlen;
+	int err;
+
+	*prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
+	if (!*prop)
+		return oldlen;
+
+	if ((err = fdt_splice_struct_(fdt, (*prop)->data, FDT_TAGALIGN(oldlen),
+				      FDT_TAGALIGN(len))))
+		return err;
+
+	(*prop)->len = cpu_to_fdt32(len);
+	return 0;
+}
+
+static int fdt_add_property_(void *fdt, int nodeoffset, const char *name,
+			     int len, struct fdt_property **prop)
+{
+	int proplen;
+	int nextoffset;
+	int namestroff;
+	int err;
+	int allocated;
+
+	if ((nextoffset = fdt_check_node_offset_(fdt, nodeoffset)) < 0)
+		return nextoffset;
+
+	namestroff = fdt_find_add_string_(fdt, name, &allocated);
+	if (namestroff < 0)
+		return namestroff;
+
+	*prop = fdt_offset_ptr_w_(fdt, nextoffset);
+	proplen = sizeof(**prop) + FDT_TAGALIGN(len);
+
+	err = fdt_splice_struct_(fdt, *prop, 0, proplen);
+	if (err) {
+		/* Delete the string if we failed to add it */
+		if (!can_assume(NO_ROLLBACK) && allocated)
+			fdt_del_last_string_(fdt, name);
+		return err;
+	}
+
+	(*prop)->tag = cpu_to_fdt32(FDT_PROP);
+	(*prop)->nameoff = cpu_to_fdt32(namestroff);
+	(*prop)->len = cpu_to_fdt32(len);
+	return 0;
+}
+
+int fdt_set_name(void *fdt, int nodeoffset, const char *name)
+{
+	char *namep;
+	int oldlen, newlen;
+	int err;
+
+	FDT_RW_PROBE(fdt);
+
+	namep = (char *)(uintptr_t)fdt_get_name(fdt, nodeoffset, &oldlen);
+	if (!namep)
+		return oldlen;
+
+	newlen = strlen(name);
+
+	err = fdt_splice_struct_(fdt, namep, FDT_TAGALIGN(oldlen+1),
+				 FDT_TAGALIGN(newlen+1));
+	if (err)
+		return err;
+
+	memcpy(namep, name, newlen+1);
+	return 0;
+}
+
+int fdt_setprop_placeholder(void *fdt, int nodeoffset, const char *name,
+			    int len, void **prop_data)
+{
+	struct fdt_property *prop;
+	int err;
+
+	FDT_RW_PROBE(fdt);
+
+	err = fdt_resize_property_(fdt, nodeoffset, name, len, &prop);
+	if (err == -FDT_ERR_NOTFOUND)
+		err = fdt_add_property_(fdt, nodeoffset, name, len, &prop);
+	if (err)
+		return err;
+
+	*prop_data = prop->data;
+	return 0;
+}
+
+int fdt_setprop(void *fdt, int nodeoffset, const char *name,
+		const void *val, int len)
+{
+	void *prop_data;
+	int err;
+
+	err = fdt_setprop_placeholder(fdt, nodeoffset, name, len, &prop_data);
+	if (err)
+		return err;
+
+	if (len)
+		memcpy(prop_data, val, len);
+	return 0;
+}
+
+int fdt_appendprop(void *fdt, int nodeoffset, const char *name,
+		   const void *val, int len)
+{
+	struct fdt_property *prop;
+	int err, oldlen, newlen;
+
+	FDT_RW_PROBE(fdt);
+
+	prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
+	if (prop) {
+		newlen = len + oldlen;
+		err = fdt_splice_struct_(fdt, prop->data,
+					 FDT_TAGALIGN(oldlen),
+					 FDT_TAGALIGN(newlen));
+		if (err)
+			return err;
+		prop->len = cpu_to_fdt32(newlen);
+		memcpy(prop->data + oldlen, val, len);
+	} else {
+		err = fdt_add_property_(fdt, nodeoffset, name, len, &prop);
+		if (err)
+			return err;
+		memcpy(prop->data, val, len);
+	}
+	return 0;
+}
+
+int fdt_delprop(void *fdt, int nodeoffset, const char *name)
+{
+	struct fdt_property *prop;
+	int len, proplen;
+
+	FDT_RW_PROBE(fdt);
+
+	prop = fdt_get_property_w(fdt, nodeoffset, name, &len);
+	if (!prop)
+		return len;
+
+	proplen = sizeof(*prop) + FDT_TAGALIGN(len);
+	return fdt_splice_struct_(fdt, prop, proplen, 0);
+}
+
+int fdt_add_subnode_namelen(void *fdt, int parentoffset,
+			    const char *name, int namelen)
+{
+	struct fdt_node_header *nh;
+	int offset, nextoffset;
+	int nodelen;
+	int err;
+	uint32_t tag;
+	fdt32_t *endtag;
+
+	FDT_RW_PROBE(fdt);
+
+	offset = fdt_subnode_offset_namelen(fdt, parentoffset, name, namelen);
+	if (offset >= 0)
+		return -FDT_ERR_EXISTS;
+	else if (offset != -FDT_ERR_NOTFOUND)
+		return offset;
+
+	/* Try to place the new node after the parent's properties */
+	fdt_next_tag(fdt, parentoffset, &nextoffset); /* skip the BEGIN_NODE */
+	do {
+		offset = nextoffset;
+		tag = fdt_next_tag(fdt, offset, &nextoffset);
+	} while ((tag == FDT_PROP) || (tag == FDT_NOP));
+
+	nh = fdt_offset_ptr_w_(fdt, offset);
+	nodelen = sizeof(*nh) + FDT_TAGALIGN(namelen+1) + FDT_TAGSIZE;
+
+	err = fdt_splice_struct_(fdt, nh, 0, nodelen);
+	if (err)
+		return err;
+
+	nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
+	memset(nh->name, 0, FDT_TAGALIGN(namelen+1));
+	memcpy(nh->name, name, namelen);
+	endtag = (fdt32_t *)((char *)nh + nodelen - FDT_TAGSIZE);
+	*endtag = cpu_to_fdt32(FDT_END_NODE);
+
+	return offset;
+}
+
+int fdt_add_subnode(void *fdt, int parentoffset, const char *name)
+{
+	return fdt_add_subnode_namelen(fdt, parentoffset, name, strlen(name));
+}
+
+int fdt_del_node(void *fdt, int nodeoffset)
+{
+	int endoffset;
+
+	FDT_RW_PROBE(fdt);
+
+	endoffset = fdt_node_end_offset_(fdt, nodeoffset);
+	if (endoffset < 0)
+		return endoffset;
+
+	return fdt_splice_struct_(fdt, fdt_offset_ptr_w_(fdt, nodeoffset),
+				  endoffset - nodeoffset, 0);
+}
+
+static void fdt_packblocks_(const char *old, char *new,
+			    int mem_rsv_size, int struct_size)
+{
+	int mem_rsv_off, struct_off, strings_off;
+
+	mem_rsv_off = FDT_ALIGN(sizeof(struct fdt_header), 8);
+	struct_off = mem_rsv_off + mem_rsv_size;
+	strings_off = struct_off + struct_size;
+
+	memmove(new + mem_rsv_off, old + fdt_off_mem_rsvmap(old), mem_rsv_size);
+	fdt_set_off_mem_rsvmap(new, mem_rsv_off);
+
+	memmove(new + struct_off, old + fdt_off_dt_struct(old), struct_size);
+	fdt_set_off_dt_struct(new, struct_off);
+	fdt_set_size_dt_struct(new, struct_size);
+
+	memmove(new + strings_off, old + fdt_off_dt_strings(old),
+		fdt_size_dt_strings(old));
+	fdt_set_off_dt_strings(new, strings_off);
+	fdt_set_size_dt_strings(new, fdt_size_dt_strings(old));
+}
+
+int fdt_open_into(const void *fdt, void *buf, int bufsize)
+{
+	int err;
+	int mem_rsv_size, struct_size;
+	int newsize;
+	const char *fdtstart = fdt;
+	const char *fdtend = fdtstart + fdt_totalsize(fdt);
+	char *tmp;
+
+	FDT_RO_PROBE(fdt);
+
+	mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
+		* sizeof(struct fdt_reserve_entry);
+
+	if (can_assume(LATEST) || fdt_version(fdt) >= 17) {
+		struct_size = fdt_size_dt_struct(fdt);
+	} else {
+		struct_size = 0;
+		while (fdt_next_tag(fdt, struct_size, &struct_size) != FDT_END)
+			;
+		if (struct_size < 0)
+			return struct_size;
+	}
+
+	if (can_assume(LIBFDT_ORDER) |
+	    !fdt_blocks_misordered_(fdt, mem_rsv_size, struct_size)) {
+		/* no further work necessary */
+		err = fdt_move(fdt, buf, bufsize);
+		if (err)
+			return err;
+		fdt_set_version(buf, 17);
+		fdt_set_size_dt_struct(buf, struct_size);
+		fdt_set_totalsize(buf, bufsize);
+		return 0;
+	}
+
+	/* Need to reorder */
+	newsize = FDT_ALIGN(sizeof(struct fdt_header), 8) + mem_rsv_size
+		+ struct_size + fdt_size_dt_strings(fdt);
+
+	if (bufsize < newsize)
+		return -FDT_ERR_NOSPACE;
+
+	/* First attempt to build converted tree at beginning of buffer */
+	tmp = buf;
+	/* But if that overlaps with the old tree... */
+	if (((tmp + newsize) > fdtstart) && (tmp < fdtend)) {
+		/* Try right after the old tree instead */
+		tmp = (char *)(uintptr_t)fdtend;
+		if ((tmp + newsize) > ((char *)buf + bufsize))
+			return -FDT_ERR_NOSPACE;
+	}
+
+	fdt_packblocks_(fdt, tmp, mem_rsv_size, struct_size);
+	memmove(buf, tmp, newsize);
+
+	fdt_set_magic(buf, FDT_MAGIC);
+	fdt_set_totalsize(buf, bufsize);
+	fdt_set_version(buf, 17);
+	fdt_set_last_comp_version(buf, 16);
+	fdt_set_boot_cpuid_phys(buf, fdt_boot_cpuid_phys(fdt));
+
+	return 0;
+}
+
+int fdt_pack(void *fdt)
+{
+	int mem_rsv_size;
+
+	FDT_RW_PROBE(fdt);
+
+	mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
+		* sizeof(struct fdt_reserve_entry);
+	fdt_packblocks_(fdt, fdt, mem_rsv_size, fdt_size_dt_struct(fdt));
+	fdt_set_totalsize(fdt, fdt_data_size_(fdt));
+
+	return 0;
+}

lib/utils/libfdt/fdt_strerror.c

@@ -0,0 +1,59 @@
+// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *     EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+#include "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+struct fdt_errtabent {
+	const char *str;
+};
+
+#define FDT_ERRTABENT(val) \
+	[(val)] = { .str = #val, }
+
+static struct fdt_errtabent fdt_errtable[] = {
+	FDT_ERRTABENT(FDT_ERR_NOTFOUND),
+	FDT_ERRTABENT(FDT_ERR_EXISTS),
+	FDT_ERRTABENT(FDT_ERR_NOSPACE),
+
+	FDT_ERRTABENT(FDT_ERR_BADOFFSET),
+	FDT_ERRTABENT(FDT_ERR_BADPATH),
+	FDT_ERRTABENT(FDT_ERR_BADPHANDLE),
+	FDT_ERRTABENT(FDT_ERR_BADSTATE),
+
+	FDT_ERRTABENT(FDT_ERR_TRUNCATED),
+	FDT_ERRTABENT(FDT_ERR_BADMAGIC),
+	FDT_ERRTABENT(FDT_ERR_BADVERSION),
+	FDT_ERRTABENT(FDT_ERR_BADSTRUCTURE),
+	FDT_ERRTABENT(FDT_ERR_BADLAYOUT),
+	FDT_ERRTABENT(FDT_ERR_INTERNAL),
+	FDT_ERRTABENT(FDT_ERR_BADNCELLS),
+	FDT_ERRTABENT(FDT_ERR_BADVALUE),
+	FDT_ERRTABENT(FDT_ERR_BADOVERLAY),
+	FDT_ERRTABENT(FDT_ERR_NOPHANDLES),
+	FDT_ERRTABENT(FDT_ERR_BADFLAGS),
+};
+#define FDT_ERRTABSIZE	(sizeof(fdt_errtable) / sizeof(fdt_errtable[0]))
+
+const char *fdt_strerror(int errval)
+{
+	if (errval > 0)
+		return "<valid offset/length>";
+	else if (errval == 0)
+		return "<no error>";
+	else if (errval > -FDT_ERRTABSIZE) {
+		const char *s = fdt_errtable[-errval].str;
+
+		if (s)
+			return s;
+	}
+
+	return "<unknown error>";
+}

lib/utils/libfdt/fdt_sw.c

@@ -0,0 +1,381 @@
+// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ */
+#include "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+static int fdt_sw_probe_(void *fdt)
+{
+	if (!can_assume(VALID_INPUT)) {
+		if (fdt_magic(fdt) == FDT_MAGIC)
+			return -FDT_ERR_BADSTATE;
+		else if (fdt_magic(fdt) != FDT_SW_MAGIC)
+			return -FDT_ERR_BADMAGIC;
+	}
+
+	return 0;
+}
+
+#define FDT_SW_PROBE(fdt) \
+	{ \
+		int err; \
+		if ((err = fdt_sw_probe_(fdt)) != 0) \
+			return err; \
+	}
+
+/* 'memrsv' state:	Initial state after fdt_create()
+ *
+ * Allowed functions:
+ *	fdt_add_reservmap_entry()
+ *	fdt_finish_reservemap()		[moves to 'struct' state]
+ */
+static int fdt_sw_probe_memrsv_(void *fdt)
+{
+	int err = fdt_sw_probe_(fdt);
+	if (err)
+		return err;
+
+	if (!can_assume(VALID_INPUT) && fdt_off_dt_strings(fdt) != 0)
+		return -FDT_ERR_BADSTATE;
+	return 0;
+}
+
+#define FDT_SW_PROBE_MEMRSV(fdt) \
+	{ \
+		int err; \
+		if ((err = fdt_sw_probe_memrsv_(fdt)) != 0) \
+			return err; \
+	}
+
+/* 'struct' state:	Enter this state after fdt_finish_reservemap()
+ *
+ * Allowed functions:
+ *	fdt_begin_node()
+ *	fdt_end_node()
+ *	fdt_property*()
+ *	fdt_finish()			[moves to 'complete' state]
+ */
+static int fdt_sw_probe_struct_(void *fdt)
+{
+	int err = fdt_sw_probe_(fdt);
+	if (err)
+		return err;
+
+	if (!can_assume(VALID_INPUT) &&
+	    fdt_off_dt_strings(fdt) != fdt_totalsize(fdt))
+		return -FDT_ERR_BADSTATE;
+	return 0;
+}
+
+#define FDT_SW_PROBE_STRUCT(fdt) \
+	{ \
+		int err; \
+		if ((err = fdt_sw_probe_struct_(fdt)) != 0) \
+			return err; \
+	}
+
+static inline uint32_t sw_flags(void *fdt)
+{
+	/* assert: (fdt_magic(fdt) == FDT_SW_MAGIC) */
+	return fdt_last_comp_version(fdt);
+}
+
+/* 'complete' state:	Enter this state after fdt_finish()
+ *
+ * Allowed functions: none
+ */
+
+static void *fdt_grab_space_(void *fdt, size_t len)
+{
+	int offset = fdt_size_dt_struct(fdt);
+	int spaceleft;
+
+	spaceleft = fdt_totalsize(fdt) - fdt_off_dt_struct(fdt)
+		- fdt_size_dt_strings(fdt);
+
+	if ((offset + len < offset) || (offset + len > spaceleft))
+		return NULL;
+
+	fdt_set_size_dt_struct(fdt, offset + len);
+	return fdt_offset_ptr_w_(fdt, offset);
+}
+
+int fdt_create_with_flags(void *buf, int bufsize, uint32_t flags)
+{
+	const size_t hdrsize = FDT_ALIGN(sizeof(struct fdt_header),
+					 sizeof(struct fdt_reserve_entry));
+	void *fdt = buf;
+
+	if (bufsize < hdrsize)
+		return -FDT_ERR_NOSPACE;
+
+	if (flags & ~FDT_CREATE_FLAGS_ALL)
+		return -FDT_ERR_BADFLAGS;
+
+	memset(buf, 0, bufsize);
+
+	/*
+	 * magic and last_comp_version keep intermediate state during the fdt
+	 * creation process, which is replaced with the proper FDT format by
+	 * fdt_finish().
+	 *
+	 * flags should be accessed with sw_flags().
+	 */
+	fdt_set_magic(fdt, FDT_SW_MAGIC);
+	fdt_set_version(fdt, FDT_LAST_SUPPORTED_VERSION);
+	fdt_set_last_comp_version(fdt, flags);
+
+	fdt_set_totalsize(fdt,  bufsize);
+
+	fdt_set_off_mem_rsvmap(fdt, hdrsize);
+	fdt_set_off_dt_struct(fdt, fdt_off_mem_rsvmap(fdt));
+	fdt_set_off_dt_strings(fdt, 0);
+
+	return 0;
+}
+
+int fdt_create(void *buf, int bufsize)
+{
+	return fdt_create_with_flags(buf, bufsize, 0);
+}
+
+int fdt_resize(void *fdt, void *buf, int bufsize)
+{
+	size_t headsize, tailsize;
+	char *oldtail, *newtail;
+
+	FDT_SW_PROBE(fdt);
+
+	headsize = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
+	tailsize = fdt_size_dt_strings(fdt);
+
+	if (!can_assume(VALID_DTB) &&
+	    headsize + tailsize > fdt_totalsize(fdt))
+		return -FDT_ERR_INTERNAL;
+
+	if ((headsize + tailsize) > bufsize)
+		return -FDT_ERR_NOSPACE;
+
+	oldtail = (char *)fdt + fdt_totalsize(fdt) - tailsize;
+	newtail = (char *)buf + bufsize - tailsize;
+
+	/* Two cases to avoid clobbering data if the old and new
+	 * buffers partially overlap */
+	if (buf <= fdt) {
+		memmove(buf, fdt, headsize);
+		memmove(newtail, oldtail, tailsize);
+	} else {
+		memmove(newtail, oldtail, tailsize);
+		memmove(buf, fdt, headsize);
+	}
+
+	fdt_set_totalsize(buf, bufsize);
+	if (fdt_off_dt_strings(buf))
+		fdt_set_off_dt_strings(buf, bufsize);
+
+	return 0;
+}
+
+int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size)
+{
+	struct fdt_reserve_entry *re;
+	int offset;
+
+	FDT_SW_PROBE_MEMRSV(fdt);
+
+	offset = fdt_off_dt_struct(fdt);
+	if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
+		return -FDT_ERR_NOSPACE;
+
+	re = (struct fdt_reserve_entry *)((char *)fdt + offset);
+	re->address = cpu_to_fdt64(addr);
+	re->size = cpu_to_fdt64(size);
+
+	fdt_set_off_dt_struct(fdt, offset + sizeof(*re));
+
+	return 0;
+}
+
+int fdt_finish_reservemap(void *fdt)
+{
+	int err = fdt_add_reservemap_entry(fdt, 0, 0);
+
+	if (err)
+		return err;
+
+	fdt_set_off_dt_strings(fdt, fdt_totalsize(fdt));
+	return 0;
+}
+
+int fdt_begin_node(void *fdt, const char *name)
+{
+	struct fdt_node_header *nh;
+	int namelen;
+
+	FDT_SW_PROBE_STRUCT(fdt);
+
+	namelen = strlen(name) + 1;
+	nh = fdt_grab_space_(fdt, sizeof(*nh) + FDT_TAGALIGN(namelen));
+	if (! nh)
+		return -FDT_ERR_NOSPACE;
+
+	nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
+	memcpy(nh->name, name, namelen);
+	return 0;
+}
+
+int fdt_end_node(void *fdt)
+{
+	fdt32_t *en;
+
+	FDT_SW_PROBE_STRUCT(fdt);
+
+	en = fdt_grab_space_(fdt, FDT_TAGSIZE);
+	if (! en)
+		return -FDT_ERR_NOSPACE;
+
+	*en = cpu_to_fdt32(FDT_END_NODE);
+	return 0;
+}
+
+static int fdt_add_string_(void *fdt, const char *s)
+{
+	char *strtab = (char *)fdt + fdt_totalsize(fdt);
+	int strtabsize = fdt_size_dt_strings(fdt);
+	int len = strlen(s) + 1;
+	int struct_top, offset;
+
+	offset = -strtabsize - len;
+	struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
+	if (fdt_totalsize(fdt) + offset < struct_top)
+		return 0; /* no more room :( */
+
+	memcpy(strtab + offset, s, len);
+	fdt_set_size_dt_strings(fdt, strtabsize + len);
+	return offset;
+}
+
+/* Must only be used to roll back in case of error */
+static void fdt_del_last_string_(void *fdt, const char *s)
+{
+	int strtabsize = fdt_size_dt_strings(fdt);
+	int len = strlen(s) + 1;
+
+	fdt_set_size_dt_strings(fdt, strtabsize - len);
+}
+
+static int fdt_find_add_string_(void *fdt, const char *s, int *allocated)
+{
+	char *strtab = (char *)fdt + fdt_totalsize(fdt);
+	int strtabsize = fdt_size_dt_strings(fdt);
+	const char *p;
+
+	*allocated = 0;
+
+	p = fdt_find_string_(strtab - strtabsize, strtabsize, s);
+	if (p)
+		return p - strtab;
+
+	*allocated = 1;
+
+	return fdt_add_string_(fdt, s);
+}
+
+int fdt_property_placeholder(void *fdt, const char *name, int len, void **valp)
+{
+	struct fdt_property *prop;
+	int nameoff;
+	int allocated;
+
+	FDT_SW_PROBE_STRUCT(fdt);
+
+	/* String de-duplication can be slow, _NO_NAME_DEDUP skips it */
+	if (sw_flags(fdt) & FDT_CREATE_FLAG_NO_NAME_DEDUP) {
+		allocated = 1;
+		nameoff = fdt_add_string_(fdt, name);
+	} else {
+		nameoff = fdt_find_add_string_(fdt, name, &allocated);
+	}
+	if (nameoff == 0)
+		return -FDT_ERR_NOSPACE;
+
+	prop = fdt_grab_space_(fdt, sizeof(*prop) + FDT_TAGALIGN(len));
+	if (! prop) {
+		if (allocated)
+			fdt_del_last_string_(fdt, name);
+		return -FDT_ERR_NOSPACE;
+	}
+
+	prop->tag = cpu_to_fdt32(FDT_PROP);
+	prop->nameoff = cpu_to_fdt32(nameoff);
+	prop->len = cpu_to_fdt32(len);
+	*valp = prop->data;
+	return 0;
+}
+
+int fdt_property(void *fdt, const char *name, const void *val, int len)
+{
+	void *ptr;
+	int ret;
+
+	ret = fdt_property_placeholder(fdt, name, len, &ptr);
+	if (ret)
+		return ret;
+	memcpy(ptr, val, len);
+	return 0;
+}
+
+int fdt_finish(void *fdt)
+{
+	char *p = (char *)fdt;
+	fdt32_t *end;
+	int oldstroffset, newstroffset;
+	uint32_t tag;
+	int offset, nextoffset;
+
+	FDT_SW_PROBE_STRUCT(fdt);
+
+	/* Add terminator */
+	end = fdt_grab_space_(fdt, sizeof(*end));
+	if (! end)
+		return -FDT_ERR_NOSPACE;
+	*end = cpu_to_fdt32(FDT_END);
+
+	/* Relocate the string table */
+	oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt);
+	newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
+	memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt));
+	fdt_set_off_dt_strings(fdt, newstroffset);
+
+	/* Walk the structure, correcting string offsets */
+	offset = 0;
+	while ((tag = fdt_next_tag(fdt, offset, &nextoffset)) != FDT_END) {
+		if (tag == FDT_PROP) {
+			struct fdt_property *prop =
+				fdt_offset_ptr_w_(fdt, offset);
+			int nameoff;
+
+			nameoff = fdt32_to_cpu(prop->nameoff);
+			nameoff += fdt_size_dt_strings(fdt);
+			prop->nameoff = cpu_to_fdt32(nameoff);
+		}
+		offset = nextoffset;
+	}
+	if (nextoffset < 0)
+		return nextoffset;
+
+	/* Finally, adjust the header */
+	fdt_set_totalsize(fdt, newstroffset + fdt_size_dt_strings(fdt));
+
+	/* And fix up fields that were keeping intermediate state. */
+	fdt_set_last_comp_version(fdt, FDT_FIRST_SUPPORTED_VERSION);
+	fdt_set_magic(fdt, FDT_MAGIC);
+
+	return 0;
+}

lib/utils/libfdt/fdt_wip.c

@@ -0,0 +1,94 @@
+// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ */
+#include "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+int fdt_setprop_inplace_namelen_partial(void *fdt, int nodeoffset,
+					const char *name, int namelen,
+					uint32_t idx, const void *val,
+					int len)
+{
+	void *propval;
+	int proplen;
+
+	propval = fdt_getprop_namelen_w(fdt, nodeoffset, name, namelen,
+					&proplen);
+	if (!propval)
+		return proplen;
+
+	if (proplen < (len + idx))
+		return -FDT_ERR_NOSPACE;
+
+	memcpy((char *)propval + idx, val, len);
+	return 0;
+}
+
+int fdt_setprop_inplace(void *fdt, int nodeoffset, const char *name,
+			const void *val, int len)
+{
+	const void *propval;
+	int proplen;
+
+	propval = fdt_getprop(fdt, nodeoffset, name, &proplen);
+	if (!propval)
+		return proplen;
+
+	if (proplen != len)
+		return -FDT_ERR_NOSPACE;
+
+	return fdt_setprop_inplace_namelen_partial(fdt, nodeoffset, name,
+						   strlen(name), 0,
+						   val, len);
+}
+
+static void fdt_nop_region_(void *start, int len)
+{
+	fdt32_t *p;
+
+	for (p = start; (char *)p < ((char *)start + len); p++)
+		*p = cpu_to_fdt32(FDT_NOP);
+}
+
+int fdt_nop_property(void *fdt, int nodeoffset, const char *name)
+{
+	struct fdt_property *prop;
+	int len;
+
+	prop = fdt_get_property_w(fdt, nodeoffset, name, &len);
+	if (!prop)
+		return len;
+
+	fdt_nop_region_(prop, len + sizeof(*prop));
+
+	return 0;
+}
+
+int fdt_node_end_offset_(void *fdt, int offset)
+{
+	int depth = 0;
+
+	while ((offset >= 0) && (depth >= 0))
+		offset = fdt_next_node(fdt, offset, &depth);
+
+	return offset;
+}
+
+int fdt_nop_node(void *fdt, int nodeoffset)
+{
+	int endoffset;
+
+	endoffset = fdt_node_end_offset_(fdt, nodeoffset);
+	if (endoffset < 0)
+		return endoffset;
+
+	fdt_nop_region_(fdt_offset_ptr_w(fdt, nodeoffset, 0),
+			endoffset - nodeoffset);
+	return 0;
+}

lib/utils/libfdt/libfdt_env.h

@@ -0,0 +1,110 @@
+/* SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause) */
+#ifndef LIBFDT_ENV_H
+#define LIBFDT_ENV_H
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ * Copyright 2012 Kim Phillips, Freescale Semiconductor.
+ */
+
+#include <sbi/sbi_string.h>
+#include <sbi/sbi_types.h>
+
+#define INT_MAX		((int)(~0U >> 1))
+#define UINT_MAX	((unsigned int)~0U)
+#define INT32_MAX	INT_MAX
+#define UINT32_MAX	UINT_MAX
+
+#ifdef __CHECKER__
+#define FDT_FORCE __attribute__((force))
+#define FDT_BITWISE __attribute__((bitwise))
+#else
+#define FDT_FORCE
+#define FDT_BITWISE
+#endif
+
+#define memmove		sbi_memmove
+#define memcpy		sbi_memcpy
+#define memcmp		sbi_memcmp
+#define memchr		sbi_memchr
+#define memset		sbi_memset
+#define strchr		sbi_strchr
+#define strrchr		sbi_strrchr
+#define strcpy		sbi_strcpy
+#define strcmp		sbi_strcmp
+#define strncmp		sbi_strncmp
+#define strlen		sbi_strlen
+#define strnlen		sbi_strnlen
+
+typedef uint16_t FDT_BITWISE fdt16_t;
+typedef uint32_t FDT_BITWISE fdt32_t;
+typedef uint64_t FDT_BITWISE fdt64_t;
+
+#define EXTRACT_BYTE(x, n)	((unsigned long long)((uint8_t *)&x)[n])
+#define CPU_TO_FDT16(x) ((EXTRACT_BYTE(x, 0) << 8) | EXTRACT_BYTE(x, 1))
+#define CPU_TO_FDT32(x) ((EXTRACT_BYTE(x, 0) << 24) | (EXTRACT_BYTE(x, 1) << 16) | \
+			 (EXTRACT_BYTE(x, 2) << 8) | EXTRACT_BYTE(x, 3))
+#define CPU_TO_FDT64(x) ((EXTRACT_BYTE(x, 0) << 56) | (EXTRACT_BYTE(x, 1) << 48) | \
+			 (EXTRACT_BYTE(x, 2) << 40) | (EXTRACT_BYTE(x, 3) << 32) | \
+			 (EXTRACT_BYTE(x, 4) << 24) | (EXTRACT_BYTE(x, 5) << 16) | \
+			 (EXTRACT_BYTE(x, 6) << 8) | EXTRACT_BYTE(x, 7))
+
+static inline uint16_t fdt16_to_cpu(fdt16_t x)
+{
+	return (FDT_FORCE uint16_t)CPU_TO_FDT16(x);
+}
+static inline fdt16_t cpu_to_fdt16(uint16_t x)
+{
+	return (FDT_FORCE fdt16_t)CPU_TO_FDT16(x);
+}
+
+static inline uint32_t fdt32_to_cpu(fdt32_t x)
+{
+	return (FDT_FORCE uint32_t)CPU_TO_FDT32(x);
+}
+static inline fdt32_t cpu_to_fdt32(uint32_t x)
+{
+	return (FDT_FORCE fdt32_t)CPU_TO_FDT32(x);
+}
+
+static inline uint64_t fdt64_to_cpu(fdt64_t x)
+{
+	return (FDT_FORCE uint64_t)CPU_TO_FDT64(x);
+}
+static inline fdt64_t cpu_to_fdt64(uint64_t x)
+{
+	return (FDT_FORCE fdt64_t)CPU_TO_FDT64(x);
+}
+#undef CPU_TO_FDT64
+#undef CPU_TO_FDT32
+#undef CPU_TO_FDT16
+#undef EXTRACT_BYTE
+
+#ifdef __APPLE__
+#include <AvailabilityMacros.h>
+
+/* strnlen() is not available on Mac OS < 10.7 */
+# if !defined(MAC_OS_X_VERSION_10_7) || (MAC_OS_X_VERSION_MAX_ALLOWED < \
+                                         MAC_OS_X_VERSION_10_7)
+
+#define strnlen fdt_strnlen
+
+/*
+ * fdt_strnlen: returns the length of a string or max_count - which ever is
+ * smallest.
+ * Input 1 string: the string whose size is to be determined
+ * Input 2 max_count: the maximum value returned by this function
+ * Output: length of the string or max_count (the smallest of the two)
+ */
+static inline size_t fdt_strnlen(const char *string, size_t max_count)
+{
+    const char *p = memchr(string, 0, max_count);
+    return p ? p - string : max_count;
+}
+
+#endif /* !defined(MAC_OS_X_VERSION_10_7) || (MAC_OS_X_VERSION_MAX_ALLOWED <
+          MAC_OS_X_VERSION_10_7) */
+
+#endif /* __APPLE__ */
+
+#endif /* LIBFDT_ENV_H */

lib/utils/libfdt/libfdt_internal.h

@@ -0,0 +1,173 @@
+/* SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause) */
+#ifndef LIBFDT_INTERNAL_H
+#define LIBFDT_INTERNAL_H
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ */
+#include <fdt.h>
+
+#define FDT_ALIGN(x, a)		(((x) + (a) - 1) & ~((a) - 1))
+#define FDT_TAGALIGN(x)		(FDT_ALIGN((x), FDT_TAGSIZE))
+
+int32_t fdt_ro_probe_(const void *fdt);
+#define FDT_RO_PROBE(fdt)					\
+	{							\
+		int32_t totalsize_;				\
+		if ((totalsize_ = fdt_ro_probe_(fdt)) < 0)	\
+			return totalsize_;			\
+	}
+
+int fdt_check_node_offset_(const void *fdt, int offset);
+int fdt_check_prop_offset_(const void *fdt, int offset);
+const char *fdt_find_string_(const char *strtab, int tabsize, const char *s);
+int fdt_node_end_offset_(void *fdt, int nodeoffset);
+
+static inline const void *fdt_offset_ptr_(const void *fdt, int offset)
+{
+	return (const char *)fdt + fdt_off_dt_struct(fdt) + offset;
+}
+
+static inline void *fdt_offset_ptr_w_(void *fdt, int offset)
+{
+	return (void *)(uintptr_t)fdt_offset_ptr_(fdt, offset);
+}
+
+static inline const struct fdt_reserve_entry *fdt_mem_rsv_(const void *fdt, int n)
+{
+	const struct fdt_reserve_entry *rsv_table =
+		(const struct fdt_reserve_entry *)
+		((const char *)fdt + fdt_off_mem_rsvmap(fdt));
+
+	return rsv_table + n;
+}
+static inline struct fdt_reserve_entry *fdt_mem_rsv_w_(void *fdt, int n)
+{
+	return (void *)(uintptr_t)fdt_mem_rsv_(fdt, n);
+}
+
+#define FDT_SW_MAGIC		(~FDT_MAGIC)
+
+/**********************************************************************/
+/* Checking controls                                                  */
+/**********************************************************************/
+
+#ifndef FDT_ASSUME_MASK
+#define FDT_ASSUME_MASK 0
+#endif
+
+/*
+ * Defines assumptions which can be enabled. Each of these can be enabled
+ * individually. For maximum safety, don't enable any assumptions!
+ *
+ * For minimal code size and no safety, use ASSUME_PERFECT at your own risk.
+ * You should have another method of validating the device tree, such as a
+ * signature or hash check before using libfdt.
+ *
+ * For situations where security is not a concern it may be safe to enable
+ * ASSUME_SANE.
+ */
+enum {
+	/*
+	 * This does essentially no checks. Only the latest device-tree
+	 * version is correctly handled. Inconsistencies or errors in the device
+	 * tree may cause undefined behaviour or crashes. Invalid parameters
+	 * passed to libfdt may do the same.
+	 *
+	 * If an error occurs when modifying the tree it may leave the tree in
+	 * an intermediate (but valid) state. As an example, adding a property
+	 * where there is insufficient space may result in the property name
+	 * being added to the string table even though the property itself is
+	 * not added to the struct section.
+	 *
+	 * Only use this if you have a fully validated device tree with
+	 * the latest supported version and wish to minimise code size.
+	 */
+	ASSUME_PERFECT		= 0xff,
+
+	/*
+	 * This assumes that the device tree is sane. i.e. header metadata
+	 * and basic hierarchy are correct.
+	 *
+	 * With this assumption enabled, normal device trees produced by libfdt
+	 * and the compiler should be handled safely. Malicious device trees and
+	 * complete garbage may cause libfdt to behave badly or crash. Truncated
+	 * device trees (e.g. those only partially loaded) can also cause
+	 * problems.
+	 *
+	 * Note: Only checks that relate exclusively to the device tree itself
+	 * (not the parameters passed to libfdt) are disabled by this
+	 * assumption. This includes checking headers, tags and the like.
+	 */
+	ASSUME_VALID_DTB	= 1 << 0,
+
+	/*
+	 * This builds on ASSUME_VALID_DTB and further assumes that libfdt
+	 * functions are called with valid parameters, i.e. not trigger
+	 * FDT_ERR_BADOFFSET or offsets that are out of bounds. It disables any
+	 * extensive checking of parameters and the device tree, making various
+	 * assumptions about correctness.
+	 *
+	 * It doesn't make sense to enable this assumption unless
+	 * ASSUME_VALID_DTB is also enabled.
+	 */
+	ASSUME_VALID_INPUT	= 1 << 1,
+
+	/*
+	 * This disables checks for device-tree version and removes all code
+	 * which handles older versions.
+	 *
+	 * Only enable this if you know you have a device tree with the latest
+	 * version.
+	 */
+	ASSUME_LATEST		= 1 << 2,
+
+	/*
+	 * This assumes that it is OK for a failed addition to the device tree,
+	 * due to lack of space or some other problem, to skip any rollback
+	 * steps (such as dropping the property name from the string table).
+	 * This is safe to enable in most circumstances, even though it may
+	 * leave the tree in a sub-optimal state.
+	 */
+	ASSUME_NO_ROLLBACK	= 1 << 3,
+
+	/*
+	 * This assumes that the device tree components appear in a 'convenient'
+	 * order, i.e. the memory reservation block first, then the structure
+	 * block and finally the string block.
+	 *
+	 * This order is not specified by the device-tree specification,
+	 * but is expected by libfdt. The device-tree compiler always created
+	 * device trees with this order.
+	 *
+	 * This assumption disables a check in fdt_open_into() and removes the
+	 * ability to fix the problem there. This is safe if you know that the
+	 * device tree is correctly ordered. See fdt_blocks_misordered_().
+	 */
+	ASSUME_LIBFDT_ORDER	= 1 << 4,
+
+	/*
+	 * This assumes that libfdt itself does not have any internal bugs. It
+	 * drops certain checks that should never be needed unless libfdt has an
+	 * undiscovered bug.
+	 *
+	 * This can generally be considered safe to enable.
+	 */
+	ASSUME_LIBFDT_FLAWLESS	= 1 << 5,
+};
+
+/**
+ * can_assume_() - check if a particular assumption is enabled
+ *
+ * @mask: Mask to check (ASSUME_...)
+ * @return true if that assumption is enabled, else false
+ */
+static inline bool can_assume_(int mask)
+{
+	return FDT_ASSUME_MASK & mask;
+}
+
+/** helper macros for checking assumptions */
+#define can_assume(_assume)	can_assume_(ASSUME_ ## _assume)
+
+#endif /* LIBFDT_INTERNAL_H */

lib/utils/libfdt/objects.mk

@@ -0,0 +1,16 @@
+#
+# SPDX-License-Identifier: BSD-2-Clause
+#
+# Copyright (c) 2019 Western Digital Corporation or its affiliates.
+#
+# Authors:
+#   Atish Patra<atish.patra@wdc.com>
+#
+
+libfdt_files = fdt.o fdt_addresses.o fdt_check.o fdt_empty_tree.o fdt_ro.o fdt_rw.o \
+               fdt_strerror.o fdt_sw.o fdt_wip.o
+$(foreach file, $(libfdt_files), \
+        $(eval CFLAGS_$(file) = -I$(src)/../../utils/libfdt))
+
+libsbiutils-objs-y += $(addprefix libfdt/,$(libfdt_files))
+libsbiutils-genflags-y  += -I$(libsbiutils_dir)/libfdt/

lib/utils/libfdt/version.lds

@@ -0,0 +1,82 @@
+/* SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause) */
+LIBFDT_1.2 {
+	global:
+		fdt_next_node;
+		fdt_check_header;
+		fdt_move;
+		fdt_string;
+		fdt_num_mem_rsv;
+		fdt_get_mem_rsv;
+		fdt_subnode_offset_namelen;
+		fdt_subnode_offset;
+		fdt_path_offset_namelen;
+		fdt_path_offset;
+		fdt_get_name;
+		fdt_get_property_namelen;
+		fdt_get_property;
+		fdt_getprop_namelen;
+		fdt_getprop;
+		fdt_get_phandle;
+		fdt_get_alias_namelen;
+		fdt_get_alias;
+		fdt_get_path;
+                fdt_header_size;
+		fdt_supernode_atdepth_offset;
+		fdt_node_depth;
+		fdt_parent_offset;
+		fdt_node_offset_by_prop_value;
+		fdt_node_offset_by_phandle;
+		fdt_node_check_compatible;
+		fdt_node_offset_by_compatible;
+		fdt_setprop_inplace;
+		fdt_nop_property;
+		fdt_nop_node;
+		fdt_create;
+		fdt_add_reservemap_entry;
+		fdt_finish_reservemap;
+		fdt_begin_node;
+		fdt_property;
+		fdt_end_node;
+		fdt_finish;
+		fdt_open_into;
+		fdt_pack;
+		fdt_add_mem_rsv;
+		fdt_del_mem_rsv;
+		fdt_set_name;
+		fdt_setprop;
+		fdt_delprop;
+		fdt_add_subnode_namelen;
+		fdt_add_subnode;
+		fdt_del_node;
+		fdt_strerror;
+		fdt_offset_ptr;
+		fdt_next_tag;
+		fdt_appendprop;
+		fdt_create_empty_tree;
+		fdt_first_property_offset;
+		fdt_get_property_by_offset;
+		fdt_getprop_by_offset;
+		fdt_next_property_offset;
+		fdt_first_subnode;
+		fdt_next_subnode;
+		fdt_address_cells;
+		fdt_size_cells;
+		fdt_stringlist_contains;
+		fdt_stringlist_count;
+		fdt_stringlist_search;
+		fdt_stringlist_get;
+		fdt_resize;
+		fdt_overlay_apply;
+		fdt_get_string;
+		fdt_find_max_phandle;
+		fdt_generate_phandle;
+		fdt_check_full;
+		fdt_setprop_placeholder;
+		fdt_property_placeholder;
+		fdt_header_size_;
+		fdt_appendprop_addrrange;
+		fdt_setprop_inplace_namelen_partial;
+		fdt_create_with_flags;
+	local:
+		*;
+};

lib/utils/reset/fdt_reset.c

@@ -0,0 +1,50 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/sbi_scratch.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/reset/fdt_reset.h>
+
+extern struct fdt_reset fdt_reset_sifive;
+extern struct fdt_reset fdt_reset_htif;
+extern struct fdt_reset fdt_reset_thead;
+
+static struct fdt_reset *reset_drivers[] = {
+	&fdt_reset_sifive,
+	&fdt_reset_htif,
+	&fdt_reset_thead,
+};
+
+static struct fdt_reset *current_driver = NULL;
+
+int fdt_reset_init(void)
+{
+	int pos, noff, rc;
+	struct fdt_reset *drv;
+	const struct fdt_match *match;
+	void *fdt = sbi_scratch_thishart_arg1_ptr();
+
+	for (pos = 0; pos < array_size(reset_drivers); pos++) {
+		drv = reset_drivers[pos];
+
+		noff = fdt_find_match(fdt, -1, drv->match_table, &match);
+		if (noff < 0)
+			continue;
+
+		if (drv->init) {
+			rc = drv->init(fdt, noff, match);
+			if (rc)
+				return rc;
+		}
+		current_driver = drv;
+		break;
+	}
+
+	return 0;
+}

lib/utils/reset/fdt_reset_htif.c

@@ -0,0 +1,28 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi_utils/reset/fdt_reset.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/sys/htif.h>
+
+static int htif_reset_init(void *fdt, int nodeoff,
+			   const struct fdt_match *match)
+{
+	return htif_system_reset_init();
+}
+
+static const struct fdt_match htif_reset_match[] = {
+	{ .compatible = "ucb,htif0" },
+	{ },
+};
+
+struct fdt_reset fdt_reset_htif = {
+	.match_table = htif_reset_match,
+	.init = htif_reset_init
+};

lib/utils/reset/fdt_reset_sifive.c

@@ -0,0 +1,36 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/sbi_scratch.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/reset/fdt_reset.h>
+#include <sbi_utils/sys/sifive_test.h>
+
+static int sifive_test_reset_init(void *fdt, int nodeoff,
+				  const struct fdt_match *match)
+{
+	int rc;
+	unsigned long addr;
+
+	rc = fdt_get_node_addr_size(fdt, nodeoff, &addr, NULL);
+	if (rc)
+		return rc;
+
+	return sifive_test_init(addr);
+}
+
+static const struct fdt_match sifive_test_reset_match[] = {
+	{ .compatible = "sifive,test1" },
+	{ },
+};
+
+struct fdt_reset fdt_reset_sifive = {
+	.match_table = sifive_test_reset_match,
+	.init = sifive_test_reset_init,
+};

lib/utils/reset/fdt_reset_thead.c

@@ -0,0 +1,140 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ */
+
+#include <libfdt.h>
+#include <sbi/riscv_io.h>
+#include <sbi/sbi_bitops.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi/sbi_system.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/reset/fdt_reset.h>
+
+#include "fdt_reset_thead.h"
+
+struct custom_csr custom_csr[MAX_CUSTOM_CSR];
+
+#define CSR_OPCODE 0x39073
+static void clone_csrs(int cnt)
+{
+	unsigned long i;
+
+	for (i = 0; i < cnt; i++) {
+		/* Write csr BIT[31 - 20] to stub */
+		__reset_thead_csr_stub[3*i + 1] =
+				CSR_OPCODE | (custom_csr[i].index << 20);
+
+		/* Mask csr BIT[31 - 20] */
+		*(u32 *)&__fdt_reset_thead_csrr &= BIT(20) - 1;
+		smp_mb();
+
+		/* Write csr BIT[31 - 20] to __fdt_reset_thead_csrr */
+		*(u32 *)&__fdt_reset_thead_csrr |= custom_csr[i].index << 20;
+		smp_mb();
+
+		RISCV_FENCE_I;
+
+		custom_csr[i].value = __fdt_reset_thead_csrr();
+	}
+}
+
+static int thead_system_reset_check(u32 type, u32 reason)
+{
+	return 1;
+}
+
+static void thead_system_reset(u32 type, u32 reason)
+{
+	ebreak();
+}
+
+static struct sbi_system_reset_device thead_reset = {
+	.name = "thead_reset",
+	.system_reset_check = thead_system_reset_check,
+	.system_reset = thead_system_reset
+};
+
+extern void __thead_pre_start_warm(void);
+static int thead_reset_init(void *fdt, int nodeoff,
+				 const struct fdt_match *match)
+{
+	void *p;
+	const fdt64_t *val;
+	const fdt32_t *val_w;
+	int len, i, cnt;
+	u32 t, tmp = 0;
+
+	/* Prepare clone csrs */
+	val_w = fdt_getprop(fdt, nodeoff, "csr-copy", &len);
+	if (len > 0 && val_w) {
+		cnt = len / sizeof(fdt32_t);
+
+		if (cnt > MAX_CUSTOM_CSR)
+			sbi_hart_hang();
+
+		for (i = 0; i < cnt; i++) {
+			custom_csr[i].index = fdt32_to_cpu(val_w[i]);
+		}
+	}
+
+	if (cnt)
+		clone_csrs(cnt);
+
+	/* Delegate plic enable regs for S-mode */
+	val = fdt_getprop(fdt, nodeoff, "plic-delegate", &len);
+	if (len > 0 && val) {
+		p = (void *)(ulong)fdt64_to_cpu(*val);
+		writel(BIT(0), p);
+	}
+
+	/* Old reset method for secondary harts */
+	if (fdt_getprop(fdt, nodeoff, "using-csr-reset", &len)) {
+		csr_write(0x7c7, (ulong)&__thead_pre_start_warm);
+		csr_write(0x7c6, -1);
+	}
+
+	/* Custom reset method for secondary harts */
+	val = fdt_getprop(fdt, nodeoff, "entry-reg", &len);
+	if (len > 0 && val) {
+		p = (void *)(ulong)fdt64_to_cpu(*val);
+
+		val_w = fdt_getprop(fdt, nodeoff, "entry-cnt", &len);
+		if (len > 0 && val_w) {
+			tmp = fdt32_to_cpu(*val_w);
+
+			for (i = 0; i < tmp; i++) {
+				t = (ulong)&__thead_pre_start_warm;
+				writel(t, p + (8 * i));
+				t = (u64)(ulong)&__thead_pre_start_warm >> 32;
+				writel(t, p + (8 * i) + 4);
+			}
+		}
+
+		val = fdt_getprop(fdt, nodeoff, "control-reg", &len);
+		if (len > 0 && val) {
+			p = (void *)(ulong)fdt64_to_cpu(*val);
+
+			val_w = fdt_getprop(fdt, nodeoff, "control-val", &len);
+			if (len > 0 && val_w) {
+				tmp = fdt32_to_cpu(*val_w);
+				tmp |= readl(p);
+				writel(tmp, p);
+			}
+		}
+	}
+
+	sbi_system_reset_set_device(&thead_reset);
+
+	return 0;
+}
+
+static const struct fdt_match thead_reset_match[] = {
+	{ .compatible = "thead,reset-sample" },
+	{ },
+};
+
+struct fdt_reset fdt_reset_thead = {
+	.match_table = thead_reset_match,
+	.init = thead_reset_init
+};

lib/utils/reset/fdt_reset_thead.h

@@ -0,0 +1,23 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ */
+
+#ifndef __FDT_RESET_THEAD_H__
+#define __FDT_RESET_THEAD_H__
+
+#define MAX_CUSTOM_CSR	32
+
+#ifndef __ASSEMBLER__
+struct custom_csr {
+	unsigned long index;
+	unsigned long value;
+};
+
+u64  __fdt_reset_thead_csrr(void);
+
+extern struct custom_csr custom_csr[MAX_CUSTOM_CSR];
+extern u32 __reset_thead_csr_stub[];
+
+#endif /* __ASSEMBLER__ */
+
+#endif /* __FDT_RESET_THEAD_H__ */

lib/utils/reset/fdt_reset_thead_asm.S

@@ -0,0 +1,47 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ */
+
+#include <sbi/riscv_asm.h>
+#include "fdt_reset_thead.h"
+
+/*
+ * csrrs rd, csr, rs1
+ * |31   20|19   15|14   12|11  7|6       0|
+ *    csr     rs1     010     rd   1110011
+ */
+#define CSR_STUB	addi    x0, x0, 0
+
+	.option norvc
+	.align 3
+	.global __fdt_reset_thead_csrr
+__fdt_reset_thead_csrr:
+	csrrs a0, 0, x0
+	ret
+
+	.align 3
+	.global __thead_pre_start_warm
+__thead_pre_start_warm:
+	/*
+	 * Clear L1 cache & BTB & BHT ...
+	 */
+	li	t1, 0x70013
+	csrw	0x7c2, t1
+	fence rw,rw
+
+	lla	t1, custom_csr
+
+	.global __reset_thead_csr_stub
+__reset_thead_csr_stub:
+.rept	MAX_CUSTOM_CSR
+	REG_L	t2, 8(t1)
+	CSR_STUB
+	addi	t1, t1, 16
+.endr
+	/*
+	 * Clear L1 cache & BTB & BHT ...
+	 */
+	li	t1, 0x70013
+	csrw	0x7c2, t1
+	fence rw,rw
+	j _start_warm

lib/utils/reset/objects.mk

@@ -0,0 +1,14 @@
+#
+# SPDX-License-Identifier: BSD-2-Clause
+#
+# Copyright (c) 2020 Western Digital Corporation or its affiliates.
+#
+# Authors:
+#   Anup Patel <anup.patel@wdc.com>
+#
+
+libsbiutils-objs-y += reset/fdt_reset.o
+libsbiutils-objs-y += reset/fdt_reset_htif.o
+libsbiutils-objs-y += reset/fdt_reset_sifive.o
+libsbiutils-objs-y += reset/fdt_reset_thead.o
+libsbiutils-objs-y += reset/fdt_reset_thead_asm.o

lib/utils/serial/fdt_serial.c

@@ -0,0 +1,90 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <libfdt.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/serial/fdt_serial.h>
+
+extern struct fdt_serial fdt_serial_uart8250;
+extern struct fdt_serial fdt_serial_sifive;
+extern struct fdt_serial fdt_serial_htif;
+extern struct fdt_serial fdt_serial_shakti;
+
+static struct fdt_serial *serial_drivers[] = {
+	&fdt_serial_uart8250,
+	&fdt_serial_sifive,
+	&fdt_serial_htif,
+	&fdt_serial_shakti,
+};
+
+static struct fdt_serial dummy = {
+	.match_table = NULL,
+	.init = NULL,
+};
+
+static struct fdt_serial *current_driver = &dummy;
+
+int fdt_serial_init(void)
+{
+	const void *prop;
+	struct fdt_serial *drv;
+	const struct fdt_match *match;
+	int pos, noff = -1, len, coff, rc;
+	void *fdt = sbi_scratch_thishart_arg1_ptr();
+
+	/* Find offset of node pointed by stdout-path */
+	coff = fdt_path_offset(fdt, "/chosen");
+	if (-1 < coff) {
+		prop = fdt_getprop(fdt, coff, "stdout-path", &len);
+		if (prop && len)
+			noff = fdt_path_offset(fdt, prop);
+	}
+
+	/* First check DT node pointed by stdout-path */
+	for (pos = 0; pos < array_size(serial_drivers) && -1 < noff; pos++) {
+		drv = serial_drivers[pos];
+
+		match = fdt_match_node(fdt, noff, drv->match_table);
+		if (!match)
+			continue;
+
+		if (drv->init) {
+			rc = drv->init(fdt, noff, match);
+			if (rc)
+				return rc;
+		}
+		current_driver = drv;
+		break;
+	}
+
+	/* Check if we found desired driver */
+	if (current_driver != &dummy)
+		goto done;
+
+	/* Lastly check all DT nodes */
+	for (pos = 0; pos < array_size(serial_drivers); pos++) {
+		drv = serial_drivers[pos];
+
+		noff = fdt_find_match(fdt, -1, drv->match_table, &match);
+		if (noff < 0)
+			continue;
+
+		if (drv->init) {
+			rc = drv->init(fdt, noff, match);
+			if (rc)
+				return rc;
+		}
+		current_driver = drv;
+		break;
+	}
+
+done:
+	return 0;
+}

lib/utils/serial/fdt_serial_htif.c

@@ -0,0 +1,28 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/serial/fdt_serial.h>
+#include <sbi_utils/sys/htif.h>
+
+static const struct fdt_match serial_htif_match[] = {
+	{ .compatible = "ucb,htif0" },
+	{ },
+};
+
+static int serial_htif_init(void *fdt, int nodeoff,
+			    const struct fdt_match *match)
+{
+	return htif_serial_init();
+}
+
+struct fdt_serial fdt_serial_htif = {
+	.match_table = serial_htif_match,
+	.init = serial_htif_init
+};

lib/utils/serial/fdt_serial_shakti.c

@@ -0,0 +1,33 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Vijai Kumar K <vijai@behindbytes.com>
+ *
+ */
+
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/serial/fdt_serial.h>
+#include <sbi_utils/serial/shakti-uart.h>
+
+static int serial_shakti_init(void *fdt, int nodeoff,
+				const struct fdt_match *match)
+{
+	int rc;
+	struct platform_uart_data uart;
+
+	rc = fdt_parse_shakti_uart_node(fdt, nodeoff, &uart);
+	if (rc)
+		return rc;
+
+	return shakti_uart_init(uart.addr, uart.freq, uart.baud);
+}
+
+static const struct fdt_match serial_shakti_match[] = {
+	{ .compatible = "shakti,uart0" },
+	{ },
+};
+
+struct fdt_serial fdt_serial_shakti = {
+	.match_table = serial_shakti_match,
+	.init = serial_shakti_init
+};

lib/utils/serial/fdt_serial_sifive.c

@@ -0,0 +1,36 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/serial/fdt_serial.h>
+#include <sbi_utils/serial/sifive-uart.h>
+
+static int serial_sifive_init(void *fdt, int nodeoff,
+				const struct fdt_match *match)
+{
+	int rc;
+	struct platform_uart_data uart;
+
+	rc = fdt_parse_sifive_uart_node(fdt, nodeoff, &uart);
+	if (rc)
+		return rc;
+
+	return sifive_uart_init(uart.addr, uart.freq, uart.baud);
+}
+
+static const struct fdt_match serial_sifive_match[] = {
+	{ .compatible = "sifive,fu540-c000-uart" },
+	{ .compatible = "sifive,uart0" },
+	{ },
+};
+
+struct fdt_serial fdt_serial_sifive = {
+	.match_table = serial_sifive_match,
+	.init = serial_sifive_init
+};

lib/utils/serial/fdt_serial_uart8250.c

@@ -0,0 +1,37 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/serial/fdt_serial.h>
+#include <sbi_utils/serial/uart8250.h>
+
+static int serial_uart8250_init(void *fdt, int nodeoff,
+				const struct fdt_match *match)
+{
+	int rc;
+	struct platform_uart_data uart;
+
+	rc = fdt_parse_uart8250_node(fdt, nodeoff, &uart);
+	if (rc)
+		return rc;
+
+	return uart8250_init(uart.addr, uart.freq, uart.baud,
+			     uart.reg_shift, uart.reg_io_width);
+}
+
+static const struct fdt_match serial_uart8250_match[] = {
+	{ .compatible = "ns16550" },
+	{ .compatible = "ns16550a" },
+	{ },
+};
+
+struct fdt_serial fdt_serial_uart8250 = {
+	.match_table = serial_uart8250_match,
+	.init = serial_uart8250_init,
+};

lib/utils/serial/objects.mk

@@ -0,0 +1,17 @@
+#
+# SPDX-License-Identifier: BSD-2-Clause
+#
+# Copyright (c) 2019 Western Digital Corporation or its affiliates.
+#
+# Authors:
+#   Anup Patel <anup.patel@wdc.com>
+#
+
+libsbiutils-objs-y += serial/fdt_serial.o
+libsbiutils-objs-y += serial/fdt_serial_htif.o
+libsbiutils-objs-y += serial/fdt_serial_shakti.o
+libsbiutils-objs-y += serial/fdt_serial_sifive.o
+libsbiutils-objs-y += serial/fdt_serial_uart8250.o
+libsbiutils-objs-y += serial/shakti-uart.o
+libsbiutils-objs-y += serial/sifive-uart.o
+libsbiutils-objs-y += serial/uart8250.o

lib/utils/serial/shakti-uart.c

@@ -0,0 +1,56 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Vijai Kumar K <vijai@behindbytes.com>
+ */
+
+#include <sbi/riscv_io.h>
+#include <sbi/sbi_console.h>
+#include <sbi_utils/serial/shakti-uart.h>
+
+#define REG_BAUD	0x00
+#define REG_TX		0x04
+#define REG_RX		0x08
+#define REG_STATUS	0x0C
+#define REG_DELAY	0x10
+#define REG_CONTROL	0x14
+#define REG_INT_EN	0x18
+#define REG_IQ_CYCLES	0x1C
+#define REG_RX_THRES	0x20
+
+#define UART_TX_FULL  0x2
+#define UART_RX_FULL  0x8
+
+static volatile void *uart_base;
+
+static void shakti_uart_putc(char ch)
+{
+	while((readw(uart_base + REG_STATUS) & UART_TX_FULL))
+		;
+	writeb(ch, uart_base + REG_TX);
+}
+
+static int shakti_uart_getc(void)
+{
+	u16 status = readw(uart_base + REG_STATUS);
+	if (status & UART_RX_FULL)
+		return readb(uart_base + REG_RX);
+	return -1;
+}
+
+static struct sbi_console_device shakti_console = {
+	.name = "shakti_uart",
+	.console_putc = shakti_uart_putc,
+	.console_getc = shakti_uart_getc
+};
+
+int shakti_uart_init(unsigned long base, u32 in_freq, u32 baudrate)
+{
+	uart_base = (volatile void *)base;
+	u16 baud = (u16)(in_freq/(16 * baudrate));
+	writew(baud, uart_base + REG_BAUD);
+
+	sbi_console_set_device(&shakti_console);
+
+	return 0;
+}

lib/utils/serial/sifive-uart.c

@@ -0,0 +1,110 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_io.h>
+#include <sbi/sbi_console.h>
+#include <sbi_utils/serial/sifive-uart.h>
+
+/* clang-format off */
+
+#define UART_REG_TXFIFO		0
+#define UART_REG_RXFIFO		1
+#define UART_REG_TXCTRL		2
+#define UART_REG_RXCTRL		3
+#define UART_REG_IE		4
+#define UART_REG_IP		5
+#define UART_REG_DIV		6
+
+#define UART_TXFIFO_FULL	0x80000000
+#define UART_RXFIFO_EMPTY	0x80000000
+#define UART_RXFIFO_DATA	0x000000ff
+#define UART_TXCTRL_TXEN	0x1
+#define UART_RXCTRL_RXEN	0x1
+
+/* clang-format on */
+
+static volatile void *uart_base;
+static u32 uart_in_freq;
+static u32 uart_baudrate;
+
+/**
+ * Find minimum divisor divides in_freq to max_target_hz;
+ * Based on uart driver n SiFive FSBL.
+ *
+ * f_baud = f_in / (div + 1) => div = (f_in / f_baud) - 1
+ * The nearest integer solution requires rounding up as to not exceed max_target_hz.
+ * div  = ceil(f_in / f_baud) - 1
+ *	= floor((f_in - 1 + f_baud) / f_baud) - 1
+ * This should not overflow as long as (f_in - 1 + f_baud) does not exceed
+ * 2^32 - 1, which is unlikely since we represent frequencies in kHz.
+ */
+static inline unsigned int uart_min_clk_divisor(uint64_t in_freq,
+						uint64_t max_target_hz)
+{
+	uint64_t quotient = (in_freq + max_target_hz - 1) / (max_target_hz);
+	/* Avoid underflow */
+	if (quotient == 0) {
+		return 0;
+	} else {
+		return quotient - 1;
+	}
+}
+
+static u32 get_reg(u32 num)
+{
+	return readl(uart_base + (num * 0x4));
+}
+
+static void set_reg(u32 num, u32 val)
+{
+	writel(val, uart_base + (num * 0x4));
+}
+
+static void sifive_uart_putc(char ch)
+{
+	while (get_reg(UART_REG_TXFIFO) & UART_TXFIFO_FULL)
+		;
+
+	set_reg(UART_REG_TXFIFO, ch);
+}
+
+static int sifive_uart_getc(void)
+{
+	u32 ret = get_reg(UART_REG_RXFIFO);
+	if (!(ret & UART_RXFIFO_EMPTY))
+		return ret & UART_RXFIFO_DATA;
+	return -1;
+}
+
+static struct sbi_console_device sifive_console = {
+	.name = "sifive_uart",
+	.console_putc = sifive_uart_putc,
+	.console_getc = sifive_uart_getc
+};
+
+int sifive_uart_init(unsigned long base, u32 in_freq, u32 baudrate)
+{
+	uart_base     = (volatile void *)base;
+	uart_in_freq  = in_freq;
+	uart_baudrate = baudrate;
+
+	/* Configure baudrate */
+	if (in_freq)
+		set_reg(UART_REG_DIV, uart_min_clk_divisor(in_freq, baudrate));
+	/* Disable interrupts */
+	set_reg(UART_REG_IE, 0);
+	/* Enable TX */
+	set_reg(UART_REG_TXCTRL, UART_TXCTRL_TXEN);
+	/* Enable Rx */
+	set_reg(UART_REG_RXCTRL, UART_RXCTRL_RXEN);
+
+	sbi_console_set_device(&sifive_console);
+
+	return 0;
+}

lib/utils/serial/uart8250.c

@@ -0,0 +1,134 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_io.h>
+#include <sbi/sbi_console.h>
+#include <sbi_utils/serial/uart8250.h>
+
+/* clang-format off */
+
+#define UART_RBR_OFFSET		0	/* In:  Recieve Buffer Register */
+#define UART_THR_OFFSET		0	/* Out: Transmitter Holding Register */
+#define UART_DLL_OFFSET		0	/* Out: Divisor Latch Low */
+#define UART_IER_OFFSET		1	/* I/O: Interrupt Enable Register */
+#define UART_DLM_OFFSET		1	/* Out: Divisor Latch High */
+#define UART_FCR_OFFSET		2	/* Out: FIFO Control Register */
+#define UART_IIR_OFFSET		2	/* I/O: Interrupt Identification Register */
+#define UART_LCR_OFFSET		3	/* Out: Line Control Register */
+#define UART_MCR_OFFSET		4	/* Out: Modem Control Register */
+#define UART_LSR_OFFSET		5	/* In:  Line Status Register */
+#define UART_MSR_OFFSET		6	/* In:  Modem Status Register */
+#define UART_SCR_OFFSET		7	/* I/O: Scratch Register */
+#define UART_MDR1_OFFSET	8	/* I/O:  Mode Register */
+
+#define UART_LSR_FIFOE		0x80	/* Fifo error */
+#define UART_LSR_TEMT		0x40	/* Transmitter empty */
+#define UART_LSR_THRE		0x20	/* Transmit-hold-register empty */
+#define UART_LSR_BI		0x10	/* Break interrupt indicator */
+#define UART_LSR_FE		0x08	/* Frame error indicator */
+#define UART_LSR_PE		0x04	/* Parity error indicator */
+#define UART_LSR_OE		0x02	/* Overrun error indicator */
+#define UART_LSR_DR		0x01	/* Receiver data ready */
+#define UART_LSR_BRK_ERROR_BITS	0x1E	/* BI, FE, PE, OE bits */
+
+/* clang-format on */
+
+static volatile void *uart8250_base;
+static u32 uart8250_in_freq;
+static u32 uart8250_baudrate;
+static u32 uart8250_reg_width;
+static u32 uart8250_reg_shift;
+
+static u32 get_reg(u32 num)
+{
+	u32 offset = num << uart8250_reg_shift;
+
+	if (uart8250_reg_width == 1)
+		return readb(uart8250_base + offset);
+	else if (uart8250_reg_width == 2)
+		return readw(uart8250_base + offset);
+	else
+		return readl(uart8250_base + offset);
+}
+
+static void set_reg(u32 num, u32 val)
+{
+	u32 offset = num << uart8250_reg_shift;
+
+	if (uart8250_reg_width == 1)
+		writeb(val, uart8250_base + offset);
+	else if (uart8250_reg_width == 2)
+		writew(val, uart8250_base + offset);
+	else
+		writel(val, uart8250_base + offset);
+}
+
+static void uart8250_putc(char ch)
+{
+	while ((get_reg(UART_LSR_OFFSET) & UART_LSR_THRE) == 0)
+		;
+
+	set_reg(UART_THR_OFFSET, ch);
+}
+
+static int uart8250_getc(void)
+{
+	if (get_reg(UART_LSR_OFFSET) & UART_LSR_DR)
+		return get_reg(UART_RBR_OFFSET);
+	return -1;
+}
+
+static struct sbi_console_device uart8250_console = {
+	.name = "uart8250",
+	.console_putc = uart8250_putc,
+	.console_getc = uart8250_getc
+};
+
+int uart8250_init(unsigned long base, u32 in_freq, u32 baudrate, u32 reg_shift,
+		  u32 reg_width)
+{
+	u16 bdiv;
+
+	uart8250_base      = (volatile void *)base;
+	uart8250_reg_shift = reg_shift;
+	uart8250_reg_width = reg_width;
+	uart8250_in_freq   = in_freq;
+	uart8250_baudrate  = baudrate;
+
+	bdiv = uart8250_in_freq / (16 * uart8250_baudrate);
+
+	/* Disable all interrupts */
+	set_reg(UART_IER_OFFSET, 0x00);
+	/* Enable DLAB */
+	set_reg(UART_LCR_OFFSET, 0x80);
+
+	if (bdiv) {
+		/* Set divisor low byte */
+		set_reg(UART_DLL_OFFSET, bdiv & 0xff);
+		/* Set divisor high byte */
+		set_reg(UART_DLM_OFFSET, (bdiv >> 8) & 0xff);
+	}
+
+	/* 8 bits, no parity, one stop bit */
+	set_reg(UART_LCR_OFFSET, 0x03);
+	/* Enable FIFO */
+	set_reg(UART_FCR_OFFSET, 0x01);
+	/* No modem control DTR RTS */
+	set_reg(UART_MCR_OFFSET, 0x00);
+	/* Clear line status */
+	get_reg(UART_LSR_OFFSET);
+	/* Read receive buffer */
+	get_reg(UART_RBR_OFFSET);
+	/* Set scratchpad */
+	set_reg(UART_SCR_OFFSET, 0x00);
+
+	sbi_console_set_device(&uart8250_console);
+
+	return 0;
+}

lib/utils/sys/clint.c

@@ -0,0 +1,256 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_atomic.h>
+#include <sbi/riscv_io.h>
+#include <sbi/sbi_domain.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_hartmask.h>
+#include <sbi/sbi_ipi.h>
+#include <sbi/sbi_timer.h>
+#include <sbi_utils/sys/clint.h>
+
+#define CLINT_IPI_OFF		0
+#define CLINT_IPI_SIZE		0x4000
+
+#define CLINT_TIME_CMP_OFF	0x4000
+#define CLINT_TIME_CMP_SIZE	0x4000
+
+#define CLINT_TIME_VAL_OFF	0xbff8
+#define CLINT_TIME_VAL_SIZE	0x4000
+
+static struct clint_data *clint_ipi_hartid2data[SBI_HARTMASK_MAX_BITS];
+
+static void clint_ipi_send(u32 target_hart)
+{
+	struct clint_data *clint;
+
+	if (SBI_HARTMASK_MAX_BITS <= target_hart)
+		return;
+	clint = clint_ipi_hartid2data[target_hart];
+	if (!clint)
+		return;
+
+	/* Set CLINT IPI */
+	writel(1, &clint->ipi[target_hart - clint->first_hartid]);
+}
+
+static void clint_ipi_clear(u32 target_hart)
+{
+	struct clint_data *clint;
+
+	if (SBI_HARTMASK_MAX_BITS <= target_hart)
+		return;
+	clint = clint_ipi_hartid2data[target_hart];
+	if (!clint)
+		return;
+
+	/* Clear CLINT IPI */
+	writel(0, &clint->ipi[target_hart - clint->first_hartid]);
+}
+
+static struct sbi_ipi_device clint_ipi = {
+	.name = "clint",
+	.ipi_send = clint_ipi_send,
+	.ipi_clear = clint_ipi_clear
+};
+
+int clint_warm_ipi_init(void)
+{
+	/* Clear CLINT IPI for current HART */
+	clint_ipi_clear(current_hartid());
+
+	return 0;
+}
+
+int clint_cold_ipi_init(struct clint_data *clint)
+{
+	u32 i;
+	int rc;
+	struct sbi_domain_memregion reg;
+
+	if (!clint)
+		return SBI_EINVAL;
+
+	/* Initialize private data */
+	clint->ipi = (void *)clint->addr;
+
+	/* Update IPI hartid table */
+	for (i = 0; i < clint->hart_count; i++)
+		clint_ipi_hartid2data[clint->first_hartid + i] = clint;
+
+	/* Add CLINT ipi region to the root domain */
+	sbi_domain_memregion_init(clint->addr + CLINT_IPI_OFF,
+				  CLINT_IPI_SIZE,
+				  SBI_DOMAIN_MEMREGION_MMIO, &reg);
+	rc = sbi_domain_root_add_memregion(&reg);
+	if (rc)
+		return rc;
+
+	sbi_ipi_set_device(&clint_ipi);
+
+	return 0;
+}
+
+static struct clint_data *clint_timer_hartid2data[SBI_HARTMASK_MAX_BITS];
+
+#if __riscv_xlen != 32
+static u64 clint_time_rd64(volatile u64 *addr)
+{
+	return readq_relaxed(addr);
+}
+
+static void clint_time_wr64(u64 value, volatile u64 *addr)
+{
+	writeq_relaxed(value, addr);
+}
+#endif
+
+static u64 clint_time_rd32(volatile u64 *addr)
+{
+	u32 lo, hi;
+
+	do {
+		hi = readl_relaxed((u32 *)addr + 1);
+		lo = readl_relaxed((u32 *)addr);
+	} while (hi != readl_relaxed((u32 *)addr + 1));
+
+	return ((u64)hi << 32) | (u64)lo;
+}
+
+static void clint_time_wr32(u64 value, volatile u64 *addr)
+{
+	u32 mask = -1U;
+
+	writel_relaxed(value & mask, (void *)(addr));
+	writel_relaxed(value >> 32, (void *)(addr) + 0x04);
+}
+
+static u64 clint_timer_value(void)
+{
+	struct clint_data *clint = clint_timer_hartid2data[current_hartid()];
+
+	/* Read CLINT Time Value */
+	return clint->time_rd(clint->time_val) + clint->time_delta;
+}
+
+static void clint_timer_event_stop(void)
+{
+	u32 target_hart = current_hartid();
+	struct clint_data *clint = clint_timer_hartid2data[target_hart];
+
+	/* Clear CLINT Time Compare */
+	clint->time_wr(-1ULL,
+			&clint->time_cmp[target_hart - clint->first_hartid]);
+}
+
+static void clint_timer_event_start(u64 next_event)
+{
+	u32 target_hart = current_hartid();
+	struct clint_data *clint = clint_timer_hartid2data[target_hart];
+
+	/* Program CLINT Time Compare */
+	clint->time_wr(next_event - clint->time_delta,
+		       &clint->time_cmp[target_hart - clint->first_hartid]);
+}
+
+static struct sbi_timer_device clint_timer = {
+	.name = "clint",
+	.timer_value = clint_timer_value,
+	.timer_event_start = clint_timer_event_start,
+	.timer_event_stop = clint_timer_event_stop
+};
+
+int clint_warm_timer_init(void)
+{
+	u64 v1, v2, mv;
+	u32 target_hart = current_hartid();
+	struct clint_data *reference;
+	struct clint_data *clint = clint_timer_hartid2data[target_hart];
+
+	if (!clint)
+		return SBI_ENODEV;
+
+	/*
+	 * Compute delta if reference available
+	 *
+	 * We deliberately compute time_delta in warm init so that time_delta
+	 * is computed on a HART which is going to use given CLINT. We use
+	 * atomic flag timer_delta_computed to ensure that only one HART does
+	 * time_delta computation.
+	 */
+	if (clint->time_delta_reference) {
+		reference = clint->time_delta_reference;
+		if (!atomic_raw_xchg_ulong(&clint->time_delta_computed, 1)) {
+			v1 = clint->time_rd(clint->time_val);
+			mv = reference->time_rd(reference->time_val);
+			v2 = clint->time_rd(clint->time_val);
+			clint->time_delta = mv - ((v1 / 2) + (v2 / 2));
+		}
+	}
+
+	/* Clear CLINT Time Compare */
+	clint->time_wr(-1ULL,
+		       &clint->time_cmp[target_hart - clint->first_hartid]);
+
+	return 0;
+}
+
+int clint_cold_timer_init(struct clint_data *clint,
+			  struct clint_data *reference)
+{
+	u32 i;
+	int rc;
+	struct sbi_domain_memregion reg;
+
+	if (!clint)
+		return SBI_EINVAL;
+
+	/* Initialize private data */
+	clint->time_delta_reference = reference;
+	clint->time_delta_computed = 0;
+	clint->time_delta = 0;
+	clint->time_val = (u64 *)((void *)clint->addr + CLINT_TIME_VAL_OFF);
+	clint->time_cmp = (u64 *)((void *)clint->addr + CLINT_TIME_CMP_OFF);
+	clint->time_rd = clint_time_rd32;
+	clint->time_wr = clint_time_wr32;
+
+	/* Override read/write accessors for 64bit MMIO */
+#if __riscv_xlen != 32
+	if (clint->has_64bit_mmio) {
+		clint->time_rd = clint_time_rd64;
+		clint->time_wr = clint_time_wr64;
+	}
+#endif
+
+	/* Update timer hartid table */
+	for (i = 0; i < clint->hart_count; i++)
+		clint_timer_hartid2data[clint->first_hartid + i] = clint;
+
+	/* Add CLINT mtime region to the root domain */
+	sbi_domain_memregion_init(clint->addr + CLINT_TIME_VAL_OFF,
+				  CLINT_TIME_VAL_SIZE,
+				  SBI_DOMAIN_MEMREGION_MMIO, &reg);
+	rc = sbi_domain_root_add_memregion(&reg);
+	if (rc)
+		return rc;
+
+	/* Add CLINT timecmp region to the root domain */
+	sbi_domain_memregion_init(clint->addr + CLINT_TIME_CMP_OFF,
+				  CLINT_TIME_CMP_SIZE,
+				  SBI_DOMAIN_MEMREGION_MMIO, &reg);
+	rc = sbi_domain_root_add_memregion(&reg);
+	if (rc)
+		return rc;
+
+	sbi_timer_set_device(&clint_timer);
+
+	return 0;
+}

lib/utils/sys/htif.c

@@ -0,0 +1,182 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * Copyright (c) 2010-2020, The Regents of the University of California
+ * (Regents).  All Rights Reserved.
+ */
+
+#include <sbi/riscv_locks.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_system.h>
+#include <sbi_utils/sys/htif.h>
+
+#define HTIF_DATA_BITS		48
+#define HTIF_DATA_MASK		((1ULL << HTIF_DATA_BITS) - 1)
+#define HTIF_DATA_SHIFT		0
+#define HTIF_CMD_BITS		8
+#define HTIF_CMD_MASK		((1ULL << HTIF_CMD_BITS) - 1)
+#define HTIF_CMD_SHIFT		48
+#define HTIF_DEV_BITS		8
+#define HTIF_DEV_MASK		((1ULL << HTIF_DEV_BITS) - 1)
+#define HTIF_DEV_SHIFT		56
+
+#define HTIF_DEV_SYSTEM		0
+#define HTIF_DEV_CONSOLE	1
+
+#define HTIF_CONSOLE_CMD_GETC	0
+#define HTIF_CONSOLE_CMD_PUTC	1
+
+#if __riscv_xlen == 64
+# define TOHOST_CMD(dev, cmd, payload) \
+	(((uint64_t)(dev) << HTIF_DEV_SHIFT) | \
+	 ((uint64_t)(cmd) << HTIF_CMD_SHIFT) | \
+	 (uint64_t)(payload))
+#else
+# define TOHOST_CMD(dev, cmd, payload) ({ \
+  if ((dev) || (cmd)) __builtin_trap(); \
+  (payload); })
+#endif
+#define FROMHOST_DEV(fromhost_value) \
+	((uint64_t)((fromhost_value) >> HTIF_DEV_SHIFT) & HTIF_DEV_MASK)
+#define FROMHOST_CMD(fromhost_value) \
+	((uint64_t)((fromhost_value) >> HTIF_CMD_SHIFT) & HTIF_CMD_MASK)
+#define FROMHOST_DATA(fromhost_value) \
+	((uint64_t)((fromhost_value) >> HTIF_DATA_SHIFT) & HTIF_DATA_MASK)
+
+#define PK_SYS_write 64
+
+volatile uint64_t tohost __attribute__((section(".htif")));
+volatile uint64_t fromhost __attribute__((section(".htif")));
+static int htif_console_buf;
+static spinlock_t htif_lock = SPIN_LOCK_INITIALIZER;
+
+static void __check_fromhost()
+{
+	uint64_t fh = fromhost;
+	if (!fh)
+		return;
+	fromhost = 0;
+
+	/* this should be from the console */
+	if (FROMHOST_DEV(fh) != HTIF_DEV_CONSOLE)
+		__builtin_trap();
+	switch (FROMHOST_CMD(fh)) {
+		case HTIF_CONSOLE_CMD_GETC:
+			htif_console_buf = 1 + (uint8_t)FROMHOST_DATA(fh);
+			break;
+		case HTIF_CONSOLE_CMD_PUTC:
+			break;
+		default:
+			__builtin_trap();
+	}
+}
+
+static void __set_tohost(uint64_t dev, uint64_t cmd, uint64_t data)
+{
+	while (tohost)
+		__check_fromhost();
+	tohost = TOHOST_CMD(dev, cmd, data);
+}
+
+#if __riscv_xlen == 32
+static void do_tohost_fromhost(uint64_t dev, uint64_t cmd, uint64_t data)
+{
+	spin_lock(&htif_lock);
+
+	__set_tohost(HTIF_DEV_SYSTEM, cmd, data);
+
+	while (1) {
+		uint64_t fh = fromhost;
+		if (fh) {
+			if (FROMHOST_DEV(fh) == HTIF_DEV_SYSTEM &&
+			    FROMHOST_CMD(fh) == cmd) {
+				fromhost = 0;
+				break;
+			}
+			__check_fromhost();
+		}
+	}
+
+	spin_unlock(&htif_lock);
+}
+
+static void htif_putc(char ch)
+{
+	/* HTIF devices are not supported on RV32, so do a proxy write call */
+	volatile uint64_t magic_mem[8];
+	magic_mem[0] = PK_SYS_write;
+	magic_mem[1] = HTIF_DEV_CONSOLE;
+	magic_mem[2] = (uint64_t)(uintptr_t)&ch;
+	magic_mem[3] = HTIF_CONSOLE_CMD_PUTC;
+	do_tohost_fromhost(HTIF_DEV_SYSTEM, 0, (uint64_t)(uintptr_t)magic_mem);
+}
+#else
+static void htif_putc(char ch)
+{
+	spin_lock(&htif_lock);
+	__set_tohost(HTIF_DEV_CONSOLE, HTIF_CONSOLE_CMD_PUTC, ch);
+	spin_unlock(&htif_lock);
+}
+#endif
+
+static int htif_getc(void)
+{
+	int ch;
+
+#if __riscv_xlen == 32
+	/* HTIF devices are not supported on RV32 */
+	return -1;
+#endif
+
+	spin_lock(&htif_lock);
+
+	__check_fromhost();
+	ch = htif_console_buf;
+	if (ch >= 0) {
+		htif_console_buf = -1;
+		__set_tohost(HTIF_DEV_CONSOLE, HTIF_CONSOLE_CMD_GETC, 0);
+	}
+
+	spin_unlock(&htif_lock);
+
+	return ch - 1;
+}
+
+static struct sbi_console_device htif_console = {
+	.name = "htif",
+	.console_putc = htif_putc,
+	.console_getc = htif_getc
+};
+
+int htif_serial_init(void)
+{
+	sbi_console_set_device(&htif_console);
+
+	return 0;
+}
+
+static int htif_system_reset_check(u32 type, u32 reason)
+{
+	return 1;
+}
+
+static void htif_system_reset(u32 type, u32 reason)
+{
+	while (1) {
+		fromhost = 0;
+		tohost = 1;
+	}
+}
+
+static struct sbi_system_reset_device htif_reset = {
+	.name = "htif",
+	.system_reset_check = htif_system_reset_check,
+	.system_reset = htif_system_reset
+};
+
+int htif_system_reset_init(void)
+{
+	sbi_system_reset_set_device(&htif_reset);
+
+	return 0;
+}

lib/utils/sys/objects.mk

@@ -0,0 +1,12 @@
+#
+# SPDX-License-Identifier: BSD-2-Clause
+#
+# Copyright (c) 2019 Western Digital Corporation or its affiliates.
+#
+# Authors:
+#   Anup Patel <anup.patel@wdc.com>
+#
+
+libsbiutils-objs-y += sys/clint.o
+libsbiutils-objs-y += sys/htif.o
+libsbiutils-objs-y += sys/sifive_test.o

lib/utils/sys/sifive_test.c

@@ -0,0 +1,65 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_io.h>
+#include <sbi/sbi_ecall_interface.h>
+#include <sbi/sbi_system.h>
+#include <sbi_utils/sys/sifive_test.h>
+
+#define FINISHER_FAIL		0x3333
+#define FINISHER_PASS		0x5555
+#define FINISHER_RESET		0x7777
+
+static void *sifive_test_base;
+
+static int sifive_test_system_reset_check(u32 type, u32 reason)
+{
+	switch (type) {
+	case SBI_SRST_RESET_TYPE_SHUTDOWN:
+	case SBI_SRST_RESET_TYPE_COLD_REBOOT:
+	case SBI_SRST_RESET_TYPE_WARM_REBOOT:
+		return 1;
+	}
+
+	return 0;
+}
+
+static void sifive_test_system_reset(u32 type, u32 reason)
+{
+	/*
+	 * Tell the "finisher" that the simulation
+	 * was successful so that QEMU exits
+	 */
+	switch (type) {
+	case SBI_SRST_RESET_TYPE_SHUTDOWN:
+		if (reason == SBI_SRST_RESET_REASON_NONE)
+			writew(FINISHER_PASS, sifive_test_base);
+		else
+			writew(FINISHER_FAIL, sifive_test_base);
+		break;
+	case SBI_SRST_RESET_TYPE_COLD_REBOOT:
+	case SBI_SRST_RESET_TYPE_WARM_REBOOT:
+		writew(FINISHER_RESET, sifive_test_base);
+		break;
+	}
+}
+
+static struct sbi_system_reset_device sifive_test_reset = {
+	.name = "sifive_test",
+	.system_reset_check = sifive_test_system_reset_check,
+	.system_reset = sifive_test_system_reset
+};
+
+int sifive_test_init(unsigned long base)
+{
+	sifive_test_base = (void *)base;
+	sbi_system_reset_set_device(&sifive_test_reset);
+
+	return 0;
+}

lib/utils/timer/fdt_timer.c

@@ -0,0 +1,81 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/sbi_scratch.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/timer/fdt_timer.h>
+
+extern struct fdt_timer fdt_timer_clint;
+
+static struct fdt_timer *timer_drivers[] = {
+	&fdt_timer_clint
+};
+
+static struct fdt_timer dummy = {
+	.match_table = NULL,
+	.cold_init = NULL,
+	.warm_init = NULL,
+	.exit = NULL,
+};
+
+static struct fdt_timer *current_driver = &dummy;
+
+void fdt_timer_exit(void)
+{
+	if (current_driver->exit)
+		current_driver->exit();
+}
+
+static int fdt_timer_warm_init(void)
+{
+	if (current_driver->warm_init)
+		return current_driver->warm_init();
+	return 0;
+}
+
+static int fdt_timer_cold_init(void)
+{
+	int pos, noff, rc;
+	struct fdt_timer *drv;
+	const struct fdt_match *match;
+	void *fdt = sbi_scratch_thishart_arg1_ptr();
+
+	for (pos = 0; pos < array_size(timer_drivers); pos++) {
+		drv = timer_drivers[pos];
+
+		noff = -1;
+		while ((noff = fdt_find_match(fdt, noff,
+					drv->match_table, &match)) >= 0) {
+			if (drv->cold_init) {
+				rc = drv->cold_init(fdt, noff, match);
+				if (rc)
+					return rc;
+			}
+			current_driver = drv;
+		}
+
+		if (current_driver != &dummy)
+			break;
+	}
+
+	return 0;
+}
+
+int fdt_timer_init(bool cold_boot)
+{
+	int rc;
+
+	if (cold_boot) {
+		rc = fdt_timer_cold_init();
+		if (rc)
+			return rc;
+	}
+
+	return fdt_timer_warm_init();
+}

lib/utils/timer/fdt_timer_clint.c

@@ -0,0 +1,50 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/sbi_error.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/timer/fdt_timer.h>
+#include <sbi_utils/sys/clint.h>
+
+#define CLINT_TIMER_MAX_NR			16
+
+static unsigned long clint_timer_count = 0;
+static struct clint_data clint_timer[CLINT_TIMER_MAX_NR];
+
+static int timer_clint_cold_init(void *fdt, int nodeoff,
+				  const struct fdt_match *match)
+{
+	int rc;
+	struct clint_data *ct, *ctmaster = NULL;
+
+	if (CLINT_TIMER_MAX_NR <= clint_timer_count)
+		return SBI_ENOSPC;
+	ct = &clint_timer[clint_timer_count++];
+	if (1 < clint_timer_count)
+		ctmaster = &clint_timer[0];
+
+	rc = fdt_parse_clint_node(fdt, nodeoff, TRUE, ct);
+	if (rc)
+		return rc;
+
+	return clint_cold_timer_init(ct, ctmaster);
+}
+
+static const struct fdt_match timer_clint_match[] = {
+	{ .compatible = "riscv,clint0" },
+	{ .compatible = "sifive,clint0" },
+	{ },
+};
+
+struct fdt_timer fdt_timer_clint = {
+	.match_table = timer_clint_match,
+	.cold_init = timer_clint_cold_init,
+	.warm_init = clint_warm_timer_init,
+	.exit = NULL,
+};

lib/utils/timer/objects.mk

@@ -0,0 +1,11 @@
+#
+# SPDX-License-Identifier: BSD-2-Clause
+#
+# Copyright (c) 2020 Western Digital Corporation or its affiliates.
+#
+# Authors:
+#   Anup Patel <anup.patel@wdc.com>
+#
+
+libsbiutils-objs-y += timer/fdt_timer.o
+libsbiutils-objs-y += timer/fdt_timer_clint.o