1 files changed, 88 insertions, 0 deletions
diff --git a/roms/opensbi/firmware/payloads/test_head.S b/roms/opensbi/firmware/payloads/test_head.S
new file mode 100644
index 000000000..840013e4e
--- /dev/null
+++ b/roms/opensbi/firmware/payloads/test_head.S
@@ -0,0 +1,88 @@
+/*
+ * 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>
+#define __ASM_STR(x)	x
+
+#if __riscv_xlen == 64
+#define __REG_SEL(a, b)		__ASM_STR(a)
+#define RISCV_PTR		.dword
+#elif __riscv_xlen == 32
+#define __REG_SEL(a, b)		__ASM_STR(b)
+#define RISCV_PTR		.word
+#else
+#error "Unexpected __riscv_xlen"
+#endif
+
+#define REG_L		__REG_SEL(ld, lw)
+#define REG_S		__REG_SEL(sd, sw)
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.globl _start
+_start:
+	/* Pick one hart to run the main boot sequence */
+	la	a3, _hart_lottery
+	li	a2, 1
+	amoadd.w a3, a2, (a3)
+	bnez	a3, _start_hang
+
+	/* Save a0 and a1 */
+	la	a3, _boot_a0
+	REG_S	a0, 0(a3)
+	la	a3, _boot_a1
+	REG_S	a1, 0(a3)
+
+	/* Zero-out BSS */
+	la	a4, _bss_start
+	la	a5, _bss_end
+_bss_zero:
+	REG_S	zero, (a4)
+	add	a4, a4, __SIZEOF_POINTER__
+	blt	a4, a5, _bss_zero
+
+_start_warm:
+	/* Disable and clear all interrupts */
+	csrw	CSR_SIE, zero
+	csrw	CSR_SIP, zero
+
+	/* Setup exception vectors */
+	la	a3, _start_hang
+	csrw	CSR_STVEC, a3
+
+	/* Setup stack */
+	la	a3, _payload_end
+	li	a4, 0x2000
+	add	sp, a3, a4
+
+	/* Jump to C main */
+	la	a3, _boot_a0
+	REG_L	a0, 0(a3)
+	la	a3, _boot_a1
+	REG_L	a1, 0(a3)
+	call	test_main
+
+	/* We don't expect to reach here hence just hang */
+	j	_start_hang
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.globl _start_hang
+_start_hang:
+	wfi
+	j	_start_hang
+
+	.section .entry, "ax", %progbits
+	.align	3
+_hart_lottery:
+	RISCV_PTR	0
+_boot_a0:
+	RISCV_PTR	0
+_boot_a1:
+	RISCV_PTR	0