void init_sec_mon(unsigned long nsec_entry)
{
struct plat_nsec_ctx *plat_ctx;
struct sm_nsec_ctx *nsec_ctx;
plat_ctx = phys_to_virt(nsec_entry, MEM_AREA_IO_SEC);
if (!plat_ctx)
panic();
/* Invalidate cache to fetch data from external memory */
cache_op_inner(DCACHE_AREA_INVALIDATE,
plat_ctx, sizeof(*plat_ctx));
/* Initialize secure monitor */
nsec_ctx = sm_get_nsec_ctx();
nsec_ctx->mode_regs.usr_sp = plat_ctx->usr_sp;
nsec_ctx->mode_regs.usr_lr = plat_ctx->usr_lr;
nsec_ctx->mode_regs.irq_spsr = plat_ctx->irq_spsr;
nsec_ctx->mode_regs.irq_sp = plat_ctx->irq_sp;
nsec_ctx->mode_regs.irq_lr = plat_ctx->irq_lr;
nsec_ctx->mode_regs.svc_spsr = plat_ctx->svc_spsr;
nsec_ctx->mode_regs.svc_sp = plat_ctx->svc_sp;
nsec_ctx->mode_regs.svc_lr = plat_ctx->svc_lr;
nsec_ctx->mode_regs.abt_spsr = plat_ctx->abt_spsr;
nsec_ctx->mode_regs.abt_sp = plat_ctx->abt_sp;
nsec_ctx->mode_regs.abt_lr = plat_ctx->abt_lr;
nsec_ctx->mode_regs.und_spsr = plat_ctx->und_spsr;
nsec_ctx->mode_regs.und_sp = plat_ctx->und_sp;
nsec_ctx->mode_regs.und_lr = plat_ctx->und_lr;
nsec_ctx->mon_lr = plat_ctx->mon_lr;
nsec_ctx->mon_spsr = plat_ctx->mon_spsr;
}
bool sm_platform_handler(struct sm_ctx *ctx)
{
if (ctx->nsec.r12 == 0x200)
return true;
switch (ctx->nsec.r12) {
case 0x0:
switch (ctx->nsec.r0) {
case SECURE_SVC_PM_LATE_SUSPEND:
sm_pm_cpu_do_suspend(suspend_regs);
cache_op_inner(DCACHE_AREA_CLEAN,
suspend_regs,
sizeof(suspend_regs));
cache_op_outer(DCACHE_AREA_CLEAN,
virt_to_phys(suspend_regs),
sizeof(suspend_regs));
ctx->nsec.r0 = API_HAL_RET_VALUE_OK;
break;
default:
ctx->nsec.r0 = API_HAL_RET_VALUE_SERVICE_UNKNWON;
break;
}
break;
case API_MONITOR_L2CACHE_SETDEBUG_INDEX:
write32(ctx->nsec.r0, pl310_base() + PL310_DEBUG_CTRL);
ctx->nsec.r0 = API_HAL_RET_VALUE_OK;
break;
case API_MONITOR_L2CACHE_CLEANINVBYPA_INDEX:
arm_cl2_cleaninvbypa(pl310_base(), ctx->nsec.r0,
(ctx->nsec.r0 + ctx->nsec.r1));
ctx->nsec.r0 = API_HAL_RET_VALUE_OK;
break;
case API_MONITOR_L2CACHE_SETCONTROL_INDEX:
write32(ctx->nsec.r0, pl310_base() + PL310_CTRL);
ctx->nsec.r0 = API_HAL_RET_VALUE_OK;
break;
case API_MONITOR_L2CACHE_SETAUXILIARYCONTROL_INDEX:
write32(ctx->nsec.r0, pl310_base() + PL310_AUX_CTRL);
ctx->nsec.r0 = API_HAL_RET_VALUE_OK;
break;
case API_MONITOR_L2CACHE_SETLATENCY_INDEX:
write32(ctx->nsec.r0, pl310_base() + PL310_TAG_RAM_CTRL);
write32(ctx->nsec.r1, pl310_base() + PL310_DATA_RAM_CTRL);
ctx->nsec.r0 = API_HAL_RET_VALUE_OK;
break;
case API_MONITOR_L2CACHE_SETPREFETCHCONTROL_INDEX:
write32(ctx->nsec.r0, pl310_base() + PL310_PREFETCH_CTRL);
ctx->nsec.r0 = API_HAL_RET_VALUE_OK;
break;
default:
ctx->nsec.r0 = API_HAL_RET_VALUE_SERVICE_UNKNWON;
break;
}
return false;
}