Add submodule dependency filesHEADmaster

Change-Id: Iaf8d18082d3991dec7c0ebbea540f092188eb4ec

14 files changed, 1575 insertions, 0 deletions

diff --git a/roms/opensbi/firmware/external_deps.mk b/roms/opensbi/firmware/external_deps.mk
new file mode 100644
index 000000000..6264005bc
--- /dev/null
+++ b/roms/opensbi/firmware/external_deps.mk
@@ -0,0 +1,14 @@
+#
+# SPDX-License-Identifier: BSD-2-Clause
+#
+# Copyright (c) 2019 Western Digital Corporation or its affiliates.
+#
+# Authors:
+#   Anup Patel <anup.patel@wdc.com>
+#
+
+$(platform_build_dir)/firmware/fw_dynamic.o: $(FW_FDT_PATH)
+$(platform_build_dir)/firmware/fw_jump.o: $(FW_FDT_PATH)
+$(platform_build_dir)/firmware/fw_payload.o: $(FW_FDT_PATH)
+
+$(platform_build_dir)/firmware/fw_payload.o: $(FW_PAYLOAD_PATH_FINAL)
diff --git a/roms/opensbi/firmware/fw_base.S b/roms/opensbi/firmware/fw_base.S
new file mode 100644
index 000000000..ab33e1102
--- /dev/null
+++ b/roms/opensbi/firmware/fw_base.S
@@ -0,0 +1,769 @@
+/*
+ * 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_platform.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi/sbi_trap.h>
+
+#define BOOT_STATUS_RELOCATE_DONE	1
+#define BOOT_STATUS_BOOT_HART_DONE	2
+
+.macro	MOV_3R __d0, __s0, __d1, __s1, __d2, __s2
+	add	\__d0, \__s0, zero
+	add	\__d1, \__s1, zero
+	add	\__d2, \__s2, zero
+.endm
+
+.macro	MOV_5R __d0, __s0, __d1, __s1, __d2, __s2, __d3, __s3, __d4, __s4
+	add	\__d0, \__s0, zero
+	add	\__d1, \__s1, zero
+	add	\__d2, \__s2, zero
+	add	\__d3, \__s3, zero
+	add	\__d4, \__s4, zero
+.endm
+
+/*
+ * If __start_reg <= __check_reg and __check_reg < __end_reg then
+ *   jump to __pass
+ */
+.macro BRANGE __start_reg, __end_reg, __check_reg, __jump_lable
+	blt	\__check_reg, \__start_reg, 999f
+	bge	\__check_reg, \__end_reg, 999f
+	j	\__jump_lable
+999:
+.endm
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.globl _start
+	.globl _start_warm
+_start:
+	/* Find preferred boot HART id */
+	MOV_3R	s0, a0, s1, a1, s2, a2
+	call	fw_boot_hart
+	add	a6, a0, zero
+	MOV_3R	a0, s0, a1, s1, a2, s2
+	li	a7, -1
+	beq	a6, a7, _try_lottery
+	/* Jump to relocation wait loop if we are not boot hart */
+	bne	a0, a6, _wait_relocate_copy_done
+_try_lottery:
+	/* Jump to relocation wait loop if we don't get relocation lottery */
+	la	a6, _relocate_lottery
+	li	a7, 1
+	amoadd.w a6, a7, (a6)
+	bnez	a6, _wait_relocate_copy_done
+
+	/* Save load address */
+	la	t0, _load_start
+	la	t1, _start
+	REG_S	t1, 0(t0)
+
+	/* Relocate if load address != link address */
+_relocate:
+	la	t0, _link_start
+	REG_L	t0, 0(t0)
+	la	t1, _link_end
+	REG_L	t1, 0(t1)
+	la	t2, _load_start
+	REG_L	t2, 0(t2)
+	sub	t3, t1, t0
+	add	t3, t3, t2
+	beq	t0, t2, _relocate_done
+	la	t4, _relocate_done
+	sub	t4, t4, t2
+	add	t4, t4, t0
+	blt	t2, t0, _relocate_copy_to_upper
+_relocate_copy_to_lower:
+	ble	t1, t2, _relocate_copy_to_lower_loop
+	la	t3, _relocate_lottery
+	BRANGE	t2, t1, t3, _start_hang
+	la	t3, _boot_status
+	BRANGE	t2, t1, t3, _start_hang
+	la	t3, _relocate
+	la	t5, _relocate_done
+	BRANGE	t2, t1, t3, _start_hang
+	BRANGE	t2, t1, t5, _start_hang
+	BRANGE  t3, t5, t2, _start_hang
+_relocate_copy_to_lower_loop:
+	REG_L	t3, 0(t2)
+	REG_S	t3, 0(t0)
+	add	t0, t0, __SIZEOF_POINTER__
+	add	t2, t2, __SIZEOF_POINTER__
+	blt	t0, t1, _relocate_copy_to_lower_loop
+	jr	t4
+_relocate_copy_to_upper:
+	ble	t3, t0, _relocate_copy_to_upper_loop
+	la	t2, _relocate_lottery
+	BRANGE	t0, t3, t2, _start_hang
+	la	t2, _boot_status
+	BRANGE	t0, t3, t2, _start_hang
+	la	t2, _relocate
+	la	t5, _relocate_done
+	BRANGE	t0, t3, t2, _start_hang
+	BRANGE	t0, t3, t5, _start_hang
+	BRANGE	t2, t5, t0, _start_hang
+_relocate_copy_to_upper_loop:
+	add	t3, t3, -__SIZEOF_POINTER__
+	add	t1, t1, -__SIZEOF_POINTER__
+	REG_L	t2, 0(t3)
+	REG_S	t2, 0(t1)
+	blt	t0, t1, _relocate_copy_to_upper_loop
+	jr	t4
+_wait_relocate_copy_done:
+	la	t0, _start
+	la	t1, _link_start
+	REG_L	t1, 0(t1)
+	beq	t0, t1, _wait_for_boot_hart
+	la	t2, _boot_status
+	la	t3, _wait_for_boot_hart
+	sub	t3, t3, t0
+	add	t3, t3, t1
+1:
+	/* waitting for relocate copy done (_boot_status == 1) */
+	li	t4, BOOT_STATUS_RELOCATE_DONE
+	REG_L	t5, 0(t2)
+	/* Reduce the bus traffic so that boot hart may proceed faster */
+	nop
+	nop
+	nop
+	bgt     t4, t5, 1b
+	jr	t3
+_relocate_done:
+
+	/*
+	 * Mark relocate copy done
+	 * Use _boot_status copy relative to the load address
+	 */
+	la	t0, _boot_status
+	la	t1, _link_start
+	REG_L	t1, 0(t1)
+	la	t2, _load_start
+	REG_L	t2, 0(t2)
+	sub	t0, t0, t1
+	add	t0, t0, t2
+	li	t1, BOOT_STATUS_RELOCATE_DONE
+	REG_S	t1, 0(t0)
+	fence	rw, rw
+
+	/* At this point we are running from link address */
+
+	/* Reset all registers for boot HART */
+	li	ra, 0
+	call	_reset_regs
+
+	/* Zero-out BSS */
+	la	s4, _bss_start
+	la	s5, _bss_end
+_bss_zero:
+	REG_S	zero, (s4)
+	add	s4, s4, __SIZEOF_POINTER__
+	blt	s4, s5, _bss_zero
+
+	/* Setup temporary trap handler */
+	la	s4, _start_hang
+	csrw	CSR_MTVEC, s4
+
+	/* Setup temporary stack */
+	la	s4, _fw_end
+	li	s5, (SBI_SCRATCH_SIZE * 2)
+	add	sp, s4, s5
+
+	/* Allow main firmware to save info */
+	MOV_5R	s0, a0, s1, a1, s2, a2, s3, a3, s4, a4
+	call	fw_save_info
+	MOV_5R	a0, s0, a1, s1, a2, s2, a3, s3, a4, s4
+
+#ifdef FW_FDT_PATH
+	/* Override previous arg1 */
+	la	a1, fw_fdt_bin
+#endif
+
+	/*
+	 * Initialize platform
+	 * Note: The a0 to a4 registers passed to the
+	 * firmware are parameters to this function.
+	 */
+	MOV_5R	s0, a0, s1, a1, s2, a2, s3, a3, s4, a4
+	call	fw_platform_init
+	add	t0, a0, zero
+	MOV_5R	a0, s0, a1, s1, a2, s2, a3, s3, a4, s4
+	add	a1, t0, zero
+
+	/* Preload HART details
+	 * s7 -> HART Count
+	 * s8 -> HART Stack Size
+	 */
+	la	a4, platform
+#if __riscv_xlen == 64
+	lwu	s7, SBI_PLATFORM_HART_COUNT_OFFSET(a4)
+	lwu	s8, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(a4)
+#else
+	lw	s7, SBI_PLATFORM_HART_COUNT_OFFSET(a4)
+	lw	s8, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(a4)
+#endif
+
+	/* Setup scratch space for all the HARTs*/
+	la	tp, _fw_end
+	mul	a5, s7, s8
+	add	tp, tp, a5
+	/* Keep a copy of tp */
+	add	t3, tp, zero
+	/* Counter */
+	li	t2, 1
+	/* hartid 0 is mandated by ISA */
+	li	t1, 0
+_scratch_init:
+	add	tp, t3, zero
+	mul	a5, s8, t1
+	sub	tp, tp, a5
+	li	a5, SBI_SCRATCH_SIZE
+	sub	tp, tp, a5
+
+	/* Initialize scratch space */
+	/* Store fw_start and fw_size in scratch space */
+	la	a4, _fw_start
+	la	a5, _fw_end
+	mul	t0, s7, s8
+	add	a5, a5, t0
+	sub	a5, a5, a4
+	REG_S	a4, SBI_SCRATCH_FW_START_OFFSET(tp)
+	REG_S	a5, SBI_SCRATCH_FW_SIZE_OFFSET(tp)
+	/* Store next arg1 in scratch space */
+	MOV_3R	s0, a0, s1, a1, s2, a2
+	call	fw_next_arg1
+	REG_S	a0, SBI_SCRATCH_NEXT_ARG1_OFFSET(tp)
+	MOV_3R	a0, s0, a1, s1, a2, s2
+	/* Store next address in scratch space */
+	MOV_3R	s0, a0, s1, a1, s2, a2
+	call	fw_next_addr
+	REG_S	a0, SBI_SCRATCH_NEXT_ADDR_OFFSET(tp)
+	MOV_3R	a0, s0, a1, s1, a2, s2
+	/* Store next mode in scratch space */
+	MOV_3R	s0, a0, s1, a1, s2, a2
+	call	fw_next_mode
+	REG_S	a0, SBI_SCRATCH_NEXT_MODE_OFFSET(tp)
+	MOV_3R	a0, s0, a1, s1, a2, s2
+	/* Store warm_boot address in scratch space */
+	la	a4, _start_warm
+	REG_S	a4, SBI_SCRATCH_WARMBOOT_ADDR_OFFSET(tp)
+	/* Store platform address in scratch space */
+	la	a4, platform
+	REG_S	a4, SBI_SCRATCH_PLATFORM_ADDR_OFFSET(tp)
+	/* Store hartid-to-scratch function address in scratch space */
+	la	a4, _hartid_to_scratch
+	REG_S	a4, SBI_SCRATCH_HARTID_TO_SCRATCH_OFFSET(tp)
+	/* Store trap-exit function address in scratch space */
+	la	a4, _trap_exit
+	REG_S	a4, SBI_SCRATCH_TRAP_EXIT_OFFSET(tp)
+	/* Clear tmp0 in scratch space */
+	REG_S	zero, SBI_SCRATCH_TMP0_OFFSET(tp)
+	/* Store firmware options in scratch space */
+	MOV_3R	s0, a0, s1, a1, s2, a2
+#ifdef FW_OPTIONS
+	li	a0, FW_OPTIONS
+#else
+	call	fw_options
+#endif
+	REG_S	a0, SBI_SCRATCH_OPTIONS_OFFSET(tp)
+	MOV_3R	a0, s0, a1, s1, a2, s2
+	/* Move to next scratch space */
+	add	t1, t1, t2
+	blt	t1, s7, _scratch_init
+
+	/*
+	 * Relocate Flatened Device Tree (FDT)
+	 * source FDT address = previous arg1
+	 * destination FDT address = next arg1
+	 *
+	 * Note: We will preserve a0 and a1 passed by
+	 * previous booting stage.
+	 */
+	beqz	a1, _fdt_reloc_done
+	/* Mask values in a3 and a4 */
+	li	a3, ~(__SIZEOF_POINTER__ - 1)
+	li	a4, 0xff
+	/* t1 = destination FDT start address */
+	MOV_3R	s0, a0, s1, a1, s2, a2
+	call	fw_next_arg1
+	add	t1, a0, zero
+	MOV_3R	a0, s0, a1, s1, a2, s2
+	beqz	t1, _fdt_reloc_done
+	beq	t1, a1, _fdt_reloc_done
+	and	t1, t1, a3
+	/* t0 = source FDT start address */
+	add	t0, a1, zero
+	and	t0, t0, a3
+	/* t2 = source FDT size in big-endian */
+#if __riscv_xlen == 64
+	lwu	t2, 4(t0)
+#else
+	lw	t2, 4(t0)
+#endif
+	/* t3 = bit[15:8] of FDT size */
+	add	t3, t2, zero
+	srli	t3, t3, 16
+	and	t3, t3, a4
+	slli	t3, t3, 8
+	/* t4 = bit[23:16] of FDT size */
+	add	t4, t2, zero
+	srli	t4, t4, 8
+	and	t4, t4, a4
+	slli	t4, t4, 16
+	/* t5 = bit[31:24] of FDT size */
+	add	t5, t2, zero
+	and	t5, t5, a4
+	slli	t5, t5, 24
+	/* t2 = bit[7:0] of FDT size */
+	srli	t2, t2, 24
+	and	t2, t2, a4
+	/* t2 = FDT size in little-endian */
+	or	t2, t2, t3
+	or	t2, t2, t4
+	or	t2, t2, t5
+	/* t2 = destination FDT end address */
+	add	t2, t1, t2
+	/* FDT copy loop */
+	ble	t2, t1, _fdt_reloc_done
+_fdt_reloc_again:
+	REG_L	t3, 0(t0)
+	REG_S	t3, 0(t1)
+	add	t0, t0, __SIZEOF_POINTER__
+	add	t1, t1, __SIZEOF_POINTER__
+	blt	t1, t2, _fdt_reloc_again
+_fdt_reloc_done:
+
+	/* mark boot hart done */
+	li	t0, BOOT_STATUS_BOOT_HART_DONE
+	la	t1, _boot_status
+	REG_S	t0, 0(t1)
+	fence	rw, rw
+	j	_start_warm
+
+	/* waiting for boot hart to be done (_boot_status == 2) */
+_wait_for_boot_hart:
+	li	t0, BOOT_STATUS_BOOT_HART_DONE
+	la	t1, _boot_status
+	REG_L	t1, 0(t1)
+	/* Reduce the bus traffic so that boot hart may proceed faster */
+	nop
+	nop
+	nop
+	bne	t0, t1, _wait_for_boot_hart
+
+_start_warm:
+	/* Reset all registers for non-boot HARTs */
+	li	ra, 0
+	call	_reset_regs
+
+	/* Disable and clear all interrupts */
+	csrw	CSR_MIE, zero
+	csrw	CSR_MIP, zero
+
+	/* Find HART count and HART stack size */
+	la	a4, platform
+#if __riscv_xlen == 64
+	lwu	s7, SBI_PLATFORM_HART_COUNT_OFFSET(a4)
+	lwu	s8, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(a4)
+#else
+	lw	s7, SBI_PLATFORM_HART_COUNT_OFFSET(a4)
+	lw	s8, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(a4)
+#endif
+	REG_L	s9, SBI_PLATFORM_HART_INDEX2ID_OFFSET(a4)
+
+	/* Find HART id */
+	csrr	s6, CSR_MHARTID
+
+	/* Find HART index */
+	beqz	s9, 3f
+	li	a4, 0
+1:
+#if __riscv_xlen == 64
+	lwu	a5, (s9)
+#else
+	lw	a5, (s9)
+#endif
+	beq	a5, s6, 2f
+	add	s9, s9, 4
+	add	a4, a4, 1
+	blt	a4, s7, 1b
+	li	a4, -1
+2:	add	s6, a4, zero
+3:	bge	s6, s7, _start_hang
+
+	/* Find the scratch space based on HART index */
+	la	tp, _fw_end
+	mul	a5, s7, s8
+	add	tp, tp, a5
+	mul	a5, s8, s6
+	sub	tp, tp, a5
+	li	a5, SBI_SCRATCH_SIZE
+	sub	tp, tp, a5
+
+	/* update the mscratch */
+	csrw	CSR_MSCRATCH, tp
+
+	/* Setup stack */
+	add	sp, tp, zero
+
+	/* Setup trap handler */
+	la	a4, _trap_handler
+#if __riscv_xlen == 32
+	csrr	a5, CSR_MISA
+	srli	a5, a5, ('H' - 'A')
+	andi	a5, a5, 0x1
+	beq	a5, zero, _skip_trap_handler_rv32_hyp
+	la	a4, _trap_handler_rv32_hyp
+_skip_trap_handler_rv32_hyp:
+#endif
+	csrw	CSR_MTVEC, a4
+
+#if __riscv_xlen == 32
+	/* Override trap exit for H-extension */
+	csrr	a5, CSR_MISA
+	srli	a5, a5, ('H' - 'A')
+	andi	a5, a5, 0x1
+	beq	a5, zero, _skip_trap_exit_rv32_hyp
+	la	a4, _trap_exit_rv32_hyp
+	csrr	a5, CSR_MSCRATCH
+	REG_S	a4, SBI_SCRATCH_TRAP_EXIT_OFFSET(a5)
+_skip_trap_exit_rv32_hyp:
+#endif
+
+	/* Initialize SBI runtime */
+	csrr	a0, CSR_MSCRATCH
+	call	sbi_init
+
+	/* We don't expect to reach here hence just hang */
+	j	_start_hang
+
+	.align 3
+_relocate_lottery:
+	RISCV_PTR	0
+_boot_status:
+	RISCV_PTR	0
+_load_start:
+	RISCV_PTR	_fw_start
+_link_start:
+	RISCV_PTR	_fw_start
+_link_end:
+	RISCV_PTR	_fw_reloc_end
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.globl _hartid_to_scratch
+_hartid_to_scratch:
+	/*
+	 * a0 -> HART ID (passed by caller)
+	 * a1 -> HART Index (passed by caller)
+	 * t0 -> HART Stack Size
+	 * t1 -> HART Stack End
+	 * t2 -> Temporary
+	 */
+	la	t2, platform
+#if __riscv_xlen == 64
+	lwu	t0, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(t2)
+	lwu	t2, SBI_PLATFORM_HART_COUNT_OFFSET(t2)
+#else
+	lw	t0, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(t2)
+	lw	t2, SBI_PLATFORM_HART_COUNT_OFFSET(t2)
+#endif
+	sub	t2, t2, a1
+	mul	t2, t2, t0
+	la	t1, _fw_end
+	add	t1, t1, t2
+	li	t2, SBI_SCRATCH_SIZE
+	sub	a0, t1, t2
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.globl _start_hang
+_start_hang:
+	wfi
+	j	_start_hang
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.globl fw_platform_init
+	.weak fw_platform_init
+fw_platform_init:
+	add	a0, a1, zero
+	ret
+
+.macro	TRAP_SAVE_AND_SETUP_SP_T0
+	/* Swap TP and MSCRATCH */
+	csrrw	tp, CSR_MSCRATCH, tp
+
+	/* Save T0 in scratch space */
+	REG_S	t0, SBI_SCRATCH_TMP0_OFFSET(tp)
+
+	/*
+	 * Set T0 to appropriate exception stack
+	 *
+	 * Came_From_M_Mode = ((MSTATUS.MPP < PRV_M) ? 1 : 0) - 1;
+	 * Exception_Stack = TP ^ (Came_From_M_Mode & (SP ^ TP))
+	 *
+	 * Came_From_M_Mode = 0    ==>    Exception_Stack = TP
+	 * Came_From_M_Mode = -1   ==>    Exception_Stack = SP
+	 */
+	csrr	t0, CSR_MSTATUS
+	srl	t0, t0, MSTATUS_MPP_SHIFT
+	and	t0, t0, PRV_M
+	slti	t0, t0, PRV_M
+	add	t0, t0, -1
+	xor	sp, sp, tp
+	and	t0, t0, sp
+	xor	sp, sp, tp
+	xor	t0, tp, t0
+
+	/* Save original SP on exception stack */
+	REG_S	sp, (SBI_TRAP_REGS_OFFSET(sp) - SBI_TRAP_REGS_SIZE)(t0)
+
+	/* Set SP to exception stack and make room for trap registers */
+	add	sp, t0, -(SBI_TRAP_REGS_SIZE)
+
+	/* Restore T0 from scratch space */
+	REG_L	t0, SBI_SCRATCH_TMP0_OFFSET(tp)
+
+	/* Save T0 on stack */
+	REG_S	t0, SBI_TRAP_REGS_OFFSET(t0)(sp)
+
+	/* Swap TP and MSCRATCH */
+	csrrw	tp, CSR_MSCRATCH, tp
+.endm
+
+.macro	TRAP_SAVE_MEPC_MSTATUS have_mstatush
+	/* Save MEPC and MSTATUS CSRs */
+	csrr	t0, CSR_MEPC
+	REG_S	t0, SBI_TRAP_REGS_OFFSET(mepc)(sp)
+	csrr	t0, CSR_MSTATUS
+	REG_S	t0, SBI_TRAP_REGS_OFFSET(mstatus)(sp)
+	.if \have_mstatush
+	csrr	t0, CSR_MSTATUSH
+	REG_S	t0, SBI_TRAP_REGS_OFFSET(mstatusH)(sp)
+	.else
+	REG_S	zero, SBI_TRAP_REGS_OFFSET(mstatusH)(sp)
+	.endif
+.endm
+
+.macro	TRAP_SAVE_GENERAL_REGS_EXCEPT_SP_T0
+	/* Save all general regisers except SP and T0 */
+	REG_S	zero, SBI_TRAP_REGS_OFFSET(zero)(sp)
+	REG_S	ra, SBI_TRAP_REGS_OFFSET(ra)(sp)
+	REG_S	gp, SBI_TRAP_REGS_OFFSET(gp)(sp)
+	REG_S	tp, SBI_TRAP_REGS_OFFSET(tp)(sp)
+	REG_S	t1, SBI_TRAP_REGS_OFFSET(t1)(sp)
+	REG_S	t2, SBI_TRAP_REGS_OFFSET(t2)(sp)
+	REG_S	s0, SBI_TRAP_REGS_OFFSET(s0)(sp)
+	REG_S	s1, SBI_TRAP_REGS_OFFSET(s1)(sp)
+	REG_S	a0, SBI_TRAP_REGS_OFFSET(a0)(sp)
+	REG_S	a1, SBI_TRAP_REGS_OFFSET(a1)(sp)
+	REG_S	a2, SBI_TRAP_REGS_OFFSET(a2)(sp)
+	REG_S	a3, SBI_TRAP_REGS_OFFSET(a3)(sp)
+	REG_S	a4, SBI_TRAP_REGS_OFFSET(a4)(sp)
+	REG_S	a5, SBI_TRAP_REGS_OFFSET(a5)(sp)
+	REG_S	a6, SBI_TRAP_REGS_OFFSET(a6)(sp)
+	REG_S	a7, SBI_TRAP_REGS_OFFSET(a7)(sp)
+	REG_S	s2, SBI_TRAP_REGS_OFFSET(s2)(sp)
+	REG_S	s3, SBI_TRAP_REGS_OFFSET(s3)(sp)
+	REG_S	s4, SBI_TRAP_REGS_OFFSET(s4)(sp)
+	REG_S	s5, SBI_TRAP_REGS_OFFSET(s5)(sp)
+	REG_S	s6, SBI_TRAP_REGS_OFFSET(s6)(sp)
+	REG_S	s7, SBI_TRAP_REGS_OFFSET(s7)(sp)
+	REG_S	s8, SBI_TRAP_REGS_OFFSET(s8)(sp)
+	REG_S	s9, SBI_TRAP_REGS_OFFSET(s9)(sp)
+	REG_S	s10, SBI_TRAP_REGS_OFFSET(s10)(sp)
+	REG_S	s11, SBI_TRAP_REGS_OFFSET(s11)(sp)
+	REG_S	t3, SBI_TRAP_REGS_OFFSET(t3)(sp)
+	REG_S	t4, SBI_TRAP_REGS_OFFSET(t4)(sp)
+	REG_S	t5, SBI_TRAP_REGS_OFFSET(t5)(sp)
+	REG_S	t6, SBI_TRAP_REGS_OFFSET(t6)(sp)
+.endm
+
+.macro	TRAP_CALL_C_ROUTINE
+	/* Call C routine */
+	add	a0, sp, zero
+	call	sbi_trap_handler
+.endm
+
+.macro	TRAP_RESTORE_GENERAL_REGS_EXCEPT_SP_T0
+	/* Restore all general regisers except SP and T0 */
+	REG_L	ra, SBI_TRAP_REGS_OFFSET(ra)(sp)
+	REG_L	gp, SBI_TRAP_REGS_OFFSET(gp)(sp)
+	REG_L	tp, SBI_TRAP_REGS_OFFSET(tp)(sp)
+	REG_L	t1, SBI_TRAP_REGS_OFFSET(t1)(sp)
+	REG_L	t2, SBI_TRAP_REGS_OFFSET(t2)(sp)
+	REG_L	s0, SBI_TRAP_REGS_OFFSET(s0)(sp)
+	REG_L	s1, SBI_TRAP_REGS_OFFSET(s1)(sp)
+	REG_L	a0, SBI_TRAP_REGS_OFFSET(a0)(sp)
+	REG_L	a1, SBI_TRAP_REGS_OFFSET(a1)(sp)
+	REG_L	a2, SBI_TRAP_REGS_OFFSET(a2)(sp)
+	REG_L	a3, SBI_TRAP_REGS_OFFSET(a3)(sp)
+	REG_L	a4, SBI_TRAP_REGS_OFFSET(a4)(sp)
+	REG_L	a5, SBI_TRAP_REGS_OFFSET(a5)(sp)
+	REG_L	a6, SBI_TRAP_REGS_OFFSET(a6)(sp)
+	REG_L	a7, SBI_TRAP_REGS_OFFSET(a7)(sp)
+	REG_L	s2, SBI_TRAP_REGS_OFFSET(s2)(sp)
+	REG_L	s3, SBI_TRAP_REGS_OFFSET(s3)(sp)
+	REG_L	s4, SBI_TRAP_REGS_OFFSET(s4)(sp)
+	REG_L	s5, SBI_TRAP_REGS_OFFSET(s5)(sp)
+	REG_L	s6, SBI_TRAP_REGS_OFFSET(s6)(sp)
+	REG_L	s7, SBI_TRAP_REGS_OFFSET(s7)(sp)
+	REG_L	s8, SBI_TRAP_REGS_OFFSET(s8)(sp)
+	REG_L	s9, SBI_TRAP_REGS_OFFSET(s9)(sp)
+	REG_L	s10, SBI_TRAP_REGS_OFFSET(s10)(sp)
+	REG_L	s11, SBI_TRAP_REGS_OFFSET(s11)(sp)
+	REG_L	t3, SBI_TRAP_REGS_OFFSET(t3)(sp)
+	REG_L	t4, SBI_TRAP_REGS_OFFSET(t4)(sp)
+	REG_L	t5, SBI_TRAP_REGS_OFFSET(t5)(sp)
+	REG_L	t6, SBI_TRAP_REGS_OFFSET(t6)(sp)
+.endm
+
+.macro	TRAP_RESTORE_MEPC_MSTATUS have_mstatush
+	/* Restore MEPC and MSTATUS CSRs */
+	REG_L	t0, SBI_TRAP_REGS_OFFSET(mepc)(sp)
+	csrw	CSR_MEPC, t0
+	REG_L	t0, SBI_TRAP_REGS_OFFSET(mstatus)(sp)
+	csrw	CSR_MSTATUS, t0
+	.if \have_mstatush
+	REG_L	t0, SBI_TRAP_REGS_OFFSET(mstatusH)(sp)
+	csrw	CSR_MSTATUSH, t0
+	.endif
+.endm
+
+.macro TRAP_RESTORE_SP_T0
+	/* Restore T0 */
+	REG_L	t0, SBI_TRAP_REGS_OFFSET(t0)(sp)
+
+	/* Restore SP */
+	REG_L	sp, SBI_TRAP_REGS_OFFSET(sp)(sp)
+.endm
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.globl _trap_handler
+_trap_handler:
+	TRAP_SAVE_AND_SETUP_SP_T0
+
+	TRAP_SAVE_MEPC_MSTATUS 0
+
+	TRAP_SAVE_GENERAL_REGS_EXCEPT_SP_T0
+
+	TRAP_CALL_C_ROUTINE
+
+	TRAP_RESTORE_GENERAL_REGS_EXCEPT_SP_T0
+
+	TRAP_RESTORE_MEPC_MSTATUS 0
+
+	TRAP_RESTORE_SP_T0
+
+	mret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.globl _trap_exit
+_trap_exit:
+	add	sp, a0, zero
+
+	TRAP_RESTORE_GENERAL_REGS_EXCEPT_SP_T0
+
+	TRAP_RESTORE_MEPC_MSTATUS 0
+
+	TRAP_RESTORE_SP_T0
+
+	mret
+
+#if __riscv_xlen == 32
+	.section .entry, "ax", %progbits
+	.align 3
+	.globl _trap_handler_rv32_hyp
+_trap_handler_rv32_hyp:
+	TRAP_SAVE_AND_SETUP_SP_T0
+
+	TRAP_SAVE_MEPC_MSTATUS 1
+
+	TRAP_SAVE_GENERAL_REGS_EXCEPT_SP_T0
+
+	TRAP_CALL_C_ROUTINE
+
+	TRAP_RESTORE_GENERAL_REGS_EXCEPT_SP_T0
+
+	TRAP_RESTORE_MEPC_MSTATUS 1
+
+	TRAP_RESTORE_SP_T0
+
+	mret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.globl _trap_exit_rv32_hyp
+_trap_exit_rv32_hyp:
+	add	sp, a0, zero
+
+	TRAP_RESTORE_GENERAL_REGS_EXCEPT_SP_T0
+
+	TRAP_RESTORE_MEPC_MSTATUS 1
+
+	TRAP_RESTORE_SP_T0
+
+	mret
+#endif
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.globl _reset_regs
+_reset_regs:
+
+	/* flush the instruction cache */
+	fence.i
+	/* Reset all registers except ra, a0, a1 and a2 */
+	li sp, 0
+	li gp, 0
+	li tp, 0
+	li t0, 0
+	li t1, 0
+	li t2, 0
+	li s0, 0
+	li s1, 0
+	li a3, 0
+	li a4, 0
+	li a5, 0
+	li a6, 0
+	li a7, 0
+	li s2, 0
+	li s3, 0
+	li s4, 0
+	li s5, 0
+	li s6, 0
+	li s7, 0
+	li s8, 0
+	li s9, 0
+	li s10, 0
+	li s11, 0
+	li t3, 0
+	li t4, 0
+	li t5, 0
+	li t6, 0
+	csrw CSR_MSCRATCH, 0
+
+	ret
+
+#ifdef FW_FDT_PATH
+	.section .rodata
+	.align 4
+	.globl fw_fdt_bin
+fw_fdt_bin:
+	.incbin FW_FDT_PATH
+#ifdef FW_FDT_PADDING
+	.fill FW_FDT_PADDING, 1, 0
+#endif
+#endif
diff --git a/roms/opensbi/firmware/fw_base.ldS b/roms/opensbi/firmware/fw_base.ldS
new file mode 100644
index 000000000..0ac75f200
--- /dev/null
+++ b/roms/opensbi/firmware/fw_base.ldS
@@ -0,0 +1,81 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+	. = FW_TEXT_START;
+
+	PROVIDE(_fw_start = .);
+
+	. = ALIGN(0x1000); /* Need this to create proper sections */
+
+	/* Beginning of the code section */
+
+	.text :
+ 	{
+		PROVIDE(_text_start = .);
+		*(.entry)
+		*(.text)
+		. = ALIGN(8);
+		PROVIDE(_text_end = .);
+	}
+
+	. = ALIGN(0x1000); /* Ensure next section is page aligned */
+
+	/* End of the code sections */
+
+	/* Beginning of the read-only data sections */
+
+	. = ALIGN(0x1000); /* Ensure next section is page aligned */
+
+	.rodata :
+	{
+		PROVIDE(_rodata_start = .);
+		*(.rodata .rodata.*)
+		. = ALIGN(8);
+		PROVIDE(_rodata_end = .);
+	}
+
+	/* End of the read-only data sections */
+
+	/* Beginning of the read-write data sections */
+
+	. = ALIGN(0x1000); /* Ensure next section is page aligned */
+
+	.data :
+	{
+		PROVIDE(_data_start = .);
+
+		*(.sdata)
+		*(.sdata.*)
+		*(.data)
+		*(.data.*)
+		*(.readmostly.data)
+		*(*.data)
+		. = ALIGN(8);
+
+		PROVIDE(_data_end = .);
+	}
+
+	. = ALIGN(0x1000); /* Ensure next section is page aligned */
+
+	.bss :
+	{
+		PROVIDE(_bss_start = .);
+		*(.sbss)
+		*(.sbss.*)
+		*(.bss)
+		*(.bss.*)
+		. = ALIGN(8);
+		PROVIDE(_bss_end = .);
+	}
+
+	/* End of the read-write data sections */
+
+	. = ALIGN(0x1000); /* Need this to create proper sections */
+
+	PROVIDE(_fw_end = .);
diff --git a/roms/opensbi/firmware/fw_dynamic.S b/roms/opensbi/firmware/fw_dynamic.S
new file mode 100644
index 000000000..8b56947af
--- /dev/null
+++ b/roms/opensbi/firmware/fw_dynamic.S
@@ -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/fw_dynamic.h>
+
+#include "fw_base.S"
+
+	.section .entry, "ax", %progbits
+	.align 3
+_bad_dynamic_info:
+	wfi
+	j	_bad_dynamic_info
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_boot_hart
+	/*
+	 * This function is called very early even before
+	 * fw_save_info() is called.
+	 * We can only use a0, a1, and a2 registers here.
+	 * The boot HART id should be returned in 'a0'.
+	 */
+fw_boot_hart:
+	/* Sanity checks */
+	li	a1, FW_DYNAMIC_INFO_MAGIC_VALUE
+	REG_L	a0, FW_DYNAMIC_INFO_MAGIC_OFFSET(a2)
+	bne	a0, a1, _bad_dynamic_info
+	li	a1, FW_DYNAMIC_INFO_VERSION_MAX
+	REG_L	a0, FW_DYNAMIC_INFO_VERSION_OFFSET(a2)
+	bgt	a0, a1, _bad_dynamic_info
+
+	/* Read boot HART id */
+	li	a1, 0x2
+	blt	a0, a1, 2f
+	REG_L	a0, FW_DYNAMIC_INFO_BOOT_HART_OFFSET(a2)
+	ret
+2:	li	a0, -1
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_save_info
+	/*
+	 * We can only use a0, a1, a2, a3, and a4 registers here.
+	 * The a0, a1, and a2 registers will be same as passed by
+	 * previous booting stage.
+	 * Nothing to be returned here.
+	 */
+fw_save_info:
+	/* Save next arg1 in 'a1' */
+	la	a4, _dynamic_next_arg1
+	REG_S	a1, (a4)
+
+	/* Sanity checks */
+	li	a4, FW_DYNAMIC_INFO_MAGIC_VALUE
+	REG_L	a3, FW_DYNAMIC_INFO_MAGIC_OFFSET(a2)
+	bne	a3, a4, _bad_dynamic_info
+	li	a4, FW_DYNAMIC_INFO_VERSION_MAX
+	REG_L	a3, FW_DYNAMIC_INFO_VERSION_OFFSET(a2)
+	bgt	a3, a4, _bad_dynamic_info
+
+	/* Save version == 0x1 fields */
+	la	a4, _dynamic_next_addr
+	REG_L	a3, FW_DYNAMIC_INFO_NEXT_ADDR_OFFSET(a2)
+	REG_S	a3, (a4)
+	la	a4, _dynamic_next_mode
+	REG_L	a3, FW_DYNAMIC_INFO_NEXT_MODE_OFFSET(a2)
+	REG_S	a3, (a4)
+	la	a4, _dynamic_options
+	REG_L	a3, FW_DYNAMIC_INFO_OPTIONS_OFFSET(a2)
+	REG_S	a3, (a4)
+
+	/* Save version == 0x2 fields */
+	li	a4, 0x2
+	REG_L	a3, FW_DYNAMIC_INFO_VERSION_OFFSET(a2)
+	blt	a3, a4, 2f
+	la	a4, _dynamic_boot_hart
+	REG_L	a3, FW_DYNAMIC_INFO_BOOT_HART_OFFSET(a2)
+	REG_S	a3, (a4)
+2:
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_next_arg1
+	/*
+	 * We can only use a0, a1, and a2 registers here.
+	 * The a0, a1, and a2 registers will be same as passed by
+	 * previous booting stage.
+	 * The next arg1 should be returned in 'a0'.
+	 */
+fw_next_arg1:
+	la	a0, _dynamic_next_arg1
+	REG_L	a0, (a0)
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_next_addr
+	/*
+	 * We can only use a0, a1, and a2 registers here.
+	 * The next address should be returned in 'a0'.
+	 */
+fw_next_addr:
+	la	a0, _dynamic_next_addr
+	REG_L	a0, (a0)
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_next_mode
+	/*
+	 * We can only use a0, a1, and a2 registers here.
+	 * The next address should be returned in 'a0'
+	 */
+fw_next_mode:
+	la	a0, _dynamic_next_mode
+	REG_L	a0, (a0)
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_options
+	/*
+	 * We can only use a0, a1, and a2 registers here.
+	 * The 'a4' register will have default options.
+	 * The next address should be returned in 'a0'.
+	 */
+fw_options:
+	la	a0, _dynamic_options
+	REG_L	a0, (a0)
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+_dynamic_next_arg1:
+	RISCV_PTR 0x0
+_dynamic_next_addr:
+	RISCV_PTR 0x0
+_dynamic_next_mode:
+	RISCV_PTR PRV_S
+_dynamic_options:
+	RISCV_PTR 0x0
+_dynamic_boot_hart:
+	RISCV_PTR -1
diff --git a/roms/opensbi/firmware/fw_dynamic.elf.ldS b/roms/opensbi/firmware/fw_dynamic.elf.ldS
new file mode 100644
index 000000000..d1e2ea89e
--- /dev/null
+++ b/roms/opensbi/firmware/fw_dynamic.elf.ldS
@@ -0,0 +1,18 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+OUTPUT_ARCH(riscv)
+ENTRY(_start)
+
+SECTIONS
+{
+	#include "fw_base.ldS"
+
+	PROVIDE(_fw_reloc_end = .);
+}
diff --git a/roms/opensbi/firmware/fw_jump.S b/roms/opensbi/firmware/fw_jump.S
new file mode 100644
index 000000000..8553f8c7f
--- /dev/null
+++ b/roms/opensbi/firmware/fw_jump.S
@@ -0,0 +1,96 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include "fw_base.S"
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_boot_hart
+	/*
+	 * This function is called very early even before
+	 * fw_save_info() is called.
+	 * We can only use a0, a1, and a2 registers here.
+	 * The boot HART id should be returned in 'a0'.
+	 */
+fw_boot_hart:
+	li	a0, -1
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_save_info
+	/*
+	 * We can only use a0, a1, a2, a3, and a4 registers here.
+	 * The a0, a1, and a2 registers will be same as passed by
+	 * previous booting stage.
+	 * Nothing to be returned here.
+	 */
+fw_save_info:
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_next_arg1
+	/*
+	 * We can only use a0, a1, and a2 registers here.
+	 * The a0, a1, and a2 registers will be same as passed by
+	 * previous booting stage.
+	 * The next arg1 should be returned in 'a0'.
+	 */
+fw_next_arg1:
+#ifdef FW_JUMP_FDT_ADDR
+	li	a0, FW_JUMP_FDT_ADDR
+#else
+	add	a0, a1, zero
+#endif
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_next_addr
+	/*
+	 * We can only use a0, a1, and a2 registers here.
+	 * The next address should be returned in 'a0'.
+	 */
+fw_next_addr:
+	la	a0, _jump_addr
+	REG_L	a0, (a0)
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_next_mode
+	/*
+	 * We can only use a0, a1, and a2 registers here.
+	 * The next address should be returned in 'a0'
+	 */
+fw_next_mode:
+	li	a0, PRV_S
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_options
+	/*
+	 * We can only use a0, a1, and a2 registers here.
+	 * The 'a4' register will have default options.
+	 * The next address should be returned in 'a0'.
+	 */
+fw_options:
+	add	a0, zero, zero
+	ret
+
+#ifndef FW_JUMP_ADDR
+#error "Must define FW_JUMP_ADDR"
+#endif
+
+	.section .entry, "ax", %progbits
+	.align 3
+_jump_addr:
+	RISCV_PTR FW_JUMP_ADDR
diff --git a/roms/opensbi/firmware/fw_jump.elf.ldS b/roms/opensbi/firmware/fw_jump.elf.ldS
new file mode 100644
index 000000000..d1e2ea89e
--- /dev/null
+++ b/roms/opensbi/firmware/fw_jump.elf.ldS
@@ -0,0 +1,18 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+OUTPUT_ARCH(riscv)
+ENTRY(_start)
+
+SECTIONS
+{
+	#include "fw_base.ldS"
+
+	PROVIDE(_fw_reloc_end = .);
+}
diff --git a/roms/opensbi/firmware/fw_payload.S b/roms/opensbi/firmware/fw_payload.S
new file mode 100644
index 000000000..1ef121e28
--- /dev/null
+++ b/roms/opensbi/firmware/fw_payload.S
@@ -0,0 +1,97 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#include "fw_base.S"
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_boot_hart
+	/*
+	 * This function is called very early even before
+	 * fw_save_info() is called.
+	 * We can only use a0, a1, and a2 registers here.
+	 * The boot HART id should be returned in 'a0'.
+	 */
+fw_boot_hart:
+	li	a0, -1
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_save_info
+	/*
+	 * We can only use a0, a1, a2, a3, and a4 registers here.
+	 * The a0, a1, and a2 registers will be same as passed by
+	 * previous booting stage.
+	 * Nothing to be returned here.
+	 */
+fw_save_info:
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_next_arg1
+	/*
+	 * We can only use a0, a1, and a2 registers here.
+	 * The a0, a1, and a2 registers will be same as passed by
+	 * previous booting stage.
+	 * The next arg1 should be returned in 'a0'.
+	 */
+fw_next_arg1:
+#ifdef FW_PAYLOAD_FDT_ADDR
+	li	a0, FW_PAYLOAD_FDT_ADDR
+#else
+	add	a0, a1, zero
+#endif
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_next_addr
+	/*
+	 * We can only use a0, a1, and a2 registers here.
+	 * The next address should be returned in 'a0'.
+	 */
+fw_next_addr:
+	la	a0, payload_bin
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_next_mode
+	/*
+	 * We can only use a0, a1, and a2 registers here.
+	 * The next address should be returned in 'a0'.
+	 */
+fw_next_mode:
+	li	a0, PRV_S
+	ret
+
+	.section .entry, "ax", %progbits
+	.align 3
+	.global fw_options
+	/*
+	 * We can only use a0, a1, and a2 registers here.
+	 * The 'a4' register will have default options.
+	 * The next address should be returned in 'a0'.
+	 */
+fw_options:
+	add	a0, zero, zero
+	ret
+
+	.section .payload, "ax", %progbits
+	.align 4
+	.globl payload_bin
+payload_bin:
+#ifndef FW_PAYLOAD_PATH
+	wfi
+	j	payload_bin
+#else
+	.incbin	FW_PAYLOAD_PATH
+#endif
diff --git a/roms/opensbi/firmware/fw_payload.elf.ldS b/roms/opensbi/firmware/fw_payload.elf.ldS
new file mode 100644
index 000000000..f1a544b80
--- /dev/null
+++ b/roms/opensbi/firmware/fw_payload.elf.ldS
@@ -0,0 +1,32 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+OUTPUT_ARCH(riscv)
+ENTRY(_start)
+
+SECTIONS
+{
+	#include "fw_base.ldS"
+
+#ifdef FW_PAYLOAD_OFFSET
+	. = FW_TEXT_START + FW_PAYLOAD_OFFSET;
+#else
+	. = ALIGN(FW_PAYLOAD_ALIGN);
+#endif
+
+	.payload :
+	{
+		PROVIDE(_payload_start = .);
+		*(.payload)
+		. = ALIGN(8);
+		PROVIDE(_payload_end = .);
+	}
+
+	PROVIDE(_fw_reloc_end = .);
+}
diff --git a/roms/opensbi/firmware/objects.mk b/roms/opensbi/firmware/objects.mk
new file mode 100644
index 000000000..b2ace7512
--- /dev/null
+++ b/roms/opensbi/firmware/objects.mk
@@ -0,0 +1,57 @@
+#
+# SPDX-License-Identifier: BSD-2-Clause
+#
+# Copyright (c) 2019 Western Digital Corporation or its affiliates.
+#
+# Authors:
+#   Anup Patel <anup.patel@wdc.com>
+#
+
+firmware-genflags-y =
+firmware-cppflags-y +=
+firmware-cflags-y +=
+firmware-asflags-y +=
+firmware-ldflags-y +=
+
+ifdef FW_TEXT_START
+firmware-genflags-y += -DFW_TEXT_START=$(FW_TEXT_START)
+endif
+
+ifdef FW_FDT_PATH
+firmware-genflags-y += -DFW_FDT_PATH=\"$(FW_FDT_PATH)\"
+ifdef FW_FDT_PADDING
+firmware-genflags-y += -DFW_FDT_PADDING=$(FW_FDT_PADDING)
+endif
+endif
+
+firmware-bins-$(FW_DYNAMIC) += fw_dynamic.bin
+
+firmware-bins-$(FW_JUMP) += fw_jump.bin
+ifdef FW_JUMP_ADDR
+firmware-genflags-$(FW_JUMP) += -DFW_JUMP_ADDR=$(FW_JUMP_ADDR)
+endif
+ifdef FW_JUMP_FDT_ADDR
+firmware-genflags-$(FW_JUMP) += -DFW_JUMP_FDT_ADDR=$(FW_JUMP_FDT_ADDR)
+endif
+
+firmware-bins-$(FW_PAYLOAD) += fw_payload.bin
+ifdef FW_PAYLOAD_PATH
+FW_PAYLOAD_PATH_FINAL=$(FW_PAYLOAD_PATH)
+else
+FW_PAYLOAD_PATH_FINAL=$(platform_build_dir)/firmware/payloads/test.bin
+endif
+firmware-genflags-$(FW_PAYLOAD) += -DFW_PAYLOAD_PATH=\"$(FW_PAYLOAD_PATH_FINAL)\"
+ifdef FW_PAYLOAD_OFFSET
+firmware-genflags-$(FW_PAYLOAD) += -DFW_PAYLOAD_OFFSET=$(FW_PAYLOAD_OFFSET)
+endif
+ifdef FW_PAYLOAD_ALIGN
+firmware-genflags-$(FW_PAYLOAD) += -DFW_PAYLOAD_ALIGN=$(FW_PAYLOAD_ALIGN)
+endif
+
+ifdef FW_PAYLOAD_FDT_ADDR
+firmware-genflags-$(FW_PAYLOAD) += -DFW_PAYLOAD_FDT_ADDR=$(FW_PAYLOAD_FDT_ADDR)
+endif
+
+ifdef FW_OPTIONS
+firmware-genflags-y += -DFW_OPTIONS=$(FW_OPTIONS)
+endif
diff --git a/roms/opensbi/firmware/payloads/objects.mk b/roms/opensbi/firmware/payloads/objects.mk
new file mode 100644
index 000000000..21e0185a8
--- /dev/null
+++ b/roms/opensbi/firmware/payloads/objects.mk
@@ -0,0 +1,19 @@
+#
+# SPDX-License-Identifier: BSD-2-Clause
+#
+# Copyright (c) 2019 Western Digital Corporation or its affiliates.
+#
+# Authors:
+#   Anup Patel <anup.patel@wdc.com>
+#
+
+firmware-bins-$(FW_PAYLOAD) += payloads/test.bin
+
+test-y += test_head.o
+test-y += test_main.o
+
+%/test.o: $(foreach obj,$(test-y),%/$(obj))
+	$(call merge_objs,$@,$^)
+
+%/test.dep: $(foreach dep,$(test-y:.o=.dep),%/$(dep))
+	$(call merge_deps,$@,$^)
diff --git a/roms/opensbi/firmware/payloads/test.elf.ldS b/roms/opensbi/firmware/payloads/test.elf.ldS
new file mode 100644
index 000000000..f3f3242ab
--- /dev/null
+++ b/roms/opensbi/firmware/payloads/test.elf.ldS
@@ -0,0 +1,87 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+OUTPUT_ARCH(riscv)
+ENTRY(_start)
+
+SECTIONS
+{
+#ifdef FW_PAYLOAD_OFFSET
+	. = FW_TEXT_START + FW_PAYLOAD_OFFSET;
+#else
+	. = ALIGN(FW_PAYLOAD_ALIGN);
+#endif
+
+	PROVIDE(_payload_start = .);
+
+	. = ALIGN(0x1000); /* Need this to create proper sections */
+
+	/* Beginning of the code section */
+
+	.text :
+	{
+		PROVIDE(_text_start = .);
+		*(.entry)
+		*(.text)
+		. = ALIGN(8);
+		PROVIDE(_text_end = .);
+	}
+
+	. = ALIGN(0x1000); /* Ensure next section is page aligned */
+
+	/* End of the code sections */
+
+	/* Beginning of the read-only data sections */
+
+	. = ALIGN(0x1000); /* Ensure next section is page aligned */
+
+	.rodata :
+	{
+		PROVIDE(_rodata_start = .);
+		*(.rodata .rodata.*)
+		. = ALIGN(8);
+		PROVIDE(_rodata_end = .);
+	}
+
+	/* End of the read-only data sections */
+
+	/* Beginning of the read-write data sections */
+
+	. = ALIGN(0x1000); /* Ensure next section is page aligned */
+
+	.data :
+	{
+		PROVIDE(_data_start = .);
+
+		*(.data)
+		*(.data.*)
+		*(.readmostly.data)
+		*(*.data)
+		. = ALIGN(8);
+
+		PROVIDE(_data_end = .);
+	}
+
+	. = ALIGN(0x1000); /* Ensure next section is page aligned */
+
+	.bss :
+	{
+		PROVIDE(_bss_start = .);
+		*(.bss)
+		*(.bss.*)
+		. = ALIGN(8);
+		PROVIDE(_bss_end = .);
+	}
+
+	/* End of the read-write data sections */
+
+	. = ALIGN(0x1000); /* Need this to create proper sections */
+
+	PROVIDE(_payload_end = .);
+}
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
diff --git a/roms/opensbi/firmware/payloads/test_main.c b/roms/opensbi/firmware/payloads/test_main.c
new file mode 100644
index 000000000..0d6593023
--- /dev/null
+++ b/roms/opensbi/firmware/payloads/test_main.c
@@ -0,0 +1,48 @@
+/*
+ * 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_ecall_interface.h>
+
+#define SBI_ECALL(__num, __a0, __a1, __a2)                                    \
+	({                                                                    \
+		register unsigned long a0 asm("a0") = (unsigned long)(__a0);  \
+		register unsigned long a1 asm("a1") = (unsigned long)(__a1);  \
+		register unsigned long a2 asm("a2") = (unsigned long)(__a2);  \
+		register unsigned long a7 asm("a7") = (unsigned long)(__num); \
+		asm volatile("ecall"                                          \
+			     : "+r"(a0)                                       \
+			     : "r"(a1), "r"(a2), "r"(a7)                      \
+			     : "memory");                                     \
+		a0;                                                           \
+	})
+
+#define SBI_ECALL_0(__num) SBI_ECALL(__num, 0, 0, 0)
+#define SBI_ECALL_1(__num, __a0) SBI_ECALL(__num, __a0, 0, 0)
+#define SBI_ECALL_2(__num, __a0, __a1) SBI_ECALL(__num, __a0, __a1, 0)
+
+#define sbi_ecall_console_putc(c) SBI_ECALL_1(SBI_EXT_0_1_CONSOLE_PUTCHAR, (c))
+
+static inline void sbi_ecall_console_puts(const char *str)
+{
+	while (str && *str)
+		sbi_ecall_console_putc(*str++);
+}
+
+#define wfi()                                             \
+	do {                                              \
+		__asm__ __volatile__("wfi" ::: "memory"); \
+	} while (0)
+
+void test_main(unsigned long a0, unsigned long a1)
+{
+	sbi_ecall_console_puts("\nTest payload running\n");
+
+	while (1)
+		wfi();
+}