forked from Mirrors/opensbi
8a1475b5a1
If the device tree is at an address that is not __SIZEOF_POINTER__ aligned, the fdt relocation code tries to align both source and destination address to __SIZEOF_POINTER__ before the memory copy. But such alignment can lead to unexpected results if either source or destination address is not aligned. In fact libfdt requires that the device tree must be at an 8-byte aligned address. Hence remove the unhelpful alignment codes. Signed-off-by: Bin Meng <bmeng.cn@gmail.com> Reviewed-by: Anup Patel <anup.patel@wdc.com>
808 lines
18 KiB
ArmAsm
808 lines
18 KiB
ArmAsm
/*
|
|
* 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_elf.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 */
|
|
lla a6, _relocate_lottery
|
|
li a7, 1
|
|
amoadd.w a6, a7, (a6)
|
|
bnez a6, _wait_relocate_copy_done
|
|
|
|
/* Save load address */
|
|
lla t0, _load_start
|
|
lla t1, _start
|
|
REG_S t1, 0(t0)
|
|
|
|
#ifdef FW_PIC
|
|
/* relocate the global table content */
|
|
lla t0, _link_start
|
|
REG_L t0, 0(t0)
|
|
/* t1 shall has the address of _start */
|
|
sub t2, t1, t0
|
|
lla t3, _runtime_offset
|
|
REG_S t2, (t3)
|
|
lla t0, __rel_dyn_start
|
|
lla t1, __rel_dyn_end
|
|
beq t0, t1, _relocate_done
|
|
j 5f
|
|
2:
|
|
REG_L t5, -(REGBYTES*2)(t0) /* t5 <-- relocation info:type */
|
|
li t3, R_RISCV_RELATIVE /* reloc type R_RISCV_RELATIVE */
|
|
bne t5, t3, 3f
|
|
REG_L t3, -(REGBYTES*3)(t0)
|
|
REG_L t5, -(REGBYTES)(t0) /* t5 <-- addend */
|
|
add t5, t5, t2
|
|
add t3, t3, t2
|
|
REG_S t5, 0(t3) /* store runtime address to the GOT entry */
|
|
j 5f
|
|
|
|
3:
|
|
lla t4, __dyn_sym_start
|
|
|
|
4:
|
|
REG_L t5, -(REGBYTES*2)(t0) /* t5 <-- relocation info:type */
|
|
srli t6, t5, SYM_INDEX /* t6 <--- sym table index */
|
|
andi t5, t5, 0xFF /* t5 <--- relocation type */
|
|
li t3, RELOC_TYPE
|
|
bne t5, t3, 5f
|
|
|
|
/* address R_RISCV_64 or R_RISCV_32 cases*/
|
|
REG_L t3, -(REGBYTES*3)(t0)
|
|
li t5, SYM_SIZE
|
|
mul t6, t6, t5
|
|
add s5, t4, t6
|
|
REG_L t6, -(REGBYTES)(t0) /* t0 <-- addend */
|
|
REG_L t5, REGBYTES(s5)
|
|
add t5, t5, t6
|
|
add t5, t5, t2 /* t5 <-- location to fix up in RAM */
|
|
add t3, t3, t2 /* t3 <-- location to fix up in RAM */
|
|
REG_S t5, 0(t3) /* store runtime address to the variable */
|
|
|
|
5:
|
|
addi t0, t0, (REGBYTES*3)
|
|
ble t0, t1, 2b
|
|
j _relocate_done
|
|
_wait_relocate_copy_done:
|
|
j _wait_for_boot_hart
|
|
#else
|
|
/* Relocate if load address != link address */
|
|
_relocate:
|
|
lla t0, _link_start
|
|
REG_L t0, 0(t0)
|
|
lla t1, _link_end
|
|
REG_L t1, 0(t1)
|
|
lla t2, _load_start
|
|
REG_L t2, 0(t2)
|
|
sub t3, t1, t0
|
|
add t3, t3, t2
|
|
beq t0, t2, _relocate_done
|
|
lla t4, _relocate_done
|
|
sub t4, t4, t2
|
|
add t4, t4, t0
|
|
blt t2, t0, _relocate_copy_to_upper
|
|
_relocate_copy_to_lower:
|
|
ble t1, t2, _relocate_copy_to_lower_loop
|
|
lla t3, _relocate_lottery
|
|
BRANGE t2, t1, t3, _start_hang
|
|
lla t3, _boot_status
|
|
BRANGE t2, t1, t3, _start_hang
|
|
lla t3, _relocate
|
|
lla t5, _relocate_done
|
|
BRANGE t2, t1, t3, _start_hang
|
|
BRANGE t2, t1, t5, _start_hang
|
|
BRANGE t3, t5, t2, _start_hang
|
|
_relocate_copy_to_lower_loop:
|
|
REG_L t3, 0(t2)
|
|
REG_S t3, 0(t0)
|
|
add t0, t0, __SIZEOF_POINTER__
|
|
add t2, t2, __SIZEOF_POINTER__
|
|
blt t0, t1, _relocate_copy_to_lower_loop
|
|
jr t4
|
|
_relocate_copy_to_upper:
|
|
ble t3, t0, _relocate_copy_to_upper_loop
|
|
lla t2, _relocate_lottery
|
|
BRANGE t0, t3, t2, _start_hang
|
|
lla t2, _boot_status
|
|
BRANGE t0, t3, t2, _start_hang
|
|
lla t2, _relocate
|
|
lla t5, _relocate_done
|
|
BRANGE t0, t3, t2, _start_hang
|
|
BRANGE t0, t3, t5, _start_hang
|
|
BRANGE t2, t5, t0, _start_hang
|
|
_relocate_copy_to_upper_loop:
|
|
add t3, t3, -__SIZEOF_POINTER__
|
|
add t1, t1, -__SIZEOF_POINTER__
|
|
REG_L t2, 0(t3)
|
|
REG_S t2, 0(t1)
|
|
blt t0, t1, _relocate_copy_to_upper_loop
|
|
jr t4
|
|
_wait_relocate_copy_done:
|
|
lla t0, _start
|
|
lla t1, _link_start
|
|
REG_L t1, 0(t1)
|
|
beq t0, t1, _wait_for_boot_hart
|
|
lla t2, _boot_status
|
|
lla t3, _wait_for_boot_hart
|
|
sub t3, t3, t0
|
|
add t3, t3, t1
|
|
1:
|
|
/* waitting for relocate copy done (_boot_status == 1) */
|
|
li t4, BOOT_STATUS_RELOCATE_DONE
|
|
REG_L t5, 0(t2)
|
|
/* Reduce the bus traffic so that boot hart may proceed faster */
|
|
nop
|
|
nop
|
|
nop
|
|
bgt t4, t5, 1b
|
|
jr t3
|
|
#endif
|
|
_relocate_done:
|
|
|
|
/*
|
|
* Mark relocate copy done
|
|
* Use _boot_status copy relative to the load address
|
|
*/
|
|
lla t0, _boot_status
|
|
#ifndef FW_PIC
|
|
lla t1, _link_start
|
|
REG_L t1, 0(t1)
|
|
lla t2, _load_start
|
|
REG_L t2, 0(t2)
|
|
sub t0, t0, t1
|
|
add t0, t0, t2
|
|
#endif
|
|
li t1, BOOT_STATUS_RELOCATE_DONE
|
|
REG_S t1, 0(t0)
|
|
fence rw, rw
|
|
|
|
/* At this point we are running from link address */
|
|
|
|
/* Reset all registers for boot HART */
|
|
li ra, 0
|
|
call _reset_regs
|
|
|
|
/* Zero-out BSS */
|
|
lla s4, _bss_start
|
|
lla s5, _bss_end
|
|
_bss_zero:
|
|
REG_S zero, (s4)
|
|
add s4, s4, __SIZEOF_POINTER__
|
|
blt s4, s5, _bss_zero
|
|
|
|
/* Setup temporary trap handler */
|
|
lla s4, _start_hang
|
|
csrw CSR_MTVEC, s4
|
|
|
|
/* Setup temporary stack */
|
|
lla 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 */
|
|
lla 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
|
|
*/
|
|
lla 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*/
|
|
lla 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:
|
|
/*
|
|
* The following registers hold values that are computed before
|
|
* entering this block, and should remain unchanged.
|
|
*
|
|
* t3 -> the firmware end address
|
|
* s7 -> HART count
|
|
* s8 -> HART stack size
|
|
*/
|
|
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 */
|
|
lla a4, _fw_start
|
|
sub a5, t3, 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 */
|
|
lla a4, _start_warm
|
|
REG_S a4, SBI_SCRATCH_WARMBOOT_ADDR_OFFSET(tp)
|
|
/* Store platform address in scratch space */
|
|
lla a4, platform
|
|
REG_S a4, SBI_SCRATCH_PLATFORM_ADDR_OFFSET(tp)
|
|
/* Store hartid-to-scratch function address in scratch space */
|
|
lla a4, _hartid_to_scratch
|
|
REG_S a4, SBI_SCRATCH_HARTID_TO_SCRATCH_OFFSET(tp)
|
|
/* Store trap-exit function address in scratch space */
|
|
lla a4, _trap_exit
|
|
REG_S a4, SBI_SCRATCH_TRAP_EXIT_OFFSET(tp)
|
|
/* Clear tmp0 in scratch space */
|
|
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 a4 */
|
|
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
|
|
/* t0 = source FDT start address */
|
|
add t0, a1, zero
|
|
/* 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
|
|
lla 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
|
|
lla 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 */
|
|
lla 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 */
|
|
lla 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 */
|
|
lla a4, _trap_handler
|
|
#if __riscv_xlen == 32
|
|
csrr a5, CSR_MISA
|
|
srli a5, a5, ('H' - 'A')
|
|
andi a5, a5, 0x1
|
|
beq a5, zero, _skip_trap_handler_rv32_hyp
|
|
lla a4, _trap_handler_rv32_hyp
|
|
_skip_trap_handler_rv32_hyp:
|
|
#endif
|
|
csrw CSR_MTVEC, a4
|
|
|
|
#if __riscv_xlen == 32
|
|
/* Override trap exit for H-extension */
|
|
csrr a5, CSR_MISA
|
|
srli a5, a5, ('H' - 'A')
|
|
andi a5, a5, 0x1
|
|
beq a5, zero, _skip_trap_exit_rv32_hyp
|
|
lla a4, _trap_exit_rv32_hyp
|
|
csrr a5, CSR_MSCRATCH
|
|
REG_S a4, SBI_SCRATCH_TRAP_EXIT_OFFSET(a5)
|
|
_skip_trap_exit_rv32_hyp:
|
|
#endif
|
|
|
|
/* Initialize SBI runtime */
|
|
csrr a0, CSR_MSCRATCH
|
|
call sbi_init
|
|
|
|
/* We don't expect to reach here hence just hang */
|
|
j _start_hang
|
|
|
|
.data
|
|
.align 3
|
|
#ifdef FW_PIC
|
|
_runtime_offset:
|
|
RISCV_PTR 0
|
|
#endif
|
|
_relocate_lottery:
|
|
RISCV_PTR 0
|
|
_boot_status:
|
|
RISCV_PTR 0
|
|
_load_start:
|
|
RISCV_PTR _fw_start
|
|
_link_start:
|
|
RISCV_PTR FW_TEXT_START
|
|
_link_end:
|
|
RISCV_PTR _fw_reloc_end
|
|
|
|
.section .entry, "ax", %progbits
|
|
.align 3
|
|
.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
|
|
*/
|
|
lla 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
|
|
lla 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
|
|
.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_A0_T0
|
|
/* Restore all general regisers except A0 and T0 */
|
|
REG_L ra, SBI_TRAP_REGS_OFFSET(ra)(a0)
|
|
REG_L sp, SBI_TRAP_REGS_OFFSET(sp)(a0)
|
|
REG_L gp, SBI_TRAP_REGS_OFFSET(gp)(a0)
|
|
REG_L tp, SBI_TRAP_REGS_OFFSET(tp)(a0)
|
|
REG_L t1, SBI_TRAP_REGS_OFFSET(t1)(a0)
|
|
REG_L t2, SBI_TRAP_REGS_OFFSET(t2)(a0)
|
|
REG_L s0, SBI_TRAP_REGS_OFFSET(s0)(a0)
|
|
REG_L s1, SBI_TRAP_REGS_OFFSET(s1)(a0)
|
|
REG_L a1, SBI_TRAP_REGS_OFFSET(a1)(a0)
|
|
REG_L a2, SBI_TRAP_REGS_OFFSET(a2)(a0)
|
|
REG_L a3, SBI_TRAP_REGS_OFFSET(a3)(a0)
|
|
REG_L a4, SBI_TRAP_REGS_OFFSET(a4)(a0)
|
|
REG_L a5, SBI_TRAP_REGS_OFFSET(a5)(a0)
|
|
REG_L a6, SBI_TRAP_REGS_OFFSET(a6)(a0)
|
|
REG_L a7, SBI_TRAP_REGS_OFFSET(a7)(a0)
|
|
REG_L s2, SBI_TRAP_REGS_OFFSET(s2)(a0)
|
|
REG_L s3, SBI_TRAP_REGS_OFFSET(s3)(a0)
|
|
REG_L s4, SBI_TRAP_REGS_OFFSET(s4)(a0)
|
|
REG_L s5, SBI_TRAP_REGS_OFFSET(s5)(a0)
|
|
REG_L s6, SBI_TRAP_REGS_OFFSET(s6)(a0)
|
|
REG_L s7, SBI_TRAP_REGS_OFFSET(s7)(a0)
|
|
REG_L s8, SBI_TRAP_REGS_OFFSET(s8)(a0)
|
|
REG_L s9, SBI_TRAP_REGS_OFFSET(s9)(a0)
|
|
REG_L s10, SBI_TRAP_REGS_OFFSET(s10)(a0)
|
|
REG_L s11, SBI_TRAP_REGS_OFFSET(s11)(a0)
|
|
REG_L t3, SBI_TRAP_REGS_OFFSET(t3)(a0)
|
|
REG_L t4, SBI_TRAP_REGS_OFFSET(t4)(a0)
|
|
REG_L t5, SBI_TRAP_REGS_OFFSET(t5)(a0)
|
|
REG_L t6, SBI_TRAP_REGS_OFFSET(t6)(a0)
|
|
.endm
|
|
|
|
.macro TRAP_RESTORE_MEPC_MSTATUS have_mstatush
|
|
/* Restore MEPC and MSTATUS CSRs */
|
|
REG_L t0, SBI_TRAP_REGS_OFFSET(mepc)(a0)
|
|
csrw CSR_MEPC, t0
|
|
REG_L t0, SBI_TRAP_REGS_OFFSET(mstatus)(a0)
|
|
csrw CSR_MSTATUS, t0
|
|
.if \have_mstatush
|
|
REG_L t0, SBI_TRAP_REGS_OFFSET(mstatusH)(a0)
|
|
csrw CSR_MSTATUSH, t0
|
|
.endif
|
|
.endm
|
|
|
|
.macro TRAP_RESTORE_A0_T0
|
|
/* Restore T0 */
|
|
REG_L t0, SBI_TRAP_REGS_OFFSET(t0)(a0)
|
|
|
|
/* Restore A0 */
|
|
REG_L a0, SBI_TRAP_REGS_OFFSET(a0)(a0)
|
|
.endm
|
|
|
|
.section .entry, "ax", %progbits
|
|
.align 3
|
|
.globl _trap_handler
|
|
.globl _trap_exit
|
|
_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_exit:
|
|
TRAP_RESTORE_GENERAL_REGS_EXCEPT_A0_T0
|
|
|
|
TRAP_RESTORE_MEPC_MSTATUS 0
|
|
|
|
TRAP_RESTORE_A0_T0
|
|
|
|
mret
|
|
|
|
#if __riscv_xlen == 32
|
|
.section .entry, "ax", %progbits
|
|
.align 3
|
|
.globl _trap_handler_rv32_hyp
|
|
.globl _trap_exit_rv32_hyp
|
|
_trap_handler_rv32_hyp:
|
|
TRAP_SAVE_AND_SETUP_SP_T0
|
|
|
|
TRAP_SAVE_MEPC_MSTATUS 1
|
|
|
|
TRAP_SAVE_GENERAL_REGS_EXCEPT_SP_T0
|
|
|
|
TRAP_CALL_C_ROUTINE
|
|
|
|
_trap_exit_rv32_hyp:
|
|
TRAP_RESTORE_GENERAL_REGS_EXCEPT_A0_T0
|
|
|
|
TRAP_RESTORE_MEPC_MSTATUS 1
|
|
|
|
TRAP_RESTORE_A0_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
|