uint64_t tbase_monitor_fastcall(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3, uint64_t x4, fc_response_t *resp ) { uint64_t mpidr = read_mpidr(); uint32_t linear_id = platform_get_core_pos(mpidr); tbase_context *tbase_ctx = &secure_context[linear_id]; uint64_t *regs = tbase_ctx->monitorCallRegs; if ((tbaseExecutionStatus&TBASE_STATUS_FASTCALL_OK_BIT)==0) { // TBASE must be initialized to be usable DBG_PRINTF( "tbase_monitor_fastcall tbase not ready for fastcall\n\r" ); return 1; } if(tbase_ctx->state == TBASE_STATE_OFF) { DBG_PRINTF( "tbase_monitor_fastcall tbase not ready for fastcall\n\r" ); return 1; } // parameters for call regs[0] = smc_fid; regs[1] = x1; regs[2] = x2; regs[3] = x3; regs[4] = x4; cpu_context_t *ns_context = (cpu_context_t *) cm_get_context(NON_SECURE); tbase_setup_entry_monitor(ns_context); tbase_synchronous_sp_entry(tbase_ctx); if (resp!=NULL) { resp->x1 = regs[1]; resp->x2 = regs[2]; resp->x3 = regs[3]; resp->x4 = regs[4]; } return 0; }
static int32_t tbase_init_entry() { DBG_PRINTF("tbase_init\n\r"); // Save el1 registers in case non-secure world has already been set up. cm_el1_sysregs_context_save(NON_SECURE); uint64_t mpidr = read_mpidr(); uint32_t linear_id = platform_get_core_pos(mpidr); tbase_context *tbase_ctx = &secure_context[linear_id]; // Note: mapping is 1:1, so physical and virtual addresses are here the same. cpu_context_t *ns_entry_context = (cpu_context_t *) cm_get_context(mpidr, NON_SECURE); // ************************************************************************************ // Configure parameter passing to tbase // Calculate page start addresses for register areas. registerFileStart[REGISTER_FILE_NWD] = page_align((uint64_t)&ns_entry_context, DOWN); registerFileStart[REGISTER_FILE_MONITOR] = page_align((uint64_t)&msm_area, DOWN); // Calculate page end addresses for register areas. registerFileEnd[REGISTER_FILE_NWD] = (uint64_t)(&ns_entry_context[TBASE_CORE_COUNT]); registerFileEnd[REGISTER_FILE_MONITOR] = ((uint64_t)&msm_area) +sizeof(msm_area); int32_t totalPages = 0; for (int area=0; area<REGISTER_FILE_COUNT; area++) { int32_t pages = page_align(registerFileEnd[area] - registerFileStart[area], UP) / PAGE_SIZE; assert( pages +totalPages <= TBASE_INTERFACE_PAGES ); tbase_init_register_file(area, totalPages, pages); totalPages += pages; } // ************************************************************************************ // Create boot structure tbaseBootCfg.magic = TBASE_BOOTCFG_MAGIC; tbaseBootCfg.length = sizeof(bootCfg_t); tbaseBootCfg.version = TBASE_MONITOR_INTERFACE_VERSION; tbaseBootCfg.dRamBase = TBASE_NWD_DRAM_BASE; tbaseBootCfg.dRamSize = TBASE_NWD_DRAM_SIZE; tbaseBootCfg.secDRamBase = TBASE_SWD_DRAM_BASE; tbaseBootCfg.secDRamSize = TBASE_SWD_DRAM_SIZE; tbaseBootCfg.secIRamBase = TBASE_SWD_IMEM_BASE; tbaseBootCfg.secIRamSize = TBASE_SWD_IMEM_SIZE; tbaseBootCfg.conf_mair_el3 = read_mair_el3(); tbaseBootCfg.MSMPteCount = totalPages; tbaseBootCfg.MSMBase = (uint64_t)registerFileL2; tbaseBootCfg.gic_distributor_base = TBASE_GIC_DIST_BASE; tbaseBootCfg.gic_cpuinterface_base = TBASE_GIC_CPU_BASE; tbaseBootCfg.gic_version = TBASE_GIC_VERSION; tbaseBootCfg.total_number_spi = TBASE_SPI_COUNT; tbaseBootCfg.ssiq_number = TBASE_SSIQ_NRO; tbaseBootCfg.flags = TBASE_MONITOR_FLAGS; DBG_PRINTF("*** tbase boot cfg ***\n\r"); DBG_PRINTF("* magic : 0x%.X\n\r",tbaseBootCfg.magic); DBG_PRINTF("* length : 0x%.X\n\r",tbaseBootCfg.length); DBG_PRINTF("* version : 0x%.X\n\r",tbaseBootCfg.version); DBG_PRINTF("* dRamBase : 0x%.X\n\r",tbaseBootCfg.dRamBase); DBG_PRINTF("* dRamSize : 0x%.X\n\r",tbaseBootCfg.dRamSize); DBG_PRINTF("* secDRamBase : 0x%.X\n\r",tbaseBootCfg.secDRamBase); DBG_PRINTF("* secDRamSize : 0x%.X\n\r",tbaseBootCfg.secDRamSize); DBG_PRINTF("* secIRamBase : 0x%.X\n\r",tbaseBootCfg.secIRamBase); DBG_PRINTF("* secIRamSize : 0x%.X\n\r",tbaseBootCfg.secIRamSize); DBG_PRINTF("* conf_mair_el3 : 0x%.X\n\r",tbaseBootCfg.conf_mair_el3); DBG_PRINTF("* MSMPteCount : 0x%.X\n\r",tbaseBootCfg.MSMPteCount); DBG_PRINTF("* MSMBase : 0x%.X\n\r",tbaseBootCfg.MSMBase); DBG_PRINTF("* gic_distributor_base : 0x%.X\n\r",tbaseBootCfg.gic_distributor_base); DBG_PRINTF("* gic_cpuinterface_base : 0x%.X\n\r",tbaseBootCfg.gic_cpuinterface_base); DBG_PRINTF("* gic_version : 0x%.X\n\r",tbaseBootCfg.gic_version); DBG_PRINTF("* total_number_spi : 0x%.X\n\r",tbaseBootCfg.total_number_spi); DBG_PRINTF("* ssiq_number : 0x%.X\n\r",tbaseBootCfg.ssiq_number); DBG_PRINTF("* flags : 0x%.X\n\r",tbaseBootCfg.flags); // ************************************************************************************ // tbaseBootCfg and l2 entries may be accesses uncached, so must flush those. flush_dcache_range((unsigned long)&tbaseBootCfg, sizeof(bootCfg_t)); flush_dcache_range((unsigned long)®isterFileL2, sizeof(registerFileL2)); // ************************************************************************************ // Set registers for tbase initialization entry cpu_context_t *s_entry_context = &tbase_ctx->cpu_ctx; gp_regs_t *s_entry_gpregs = get_gpregs_ctx(s_entry_context); write_ctx_reg(s_entry_gpregs, CTX_GPREG_X1, 0); write_ctx_reg(s_entry_gpregs, CTX_GPREG_X1, (int64_t)&tbaseBootCfg); // SPSR for SMC handling (FIQ mode) tbaseEntrySpsr = TBASE_ENTRY_SPSR; DBG_PRINTF("tbase init SPSR 0x%x\n\r", read_ctx_reg(get_el3state_ctx(&tbase_ctx->cpu_ctx), CTX_SPSR_EL3) ); DBG_PRINTF("tbase SMC SPSR %x\nr\r", tbaseEntrySpsr ); // ************************************************************************************ // Start tbase tbase_synchronous_sp_entry(tbase_ctx); tbase_ctx->state = TBASE_STATE_ON; #if TBASE_PM_ENABLE // Register power managemnt hooks with PSCI psci_register_spd_pm_hook(&tbase_pm); #endif cm_el1_sysregs_context_restore(NON_SECURE); cm_set_next_eret_context(NON_SECURE); return 1; }
//************************************************************************************************ // FIQ handler for FIQ when in NWd uint64_t tbase_fiq_handler( uint32_t id, uint32_t flags, void *handle, void *cookie) { uint64_t mpidr; uint32_t linear_id; tbase_context *tbase_ctx; mpidr = read_mpidr(); linear_id = platform_get_core_pos(mpidr); tbase_ctx = &secure_context[linear_id]; assert(&tbase_ctx->cpu_ctx == cm_get_context(SECURE)); /* Check if the vector has been entered for SGI/FIQ dump reason */ if (id == FIQ_SMP_CALL_SGI) { /* ACK gic */ { unsigned int iar; iar = gicc_read_IAR(get_plat_config()->gicc_base); gicc_write_EOIR(get_plat_config()->gicc_base, iar); } /* Save the non-secure context before entering the TSP */ cm_el1_sysregs_context_save(NON_SECURE); /* Call customer's dump implementation */ plat_tbase_dump(); // Load NWd //cm_el1_sysregs_context_restore(NON_SECURE); //cm_set_next_eret_context(NON_SECURE); } else { /* Check the security state when the exception was generated */ assert(get_interrupt_src_ss(flags) == NON_SECURE); /* Sanity check the pointer to this cpu's context */ assert(handle == cm_get_context(NON_SECURE)); if ((tbaseExecutionStatus&TBASE_STATUS_SMC_OK_BIT)==0) { // TBASE must be initialized to be usable // TODO: What should we really do here? // We should disable FIQs to prevent futher interrupts DBG_PRINTF( "tbase_interrupt_handler tbase not ready for interrupt\n\r" ); return 1; } if(tbase_ctx->state == TBASE_STATE_OFF) { DBG_PRINTF( "tbase_interrupt_handler tbase not ready for fastcall\n\r" ); return 1; } /* Save the non-secure context before entering the TSP */ cm_el1_sysregs_context_save(NON_SECURE); /* Switch to secure context now */ cm_el1_sysregs_context_restore(SECURE); cm_set_next_eret_context(SECURE); // Load SWd context tbase_setup_entry_nwd((cpu_context_t *)handle,ENTRY_OFFSET_FIQ); // Enter tbase. tbase must return using normal SMC, which will continue here. tbase_synchronous_sp_entry(tbase_ctx); // Load NWd cm_el1_sysregs_context_restore(NON_SECURE); cm_set_next_eret_context(NON_SECURE); } return 0; }
static uint64_t tbase_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3, uint64_t x4, void *cookie, void *handle, uint64_t flags) { uint64_t mpidr = read_mpidr(); uint32_t linear_id = platform_get_core_pos(mpidr); tbase_context *tbase_ctx = &secure_context[linear_id]; int caller_security_state = flags&1; DBG_PRINTF("tbase_smc_handler %d %x\n\r", caller_security_state, smc_fid); if (caller_security_state==SECURE) { // Yield to NWd // TODO: Check id if (tbaseInitStatus==TBASE_INIT_CONFIG_OK) { // Save sysregs to all cores. // After this tbase can work on any core. save_sysregs_allcore(); tbaseInitStatus = TBASE_INIT_SYSREGS_OK; if (tbaseExecutionStatus==TBASE_STATUS_UNINIT) { tbaseExecutionStatus = TBASE_STATUS_NORMAL; } } // If above check fails, it is not possible to return to tbase. tbase_synchronous_sp_exit(tbase_ctx, 0, 1); } else { if ((tbaseExecutionStatus&TBASE_STATUS_SMC_OK_BIT)==0) { // TBASE must be initialized to be usable DBG_PRINTF( "tbase_smc_handler tbase not ready for smc.\n\r"); // TODO: What is correct error code? SMC_RET1(handle, SMC_UNK); return 1; } if(tbase_ctx->state == TBASE_STATE_OFF) { DBG_PRINTF( "tbase_smc_handler tbase not ready for fastcall\n\r" ); return 1; } // NSIQ, go to SWd // TODO: Check id? // Save NWd gp_regs_t *ns_gpregs = get_gpregs_ctx((cpu_context_t *)handle); write_ctx_reg(ns_gpregs, CTX_GPREG_X0, smc_fid ); write_ctx_reg(ns_gpregs, CTX_GPREG_X1, x1 ); write_ctx_reg(ns_gpregs, CTX_GPREG_X2, x2 ); write_ctx_reg(ns_gpregs, CTX_GPREG_X3, x3 ); cm_el1_sysregs_context_save(NON_SECURE); // Load SWd tbase_setup_entry_nwd((cpu_context_t *)handle,ENTRY_OFFSET_SMC); // Enter tbase. tbase must return using normal SMC, which will continue here. tbase_synchronous_sp_entry(tbase_ctx); // Load NWd cm_el1_sysregs_context_restore(NON_SECURE); cm_set_next_eret_context(NON_SECURE); } return 0; }
// ************************************************************************************ // fastcall handler static uint64_t tbase_fastcall_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3, uint64_t x4, void *cookie, void *handle, uint64_t flags) { uint64_t mpidr = read_mpidr(); uint32_t linear_id = platform_get_core_pos(mpidr); tbase_context *tbase_ctx = &secure_context[linear_id]; int caller_security_state = flags&1; if (caller_security_state==SECURE) { switch(maskSWdRegister(smc_fid)) { case TBASE_SMC_FASTCALL_RETURN: { // Return values from fastcall already in cpu_context! // TODO: Could we skip saving sysregs? DBG_PRINTF( "tbase_fastcall_handler TBASE_SMC_FASTCALL_RETURN\n\r"); tbase_synchronous_sp_exit(tbase_ctx, 0, 1); } case TBASE_SMC_FASTCALL_CONFIG_OK: { DBG_PRINTF( "tbase_fastcall_handler TBASE_SMC_FASTCALL_CONFIG_OK\n\r"); configure_tbase(x1,x2); SMC_RET1(handle,smc_fid); break; } case TBASE_SMC_FASTCALL_OUTPUT: { output(x1,x2); SMC_RET1(handle,smc_fid); break; } case TBASE_SMC_FASTCALL_STATUS: { DBG_PRINTF( "tbase_fastcall_handler TBASE_SMC_FASTCALL_STATUS\n\r"); tbase_status(x1,x2); SMC_RET1(handle,smc_fid); break; } case TBASE_SMC_FASTCALL_INPUT: { DBG_PRINTF( "tbase_fastcall_handler TBASE_SMC_FASTCALL_INPUT\n\r"); smc_fid = plat_tbase_input(x1,&x2,&(tbase_ctx->tbase_input_fastcall)); SMC_RET3(handle,smc_fid,page_align(registerFileEnd[REGISTER_FILE_NWD] - registerFileStart[REGISTER_FILE_NWD], UP)+(uint64_t)&(tbase_ctx->tbase_input_fastcall)- registerFileStart[REGISTER_FILE_MONITOR],x2); break; } case TBASE_SMC_FASTCALL_DUMP: { DBG_PRINTF( "tbase_fastcall_handler TBASE_SMC_FASTCALL_DUMP\n\r"); tbase_triggerSgiDump(); SMC_RET1(handle,smc_fid); break; } default: { // What now? DBG_PRINTF( "tbase_fastcall_handler SMC_UNK %x\n\r", smc_fid ); SMC_RET1(handle, SMC_UNK); break; } } } else { if (smc_fid == TBASE_SMC_AEE_DUMP) // N-world can request AEE Dump function { mt_atf_trigger_WDT_FIQ(); // Once we return to the N-world's caller, // FIQ will be trigged and bring us on EL3 (ATF) on core #0 because HW wiring. // Then FIQ will be handled the same way as for HW WDT FIQ. //Do we need to save-recover n-context before being able to use it for return? cm_el1_sysregs_context_restore(NON_SECURE); cm_set_next_eret_context(NON_SECURE); return 0; } if ((tbaseExecutionStatus&TBASE_STATUS_FASTCALL_OK_BIT)==0) { // TBASE must be initialized to be usable // TODO: What is correct error code? DBG_PRINTF( "tbase_fastcall_handler tbase not ready for fastcall\n\r" ); SMC_RET1(handle, SMC_UNK); return 0; } if(tbase_ctx->state == TBASE_STATE_OFF) { DBG_PRINTF( "tbase_fastcall_handler tbase not ready for fastcall\n\r" ); SMC_RET1(handle, SMC_UNK); return 0; } DBG_PRINTF( "tbase_fastcall_handler NWd %x\n\r", smc_fid ); // So far all fastcalls go to tbase // Save NWd context gp_regs_t *ns_gpregs = get_gpregs_ctx((cpu_context_t *)handle); write_ctx_reg(ns_gpregs, CTX_GPREG_X0, smc_fid ); // These are not saved yet write_ctx_reg(ns_gpregs, CTX_GPREG_X1, x1 ); write_ctx_reg(ns_gpregs, CTX_GPREG_X2, x2 ); write_ctx_reg(ns_gpregs, CTX_GPREG_X3, x3 ); cm_el1_sysregs_context_save(NON_SECURE); // Load SWd context tbase_setup_entry_nwd((cpu_context_t *)handle,ENTRY_OFFSET_FASTCALL); #if DEBUG print_fastcall_params("entry", NON_SECURE); #endif tbase_synchronous_sp_entry(tbase_ctx); cm_el1_sysregs_context_restore(NON_SECURE); cm_set_next_eret_context(NON_SECURE); return 0; // Does not seem to matter what we return } }