NANOKVM-LicheeRV-Nano-Build-MIRROR/fsbl/lib/cpu/aarch64/bl2_helper.c

#include <debug.h>
#include <console.h>
#include <platform.h>
#include <string.h>
#include <aarch64/bl2_helper.h>
#include <bl2.h>
#include <delay_timer.h>
#include <mmio.h>

#define GICD_BASE 0x01F01000U
#define GICC_BASE 0x01F02000U

/*
 * Hand the GIC over to a non-secure single-OS image (RT-Thread et al).
 *
 * Scope: AArch64 only — this file is compiled solely for BOOT_CPU=aarch64
 * (Makefile includes lib/cpu/${BOOT_CPU}/cpu.mk), and the only caller is
 * jump_to_loader_2nd() below, i.e. the direct-jump path taken when the fip
 * carries no monitor (load_monitor() leaves monitor_entry at 0). Monitor
 * flows (dual-OS / BL31) go jump_to_monitor() -> jump_bl31() and never reach
 * this code, since the monitor reconfigures the GIC itself.
 *
 * Why it must happen here, at EL3: GICD_IGROUPR and GICD_NSACR for SGI/PPI
 * are banked-secure registers. GIC reset state puts every interrupt in
 * Group0 (secure, FIQ-only), and the NS-side driver (e.g. RT-Thread's
 * libcpu/aarch64 gic.c, running at EL1) cannot move them to Group1 — its
 * IGROUPR writes are ignored. Only secure EL3 can do the hand-off, so if it
 * is skipped the NS image boots with no interrupts at all (no tick).
 */
static void gic_secure_init_for_ns_el1(void)
{
	unsigned int i;

	mmio_write_32(GICD_BASE + 0x000, 0x0);

	/* Make all SGI/PPI/SPI Group1 so NS EL1 can receive them as IRQ. */
	for (i = 0; i < 5; i++) {
		mmio_write_32(GICD_BASE + 0x080 + i * 4, 0xffffffff);
	}

	/* Let NS access SGI/PPI control where GICv2 security extension enforces it. */
	mmio_write_32(GICD_BASE + 0x0e00, 0xffffffff); /* NSACR0 */

	mmio_write_32(GICD_BASE + 0x000, 0x3);

	mmio_write_32(GICC_BASE + 0x004, 0xff);
	mmio_write_32(GICC_BASE + 0x008, 0x7);
	mmio_write_32(GICC_BASE + 0x000, 0x3);

	__asm__ volatile ("dsb sy; isb" ::: "memory");
}


void switch_el3_to_el2_ns(uintptr_t bl2_entry)
{
	uint64_t spsr, mdcr_el2;

	NOTICE("Switch to EL2-NS at 0x%lx\n", bl2_entry);

	spsr = SPSR_64(MODE_EL2, 0, DISABLE_ALL_EXCEPTIONS);
	write_spsr_el3(spsr);

	/*
	 * Set the next EL to be AArch64.
	 * Switch to non-secure state.
	 */
	write_scr_el3(read_scr_el3() | SCR_RW_BIT | SCR_NS_BIT);

	/*
	 * EL2 present but unused, need to disable safely.
	 * SCTLR_EL2 can be ignored in this case.
	 *
	 * Initialise all fields in HCR_EL2, except HCR_EL2.RW,
	 * to zero so that Non-secure operations do not trap to
	 * EL2.
	 *
	 * HCR_EL2.RW: Set this field to match SCR_EL3.RW
	 */
	write_hcr_el2(read_hcr_el2() | HCR_RW_BIT);

	/*
	 * Initialise CPTR_EL2 setting all fields rather than
	 * relying on the hw. All fields have architecturally
	 * UNKNOWN reset values.
	 *
	 * CPTR_EL2.TCPAC: Set to zero so that Non-secure EL1
	 *  accesses to the CPACR_EL1 or CPACR from both
	 *  Execution states do not trap to EL2.
	 *
	 * CPTR_EL2.TTA: Set to zero so that Non-secure System
	 *  register accesses to the trace registers from both
	 *  Execution states do not trap to EL2.
	 *
	 * CPTR_EL2.TFP: Set to zero so that Non-secure accesses
	 *  to SIMD and floating-point functionality from both
	 *  Execution states do not trap to EL2.
	 */
	write_cptr_el2(CPTR_EL2_RESET_VAL & ~(CPTR_EL2_TCPAC_BIT | CPTR_EL2_TTA_BIT | CPTR_EL2_TFP_BIT));

	/*
	 * Initiliase CNTHCTL_EL2. All fields are
	 * architecturally UNKNOWN on reset and are set to zero
	 * except for field(s) listed below.
	 *
	 * CNTHCTL_EL2.EL1PCEN: Set to one to disable traps to
	 *  Hyp mode of Non-secure EL0 and EL1 accesses to the
	 *  physical timer registers.
	 *
	 * CNTHCTL_EL2.EL1PCTEN: Set to one to disable traps to
	 *  Hyp mode of  Non-secure EL0 and EL1 accesses to the
	 *  physical counter registers.
	 */
	write_cnthctl_el2(CNTHCTL_RESET_VAL | EL1PCEN_BIT | EL1PCTEN_BIT);

	/*
	 * Initialise CNTVOFF_EL2 to zero as it resets to an
	 * architecturally UNKNOWN value.
	 */
	write_cntvoff_el2(0);

	/*
	 * Set VPIDR_EL2 and VMPIDR_EL2 to match MIDR_EL1 and
	 * MPIDR_EL1 respectively.
	 */
	write_vpidr_el2(read_midr_el1());
	write_vmpidr_el2(read_mpidr_el1());

	/*
	 * Initialise VTTBR_EL2. All fields are architecturally
	 * UNKNOWN on reset.
	 *
	 * VTTBR_EL2.VMID: Set to zero. Even though EL1&0 stage
	 *  2 address translation is disabled, cache maintenance
	 *  operations depend on the VMID.
	 *
	 * VTTBR_EL2.BADDR: Set to zero as EL1&0 stage 2 address
	 *  translation is disabled.
	 */
	write_vttbr_el2(VTTBR_RESET_VAL &
			~((VTTBR_VMID_MASK << VTTBR_VMID_SHIFT) | (VTTBR_BADDR_MASK << VTTBR_BADDR_SHIFT)));

	/*
	 * Initialise MDCR_EL2, setting all fields rather than
	 * relying on hw. Some fields are architecturally
	 * UNKNOWN on reset.
	 *
	 * MDCR_EL2.TPMS (ARM v8.2): Do not trap statistical
	 * profiling controls to EL2.
	 *
	 * MDCR_EL2.E2PB (ARM v8.2): SPE enabled in non-secure
	 * state. Accesses to profiling buffer controls at
	 * non-secure EL1 are not trapped to EL2.
	 *
	 * MDCR_EL2.TDRA: Set to zero so that Non-secure EL0 and
	 *  EL1 System register accesses to the Debug ROM
	 *  registers are not trapped to EL2.
	 *
	 * MDCR_EL2.TDOSA: Set to zero so that Non-secure EL1
	 *  System register accesses to the powerdown debug
	 *  registers are not trapped to EL2.
	 *
	 * MDCR_EL2.TDA: Set to zero so that System register
	 *  accesses to the debug registers do not trap to EL2.
	 *
	 * MDCR_EL2.TDE: Set to zero so that debug exceptions
	 *  are not routed to EL2.
	 *
	 * MDCR_EL2.HPME: Set to zero to disable EL2 Performance
	 *  Monitors.
	 *
	 * MDCR_EL2.TPM: Set to zero so that Non-secure EL0 and
	 *  EL1 accesses to all Performance Monitors registers
	 *  are not trapped to EL2.
	 *
	 * MDCR_EL2.TPMCR: Set to zero so that Non-secure EL0
	 *  and EL1 accesses to the PMCR_EL0 or PMCR are not
	 *  trapped to EL2.
	 *
	 * MDCR_EL2.HPMN: Set to value of PMCR_EL0.N which is the
	 *  architecturally-defined reset value.
	 */
	mdcr_el2 = ((MDCR_EL2_RESET_VAL | ((read_pmcr_el0() & PMCR_EL0_N_BITS) >> PMCR_EL0_N_SHIFT)) &
		    ~(MDCR_EL2_TDRA_BIT | MDCR_EL2_TDOSA_BIT | MDCR_EL2_TDA_BIT | MDCR_EL2_TDE_BIT | MDCR_EL2_HPME_BIT |
		      MDCR_EL2_TPM_BIT | MDCR_EL2_TPMCR_BIT));

	write_mdcr_el2(mdcr_el2);

	/*
	 * Initialise HSTR_EL2. All fields are architecturally
	 * UNKNOWN on reset.
	 *
	 * HSTR_EL2.T<n>: Set all these fields to zero so that
	 *  Non-secure EL0 or EL1 accesses to System registers
	 *  do not trap to EL2.
	 */
	write_hstr_el2(HSTR_EL2_RESET_VAL & ~(HSTR_EL2_T_MASK));
	/*
	 * Initialise CNTHP_CTL_EL2. All fields are
	 * architecturally UNKNOWN on reset.
	 *
	 * CNTHP_CTL_EL2:ENABLE: Set to zero to disable the EL2
	 *  physical timer and prevent timer interrupts.
	 */
	write_cnthp_ctl_el2(CNTHP_CTL_RESET_VAL & ~(CNTHP_CTL_ENABLE_BIT));

	ATF_STATE = ATF_STATE_BL2_MAIN + 1;
	time_records->fsbl_exit = read_time_ms();

	gic_secure_init_for_ns_el1();

	call_with_eret(bl2_entry);

	ATF_STATE = ATF_STATE_BL2_MAIN + 2;
}

typedef struct param_header {
	uint8_t type; /* type of the structure */
	uint8_t version; /* version of this structure */
	uint16_t size; /* size of this structure in bytes */
	uint32_t attr; /* attributes: unused bits SBZ */
} param_header_t;

typedef struct image_info {
	param_header_t h;
	uintptr_t image_base; /* physical address of base of image */
	uint32_t image_size; /* bytes read from image file */
} image_info_t;

typedef struct aapcs64_params {
	u_register_t arg0;
	u_register_t arg1;
	u_register_t arg2;
	u_register_t arg3;
	u_register_t arg4;
	u_register_t arg5;
	u_register_t arg6;
	u_register_t arg7;
} aapcs64_params_t;

typedef struct entry_point_info {
	param_header_t h;
	uintptr_t pc;
	uint32_t spsr;
	aapcs64_params_t args;
} entry_point_info_t;

typedef struct bl31_params {
	param_header_t h;
	image_info_t *bl31_image_info;
	entry_point_info_t *bl32_ep_info;
	image_info_t *bl32_image_info;
	entry_point_info_t *bl33_ep_info;
	image_info_t *bl33_image_info;
} bl31_params_t;

#define PARAM_BL31 0x03
#define VERSION_1 0x01

#define MODE_EL2 U(0x2)
#define MODE_EL1 U(0x1)
#define NON_SECURE U(0x1)

#define PARAM_EP_SECURITY_MASK U(0x1)

#define SET_SECURITY_STATE(x, security) ((x) = ((x) & ~PARAM_EP_SECURITY_MASK) | (security))

struct {
	bl31_params_t bl31_params;
	entry_point_info_t bl33_ep_info;
} static next_info;

void jump_to_monitor(uintptr_t monitor_entry, uintptr_t next_addr)
{
	const char skip[] = "SKIP_LICENSE_CHECK\0";
	bl31_params_t *from_bl2 = &next_info.bl31_params;

	memcpy((void *)LICENSE_FILE_ADDR, skip, sizeof(skip));

	from_bl2->h.type = PARAM_BL31;
	from_bl2->h.version = VERSION_1;
	from_bl2->bl33_ep_info = &next_info.bl33_ep_info;

	SET_SECURITY_STATE(from_bl2->bl33_ep_info->h.attr, NON_SECURE);
	from_bl2->bl33_ep_info->pc = next_addr;
	from_bl2->bl33_ep_info->args.arg0 = 0;
	from_bl2->bl33_ep_info->spsr = SPSR_64(MODE_EL2, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);

	flush_dcache_range(LICENSE_FILE_ADDR, 64);
	flush_dcache_range((uintptr_t)&next_info, sizeof(next_info));

	time_records->fsbl_exit = read_time_ms();
	jump_bl31(monitor_entry, from_bl2);
}

void jump_to_loader_2nd(uintptr_t loader_2nd_entry)
{
	switch_el3_to_el2_ns(loader_2nd_entry);
}

void bl2_smc_handler(void)
{
	NOTICE("%s:%d\n", __func__, __LINE__);

	while (1)
		;
	__builtin_unreachable();
}