/******************************************************************************
* This function is invoked during warm boot. Invoke the PSCI library
* warm boot entry point which takes care of Architectural and platform setup/
* restore. Copy the relevant cpu_context register values to smc context which
* will get programmed during `smc_exit`.
*****************************************************************************/
void sp_min_warm_boot(void)
{
smc_ctx_t *next_smc_ctx;
cpu_context_t *ctx = cm_get_context(NON_SECURE);
u_register_t ns_sctlr;
psci_warmboot_entrypoint();
smc_set_next_ctx(NON_SECURE);
next_smc_ctx = smc_get_next_ctx();
zeromem(next_smc_ctx, sizeof(smc_ctx_t));
copy_cpu_ctx_to_smc_stx(get_regs_ctx(cm_get_context(NON_SECURE)),
next_smc_ctx);
/* Temporarily set the NS bit to access NS SCTLR */
write_scr(read_scr() | SCR_NS_BIT);
isb();
ns_sctlr = read_ctx_reg(get_regs_ctx(ctx), CTX_NS_SCTLR);
write_sctlr(ns_sctlr);
isb();
write_scr(read_scr() & ~SCR_NS_BIT);
isb();
}
int hexprint(unsigned int val)
{
unsigned int hx = val;
char hex[10];
int i, j;
if ( val == 0) {
write_scr("0x0\n", 4);
return 0;
}
for ( i = 9; (i > 1) && (hx); i--) {
hex[i] = digits[hx % HEX];
hx = hx >> 4;
}
hex[i] = 'x';
hex[i-1] = '0';
for (j = 0, i--; i < 10; i++, j++) {
hex[j] = hex[i];
}
write_scr(hex, j);
write_scr("\n", 1);
return 0;
}
/*******************************************************************************
* This function invokes the PSCI library interface to initialize the
* non secure cpu context and copies the relevant cpu context register values
* to smc context. These registers will get programmed during `smc_exit`.
******************************************************************************/
static void sp_min_prepare_next_image_entry(void)
{
entry_point_info_t *next_image_info;
cpu_context_t *ctx = cm_get_context(NON_SECURE);
u_register_t ns_sctlr;
/* Program system registers to proceed to non-secure */
next_image_info = sp_min_plat_get_bl33_ep_info();
assert(next_image_info);
assert(NON_SECURE == GET_SECURITY_STATE(next_image_info->h.attr));
INFO("SP_MIN: Preparing exit to normal world\n");
psci_prepare_next_non_secure_ctx(next_image_info);
smc_set_next_ctx(NON_SECURE);
/* Copy r0, lr and spsr from cpu context to SMC context */
copy_cpu_ctx_to_smc_stx(get_regs_ctx(cm_get_context(NON_SECURE)),
smc_get_next_ctx());
/* Temporarily set the NS bit to access NS SCTLR */
write_scr(read_scr() | SCR_NS_BIT);
isb();
ns_sctlr = read_ctx_reg(get_regs_ctx(ctx), CTX_NS_SCTLR);
write_sctlr(ns_sctlr);
isb();
write_scr(read_scr() & ~SCR_NS_BIT);
isb();
}
/*******************************************************************************
* This function helps the SP to translate NS/S virtual addresses.
******************************************************************************/
uint64_t tlkd_va_translate(uintptr_t va, int type)
{
uint64_t pa;
if (type & TLK_TRANSLATE_NS_VADDR) {
/* save secure context */
cm_el1_sysregs_context_save(SECURE);
/* restore non-secure context */
cm_el1_sysregs_context_restore(NON_SECURE);
/* switch NS bit to start using 64-bit, non-secure mappings */
write_scr(cm_get_scr_el3(NON_SECURE));
isb();
}
int at = type & AT_MASK;
switch (at) {
case 0:
ats12e1r(va);
break;
case 1:
ats12e1w(va);
break;
case 2:
ats12e0r(va);
break;
case 3:
ats12e0w(va);
break;
default:
assert(0);
}
/* get the (NS/S) physical address */
isb();
pa = read_par_el1();
/* Restore secure state */
if (type & TLK_TRANSLATE_NS_VADDR) {
/* restore secure context */
cm_el1_sysregs_context_restore(SECURE);
/* switch NS bit to start using 32-bit, secure mappings */
write_scr(cm_get_scr_el3(SECURE));
isb();
}
return pa;
}
/*******************************************************************************
* Function that does the first bit of architectural setup that affects
* execution in the non-secure address space.
******************************************************************************/
void bl1_arch_setup(void)
{
unsigned long tmp_reg = 0;
/* Enable alignment checks and set the exception endianess to LE */
tmp_reg = read_sctlr_el3();
tmp_reg |= (SCTLR_A_BIT | SCTLR_SA_BIT);
tmp_reg &= ~SCTLR_EE_BIT;
write_sctlr_el3(tmp_reg);
/*
* Enable HVCs, route FIQs to EL3, set the next EL to be AArch64, route
* external abort and SError interrupts to EL3
*/
tmp_reg = SCR_RES1_BITS | SCR_RW_BIT | SCR_HCE_BIT | SCR_EA_BIT |
SCR_FIQ_BIT;
write_scr(tmp_reg);
/*
* Enable SError and Debug exceptions
*/
enable_serror();
enable_debug_exceptions();
return;
}
/*******************************************************************************
* Initialize the gic, configure the SCR.
******************************************************************************/
void bl31_platform_setup(void)
{
uint32_t tmp_reg;
/*
* Initialize delay timer
*/
tegra_delay_timer_init();
/*
* Setup secondary CPU POR infrastructure.
*/
plat_secondary_setup();
/*
* Initial Memory Controller configuration.
*/
tegra_memctrl_setup();
/*
* Do initial security configuration to allow DRAM/device access.
*/
tegra_memctrl_tzdram_setup(tegra_bl31_phys_base,
plat_bl31_params_from_bl2.tzdram_size);
/* Set the next EL to be AArch64 */
tmp_reg = SCR_RES1_BITS | SCR_RW_BIT;
write_scr(tmp_reg);
/* Initialize the gic cpu and distributor interfaces */
tegra_gic_setup();
}
void draw(World * w)
{
memset(screen_buffer, 0, sizeof(CHAR_INFO) * BUFFER_CX * BUFFER_CY);
draw_map(w);
draw_system(w);
draw_fps();
write_scr((CHAR_INFO *) screen_buffer, BUFFER_CX, BUFFER_CY);
}
void change_security_state(unsigned int target_security_state)
{
unsigned long scr = read_scr();
assert(sec_state_is_valid(target_security_state));
if (target_security_state == SECURE)
scr &= ~SCR_NS_BIT;
else
scr |= SCR_NS_BIT;
write_scr(scr);
}
void change_security_state(unsigned int target_security_state)
{
unsigned long scr = read_scr();
if (target_security_state == SECURE)
scr &= ~SCR_NS_BIT;
else if (target_security_state == NON_SECURE)
scr |= SCR_NS_BIT;
else
assert(0);
write_scr(scr);
}
int kmain (void)
{
struct xmitimerval req = {{0, 200000}, {0, 0}};
struct xmtimespec st, et;
int count = 0;
long tm;
long baud = 156000;
int ret;
ret = serial_device_init(my_Console_ttyS, baud);
set_timer(&req, 0);
install_event_handler(0, timer_handler);
unmask_event(0);
enable_events_flag();
//rt_serial_write(string, 30);
while (1) {
suspend_domain (0, 0);
do {
ret = rt_serial_read(string, 1024);
if(ret > 0) {
count ++;
if(count == 1) get_time(&st);
else if(count == 100) {
get_time(&et);
write_scr("\n\n", 2);
tm = et.tv_sec-st.tv_sec;
hexprint(tm);
hexprint(et.tv_nsec);
hexprint(st.tv_nsec);
count = 0;
}
}
} while(ret > 0);
}
return 0;
}
/*******************************************************************************
* Prepare the CPU system registers for first entry into secure or normal world
*
* If execution is requested to hyp mode, HSCTLR is initialized
* If execution is requested to non-secure PL1, and the CPU supports
* HYP mode then HYP mode is disabled by configuring all necessary HYP mode
* registers.
******************************************************************************/
void cm_prepare_el3_exit(uint32_t security_state)
{
uint32_t sctlr, scr, hcptr;
cpu_context_t *ctx = cm_get_context(security_state);
assert(ctx);
if (security_state == NON_SECURE) {
scr = read_ctx_reg(get_regs_ctx(ctx), CTX_SCR);
if (scr & SCR_HCE_BIT) {
/* Use SCTLR value to initialize HSCTLR */
sctlr = read_ctx_reg(get_regs_ctx(ctx),
CTX_NS_SCTLR);
sctlr |= HSCTLR_RES1;
/* Temporarily set the NS bit to access HSCTLR */
write_scr(read_scr() | SCR_NS_BIT);
/*
* Make sure the write to SCR is complete so that
* we can access HSCTLR
*/
isb();
write_hsctlr(sctlr);
isb();
write_scr(read_scr() & ~SCR_NS_BIT);
isb();
} else if (read_id_pfr1() &
(ID_PFR1_VIRTEXT_MASK << ID_PFR1_VIRTEXT_SHIFT)) {
/*
* Set the NS bit to access NS copies of certain banked
* registers
*/
write_scr(read_scr() | SCR_NS_BIT);
isb();
/* PL2 present but unused, need to disable safely */
write_hcr(0);
/* HSCTLR : can be ignored when bypassing */
/* HCPTR : disable all traps TCPAC, TTA, TCP */
hcptr = read_hcptr();
hcptr &= ~(TCPAC_BIT | TTA_BIT | TCP11_BIT | TCP10_BIT);
write_hcptr(hcptr);
/* Enable EL1 access to timer */
write_cnthctl(PL1PCEN_BIT | PL1PCTEN_BIT);
/* Reset CNTVOFF_EL2 */
write64_cntvoff(0);
/* Set VPIDR, VMPIDR to match MIDR, MPIDR */
write_vpidr(read_midr());
write_vmpidr(read_mpidr());
/*
* Reset VTTBR.
* Needed because cache maintenance operations depend on
* the VMID even when non-secure EL1&0 stage 2 address
* translation are disabled.
*/
write64_vttbr(0);
/*
* Avoid unexpected debug traps in case where HDCR
* is not completely reset by the hardware - set
* HDCR.HPMN to PMCR.N and zero the remaining bits.
* The HDCR.HPMN and PMCR.N fields are the same size
* (5 bits) and HPMN is at offset zero within HDCR.
*/
write_hdcr((read_pmcr() & PMCR_N_BITS) >> PMCR_N_SHIFT);
/*
* Reset CNTHP_CTL to disable the EL2 physical timer and
* therefore prevent timer interrupts.
*/
write_cnthp_ctl(0);
isb();
write_scr(read_scr() & ~SCR_NS_BIT);
isb();
}
/*******************************************************************************
* Prepare the CPU system registers for first entry into secure or normal world
*
* If execution is requested to hyp mode, HSCTLR is initialized
* If execution is requested to non-secure PL1, and the CPU supports
* HYP mode then HYP mode is disabled by configuring all necessary HYP mode
* registers.
******************************************************************************/
void cm_prepare_el3_exit(uint32_t security_state)
{
uint32_t sctlr, scr, hcptr;
cpu_context_t *ctx = cm_get_context(security_state);
assert(ctx);
if (security_state == NON_SECURE) {
scr = read_ctx_reg(get_regs_ctx(ctx), CTX_SCR);
if (scr & SCR_HCE_BIT) {
/* Use SCTLR value to initialize HSCTLR */
sctlr = read_ctx_reg(get_regs_ctx(ctx),
CTX_NS_SCTLR);
sctlr |= HSCTLR_RES1;
/* Temporarily set the NS bit to access HSCTLR */
write_scr(read_scr() | SCR_NS_BIT);
/*
* Make sure the write to SCR is complete so that
* we can access HSCTLR
*/
isb();
write_hsctlr(sctlr);
isb();
write_scr(read_scr() & ~SCR_NS_BIT);
isb();
} else if (read_id_pfr1() &
(ID_PFR1_VIRTEXT_MASK << ID_PFR1_VIRTEXT_SHIFT)) {
/* Set the NS bit to access HCR, HCPTR, CNTHCTL, VPIDR, VMPIDR */
write_scr(read_scr() | SCR_NS_BIT);
isb();
/* PL2 present but unused, need to disable safely */
write_hcr(0);
/* HSCTLR : can be ignored when bypassing */
/* HCPTR : disable all traps TCPAC, TTA, TCP */
hcptr = read_hcptr();
hcptr &= ~(TCPAC_BIT | TTA_BIT | TCP11_BIT | TCP10_BIT);
write_hcptr(hcptr);
/* Enable EL1 access to timer */
write_cnthctl(PL1PCEN_BIT | PL1PCTEN_BIT);
/* Reset CNTVOFF_EL2 */
write64_cntvoff(0);
/* Set VPIDR, VMPIDR to match MIDR, MPIDR */
write_vpidr(read_midr());
write_vmpidr(read_mpidr());
/*
* Reset VTTBR.
* Needed because cache maintenance operations depend on
* the VMID even when non-secure EL1&0 stage 2 address
* translation are disabled.
*/
write64_vttbr(0);
isb();
write_scr(read_scr() & ~SCR_NS_BIT);
isb();
}
}
}
