Merge pull request #105 from athoelke:sm/support_normal_irq_in_tsp-v2
This commit is contained in:
commit
65335d45f5
|
@ -109,12 +109,22 @@ void runtime_svc_init()
|
||||||
goto error;
|
goto error;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Call the initialisation routine for this runtime service */
|
/*
|
||||||
|
* The runtime service may have seperate rt_svc_desc_t
|
||||||
|
* for its fast smc and standard smc. Since the service itself
|
||||||
|
* need to be initialized only once, only one of them will have
|
||||||
|
* an initialisation routine defined. Call the initialisation
|
||||||
|
* routine for this runtime service, if it is defined.
|
||||||
|
*/
|
||||||
|
if (rt_svc_descs[index].init) {
|
||||||
rc = rt_svc_descs[index].init();
|
rc = rt_svc_descs[index].init();
|
||||||
if (rc) {
|
if (rc) {
|
||||||
ERROR("Error initializing runtime service %s\n",
|
ERROR("Error initializing runtime service %s\n",
|
||||||
rt_svc_descs[index].name);
|
rt_svc_descs[index].name);
|
||||||
} else {
|
continue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Fill the indices corresponding to the start and end
|
* Fill the indices corresponding to the start and end
|
||||||
* owning entity numbers with the index of the
|
* owning entity numbers with the index of the
|
||||||
|
@ -129,7 +139,6 @@ void runtime_svc_init()
|
||||||
for (; start_idx <= end_idx; start_idx++)
|
for (; start_idx <= end_idx; start_idx++)
|
||||||
rt_svc_descs_indices[start_idx] = index;
|
rt_svc_descs_indices[start_idx] = index;
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
return;
|
return;
|
||||||
error:
|
error:
|
||||||
|
|
|
@ -39,8 +39,11 @@
|
||||||
.globl tsp_cpu_suspend_entry
|
.globl tsp_cpu_suspend_entry
|
||||||
.globl tsp_cpu_resume_entry
|
.globl tsp_cpu_resume_entry
|
||||||
.globl tsp_fast_smc_entry
|
.globl tsp_fast_smc_entry
|
||||||
|
.globl tsp_std_smc_entry
|
||||||
.globl tsp_fiq_entry
|
.globl tsp_fiq_entry
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/* ---------------------------------------------
|
/* ---------------------------------------------
|
||||||
* Populate the params in x0-x7 from the pointer
|
* Populate the params in x0-x7 from the pointer
|
||||||
* to the smc args structure in x0.
|
* to the smc args structure in x0.
|
||||||
|
@ -317,8 +320,22 @@ tsp_cpu_resume_panic:
|
||||||
* ---------------------------------------------
|
* ---------------------------------------------
|
||||||
*/
|
*/
|
||||||
func tsp_fast_smc_entry
|
func tsp_fast_smc_entry
|
||||||
bl tsp_fast_smc_handler
|
bl tsp_smc_handler
|
||||||
restore_args_call_smc
|
restore_args_call_smc
|
||||||
tsp_fast_smc_entry_panic:
|
tsp_fast_smc_entry_panic:
|
||||||
b tsp_fast_smc_entry_panic
|
b tsp_fast_smc_entry_panic
|
||||||
|
|
||||||
|
/*---------------------------------------------
|
||||||
|
* This entrypoint is used by the TSPD to ask
|
||||||
|
* the TSP to service a std smc request.
|
||||||
|
* We will enable preemption during execution
|
||||||
|
* of tsp_smc_handler.
|
||||||
|
* ---------------------------------------------
|
||||||
|
*/
|
||||||
|
func tsp_std_smc_entry
|
||||||
|
msr daifclr, #DAIF_FIQ_BIT | DAIF_IRQ_BIT
|
||||||
|
bl tsp_smc_handler
|
||||||
|
msr daifset, #DAIF_FIQ_BIT | DAIF_IRQ_BIT
|
||||||
|
restore_args_call_smc
|
||||||
|
tsp_std_smc_entry_panic:
|
||||||
|
b tsp_std_smc_entry_panic
|
||||||
|
|
|
@ -120,7 +120,14 @@ sync_exception_sp_elx:
|
||||||
|
|
||||||
.align 7
|
.align 7
|
||||||
irq_sp_elx:
|
irq_sp_elx:
|
||||||
b irq_sp_elx
|
save_caller_regs_and_lr
|
||||||
|
/* We just update some statistics in the handler */
|
||||||
|
bl tsp_irq_received
|
||||||
|
/* Hand over control to the normal world to handle the IRQ */
|
||||||
|
smc #0
|
||||||
|
/* The resume std smc starts from here */
|
||||||
|
restore_caller_regs_and_lr
|
||||||
|
eret
|
||||||
check_vector_size irq_sp_elx
|
check_vector_size irq_sp_elx
|
||||||
|
|
||||||
.align 7
|
.align 7
|
||||||
|
|
|
@ -107,3 +107,18 @@ int32_t tsp_fiq_handler()
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int32_t tsp_irq_received()
|
||||||
|
{
|
||||||
|
uint64_t mpidr = read_mpidr();
|
||||||
|
uint32_t linear_id = platform_get_core_pos(mpidr);
|
||||||
|
|
||||||
|
tsp_stats[linear_id].irq_count++;
|
||||||
|
spin_lock(&console_lock);
|
||||||
|
printf("TSP: cpu 0x%x received irq\n\r", mpidr);
|
||||||
|
INFO("cpu 0x%x: %d irq requests \n",
|
||||||
|
mpidr, tsp_stats[linear_id].irq_count);
|
||||||
|
spin_unlock(&console_lock);
|
||||||
|
|
||||||
|
return TSP_PREEMPTED;
|
||||||
|
}
|
||||||
|
|
|
@ -66,6 +66,7 @@ work_statistics_t tsp_stats[PLATFORM_CORE_COUNT];
|
||||||
* to change.
|
* to change.
|
||||||
******************************************************************************/
|
******************************************************************************/
|
||||||
static const entry_info_t tsp_entry_info = {
|
static const entry_info_t tsp_entry_info = {
|
||||||
|
tsp_std_smc_entry,
|
||||||
tsp_fast_smc_entry,
|
tsp_fast_smc_entry,
|
||||||
tsp_cpu_on_entry,
|
tsp_cpu_on_entry,
|
||||||
tsp_cpu_off_entry,
|
tsp_cpu_off_entry,
|
||||||
|
@ -309,9 +310,9 @@ tsp_args_t *tsp_cpu_resume_main(uint64_t suspend_level,
|
||||||
* TSP fast smc handler. The secure monitor jumps to this function by
|
* TSP fast smc handler. The secure monitor jumps to this function by
|
||||||
* doing the ERET after populating X0-X7 registers. The arguments are received
|
* doing the ERET after populating X0-X7 registers. The arguments are received
|
||||||
* in the function arguments in order. Once the service is rendered, this
|
* in the function arguments in order. Once the service is rendered, this
|
||||||
* function returns to Secure Monitor by raising SMC
|
* function returns to Secure Monitor by raising SMC.
|
||||||
******************************************************************************/
|
******************************************************************************/
|
||||||
tsp_args_t *tsp_fast_smc_handler(uint64_t func,
|
tsp_args_t *tsp_smc_handler(uint64_t func,
|
||||||
uint64_t arg1,
|
uint64_t arg1,
|
||||||
uint64_t arg2,
|
uint64_t arg2,
|
||||||
uint64_t arg3,
|
uint64_t arg3,
|
||||||
|
@ -324,18 +325,20 @@ tsp_args_t *tsp_fast_smc_handler(uint64_t func,
|
||||||
uint64_t service_args[2];
|
uint64_t service_args[2];
|
||||||
uint64_t mpidr = read_mpidr();
|
uint64_t mpidr = read_mpidr();
|
||||||
uint32_t linear_id = platform_get_core_pos(mpidr);
|
uint32_t linear_id = platform_get_core_pos(mpidr);
|
||||||
|
const char *smc_type;
|
||||||
|
|
||||||
/* Update this cpu's statistics */
|
/* Update this cpu's statistics */
|
||||||
tsp_stats[linear_id].smc_count++;
|
tsp_stats[linear_id].smc_count++;
|
||||||
tsp_stats[linear_id].eret_count++;
|
tsp_stats[linear_id].eret_count++;
|
||||||
|
|
||||||
printf("SP: cpu 0x%x received fast smc 0x%x\n", read_mpidr(), func);
|
smc_type = ((func >> 31) & 1) == 1 ? "fast" : "standard";
|
||||||
|
|
||||||
|
printf("SP: cpu 0x%x received %s smc 0x%x\n", read_mpidr(), smc_type, func);
|
||||||
INFO("cpu 0x%x: %d smcs, %d erets\n", mpidr,
|
INFO("cpu 0x%x: %d smcs, %d erets\n", mpidr,
|
||||||
tsp_stats[linear_id].smc_count,
|
tsp_stats[linear_id].smc_count,
|
||||||
tsp_stats[linear_id].eret_count);
|
tsp_stats[linear_id].eret_count);
|
||||||
|
|
||||||
/* Render secure services and obtain results here */
|
/* Render secure services and obtain results here */
|
||||||
|
|
||||||
results[0] = arg1;
|
results[0] = arg1;
|
||||||
results[1] = arg2;
|
results[1] = arg2;
|
||||||
|
|
||||||
|
@ -346,20 +349,20 @@ tsp_args_t *tsp_fast_smc_handler(uint64_t func,
|
||||||
tsp_get_magic(service_args);
|
tsp_get_magic(service_args);
|
||||||
|
|
||||||
/* Determine the function to perform based on the function ID */
|
/* Determine the function to perform based on the function ID */
|
||||||
switch (func) {
|
switch (TSP_BARE_FID(func)) {
|
||||||
case TSP_FID_ADD:
|
case TSP_ADD:
|
||||||
results[0] += service_args[0];
|
results[0] += service_args[0];
|
||||||
results[1] += service_args[1];
|
results[1] += service_args[1];
|
||||||
break;
|
break;
|
||||||
case TSP_FID_SUB:
|
case TSP_SUB:
|
||||||
results[0] -= service_args[0];
|
results[0] -= service_args[0];
|
||||||
results[1] -= service_args[1];
|
results[1] -= service_args[1];
|
||||||
break;
|
break;
|
||||||
case TSP_FID_MUL:
|
case TSP_MUL:
|
||||||
results[0] *= service_args[0];
|
results[0] *= service_args[0];
|
||||||
results[1] *= service_args[1];
|
results[1] *= service_args[1];
|
||||||
break;
|
break;
|
||||||
case TSP_FID_DIV:
|
case TSP_DIV:
|
||||||
results[0] /= service_args[0] ? service_args[0] : 1;
|
results[0] /= service_args[0] ? service_args[0] : 1;
|
||||||
results[1] /= service_args[1] ? service_args[1] : 1;
|
results[1] /= service_args[1] ? service_args[1] : 1;
|
||||||
break;
|
break;
|
||||||
|
@ -367,9 +370,9 @@ tsp_args_t *tsp_fast_smc_handler(uint64_t func,
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
return set_smc_args(func,
|
return set_smc_args(func, 0,
|
||||||
results[0],
|
results[0],
|
||||||
results[1],
|
results[1],
|
||||||
0, 0, 0, 0, 0);
|
0, 0, 0, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -51,13 +51,15 @@
|
||||||
|
|
||||||
#define GET_SMC_CC(id) ((id >> FUNCID_CC_SHIFT) & \
|
#define GET_SMC_CC(id) ((id >> FUNCID_CC_SHIFT) & \
|
||||||
FUNCID_CC_MASK)
|
FUNCID_CC_MASK)
|
||||||
|
#define GET_SMC_TYPE(id) ((id >> FUNCID_TYPE_SHIFT) & \
|
||||||
|
FUNCID_TYPE_MASK)
|
||||||
|
|
||||||
#define SMC_64 1
|
#define SMC_64 1
|
||||||
#define SMC_32 0
|
#define SMC_32 0
|
||||||
#define SMC_UNK 0xffffffff
|
#define SMC_UNK 0xffffffff
|
||||||
#define SMC_TYPE_FAST 1
|
#define SMC_TYPE_FAST 1
|
||||||
#define SMC_TYPE_STD 0
|
#define SMC_TYPE_STD 0
|
||||||
|
#define SMC_PREEMPTED 0xfffffffe
|
||||||
/*******************************************************************************
|
/*******************************************************************************
|
||||||
* Owning entity number definitions inside the function id as per the SMC
|
* Owning entity number definitions inside the function id as per the SMC
|
||||||
* calling convention
|
* calling convention
|
||||||
|
|
|
@ -40,7 +40,7 @@
|
||||||
#define TSP_OFF_DONE 0xf2000002
|
#define TSP_OFF_DONE 0xf2000002
|
||||||
#define TSP_SUSPEND_DONE 0xf2000003
|
#define TSP_SUSPEND_DONE 0xf2000003
|
||||||
#define TSP_RESUME_DONE 0xf2000004
|
#define TSP_RESUME_DONE 0xf2000004
|
||||||
#define TSP_WORK_DONE 0xf2000005
|
#define TSP_PREEMPTED 0xf2000005
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Function identifiers to handle FIQs through the synchronous handling model.
|
* Function identifiers to handle FIQs through the synchronous handling model.
|
||||||
|
@ -49,16 +49,35 @@
|
||||||
*/
|
*/
|
||||||
#define TSP_HANDLED_S_EL1_FIQ 0xf2000006
|
#define TSP_HANDLED_S_EL1_FIQ 0xf2000006
|
||||||
#define TSP_EL3_FIQ 0xf2000007
|
#define TSP_EL3_FIQ 0xf2000007
|
||||||
#define TSP_HANDLE_FIQ_AND_RETURN 0x2004
|
|
||||||
|
|
||||||
/* SMC function ID that TSP uses to request service from secure monitor */
|
/* SMC function ID that TSP uses to request service from secure monitor */
|
||||||
#define TSP_GET_ARGS 0xf2001000
|
#define TSP_GET_ARGS 0xf2001000
|
||||||
|
|
||||||
/* Function IDs for various TSP services */
|
/*
|
||||||
#define TSP_FID_ADD 0xf2002000
|
* Identifiers for various TSP services. Corresponding function IDs (whether
|
||||||
#define TSP_FID_SUB 0xf2002001
|
* fast or standard) are generated by macros defined below
|
||||||
#define TSP_FID_MUL 0xf2002002
|
*/
|
||||||
#define TSP_FID_DIV 0xf2002003
|
#define TSP_ADD 0x2000
|
||||||
|
#define TSP_SUB 0x2001
|
||||||
|
#define TSP_MUL 0x2002
|
||||||
|
#define TSP_DIV 0x2003
|
||||||
|
#define TSP_HANDLE_FIQ_AND_RETURN 0x2004
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Generate function IDs for TSP services to be used in SMC calls, by
|
||||||
|
* appropriately setting bit 31 to differentiate standard and fast SMC calls
|
||||||
|
*/
|
||||||
|
#define TSP_STD_FID(fid) ((fid) | 0x72000000 | (0 << 31))
|
||||||
|
#define TSP_FAST_FID(fid) ((fid) | 0x72000000 | (1 << 31))
|
||||||
|
|
||||||
|
/* SMC function ID to request a previously preempted std smc */
|
||||||
|
#define TSP_FID_RESUME TSP_STD_FID(0x3000)
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Identify a TSP service from function ID filtering the last 16 bits from the
|
||||||
|
* SMC function ID
|
||||||
|
*/
|
||||||
|
#define TSP_BARE_FID(fid) ((fid) & 0xffff)
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Total number of function IDs implemented for services offered to NS clients.
|
* Total number of function IDs implemented for services offered to NS clients.
|
||||||
|
@ -108,6 +127,7 @@ typedef void (*tsp_generic_fptr_t)(uint64_t arg0,
|
||||||
uint64_t arg7);
|
uint64_t arg7);
|
||||||
|
|
||||||
typedef struct entry_info {
|
typedef struct entry_info {
|
||||||
|
tsp_generic_fptr_t std_smc_entry;
|
||||||
tsp_generic_fptr_t fast_smc_entry;
|
tsp_generic_fptr_t fast_smc_entry;
|
||||||
tsp_generic_fptr_t cpu_on_entry;
|
tsp_generic_fptr_t cpu_on_entry;
|
||||||
tsp_generic_fptr_t cpu_off_entry;
|
tsp_generic_fptr_t cpu_off_entry;
|
||||||
|
@ -118,6 +138,7 @@ typedef struct entry_info {
|
||||||
|
|
||||||
typedef struct work_statistics {
|
typedef struct work_statistics {
|
||||||
uint32_t fiq_count; /* Number of FIQs on this cpu */
|
uint32_t fiq_count; /* Number of FIQs on this cpu */
|
||||||
|
uint32_t irq_count; /* Number of IRQs on this cpu */
|
||||||
uint32_t sync_fiq_count; /* Number of sync. fiqs on this cpu */
|
uint32_t sync_fiq_count; /* Number of sync. fiqs on this cpu */
|
||||||
uint32_t sync_fiq_ret_count; /* Number of fiq returns on this cpu */
|
uint32_t sync_fiq_ret_count; /* Number of fiq returns on this cpu */
|
||||||
uint32_t smc_count; /* Number of returns on this cpu */
|
uint32_t smc_count; /* Number of returns on this cpu */
|
||||||
|
@ -153,6 +174,14 @@ extern void tsp_fiq_entry(uint64_t arg0,
|
||||||
uint64_t arg5,
|
uint64_t arg5,
|
||||||
uint64_t arg6,
|
uint64_t arg6,
|
||||||
uint64_t arg7);
|
uint64_t arg7);
|
||||||
|
extern void tsp_std_smc_entry(uint64_t arg0,
|
||||||
|
uint64_t arg1,
|
||||||
|
uint64_t arg2,
|
||||||
|
uint64_t arg3,
|
||||||
|
uint64_t arg4,
|
||||||
|
uint64_t arg5,
|
||||||
|
uint64_t arg6,
|
||||||
|
uint64_t arg7);
|
||||||
extern void tsp_fast_smc_entry(uint64_t arg0,
|
extern void tsp_fast_smc_entry(uint64_t arg0,
|
||||||
uint64_t arg1,
|
uint64_t arg1,
|
||||||
uint64_t arg2,
|
uint64_t arg2,
|
||||||
|
|
|
@ -257,7 +257,6 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
|
||||||
uint64_t flags)
|
uint64_t flags)
|
||||||
{
|
{
|
||||||
cpu_context_t *ns_cpu_context;
|
cpu_context_t *ns_cpu_context;
|
||||||
gp_regs_t *ns_gp_regs;
|
|
||||||
unsigned long mpidr = read_mpidr();
|
unsigned long mpidr = read_mpidr();
|
||||||
uint32_t linear_id = platform_get_core_pos(mpidr), ns;
|
uint32_t linear_id = platform_get_core_pos(mpidr), ns;
|
||||||
tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
|
tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
|
||||||
|
@ -267,6 +266,31 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
|
||||||
|
|
||||||
switch (smc_fid) {
|
switch (smc_fid) {
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This function ID is used by TSP to indicate that it was
|
||||||
|
* preempted by a normal world IRQ.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
case TSP_PREEMPTED:
|
||||||
|
if (ns)
|
||||||
|
SMC_RET1(handle, SMC_UNK);
|
||||||
|
|
||||||
|
assert(handle == cm_get_context(mpidr, SECURE));
|
||||||
|
cm_el1_sysregs_context_save(SECURE);
|
||||||
|
/* Get a reference to the non-secure context */
|
||||||
|
ns_cpu_context = cm_get_context(mpidr, NON_SECURE);
|
||||||
|
assert(ns_cpu_context);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Restore non-secure state. There is no need to save the
|
||||||
|
* secure system register context since the TSP was supposed
|
||||||
|
* to preserve it during S-EL1 interrupt handling.
|
||||||
|
*/
|
||||||
|
cm_el1_sysregs_context_restore(NON_SECURE);
|
||||||
|
cm_set_next_eret_context(NON_SECURE);
|
||||||
|
|
||||||
|
SMC_RET1(ns_cpu_context, SMC_PREEMPTED);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* This function ID is used only by the TSP to indicate that it has
|
* This function ID is used only by the TSP to indicate that it has
|
||||||
* finished handling a S-EL1 FIQ interrupt. Execution should resume
|
* finished handling a S-EL1 FIQ interrupt. Execution should resume
|
||||||
|
@ -357,9 +381,6 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
|
||||||
*/
|
*/
|
||||||
tspd_synchronous_sp_exit(tsp_ctx, x1);
|
tspd_synchronous_sp_exit(tsp_ctx, x1);
|
||||||
|
|
||||||
/* Should never reach here */
|
|
||||||
assert(0);
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* These function IDs is used only by the SP to indicate it has
|
* These function IDs is used only by the SP to indicate it has
|
||||||
* finished:
|
* finished:
|
||||||
|
@ -392,18 +413,20 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
|
||||||
*/
|
*/
|
||||||
tspd_synchronous_sp_exit(tsp_ctx, x1);
|
tspd_synchronous_sp_exit(tsp_ctx, x1);
|
||||||
|
|
||||||
/* Should never reach here */
|
|
||||||
assert(0);
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Request from non-secure client to perform an
|
* Request from non-secure client to perform an
|
||||||
* arithmetic operation or response from secure
|
* arithmetic operation or response from secure
|
||||||
* payload to an earlier request.
|
* payload to an earlier request.
|
||||||
*/
|
*/
|
||||||
case TSP_FID_ADD:
|
case TSP_FAST_FID(TSP_ADD):
|
||||||
case TSP_FID_SUB:
|
case TSP_FAST_FID(TSP_SUB):
|
||||||
case TSP_FID_MUL:
|
case TSP_FAST_FID(TSP_MUL):
|
||||||
case TSP_FID_DIV:
|
case TSP_FAST_FID(TSP_DIV):
|
||||||
|
|
||||||
|
case TSP_STD_FID(TSP_ADD):
|
||||||
|
case TSP_STD_FID(TSP_SUB):
|
||||||
|
case TSP_STD_FID(TSP_MUL):
|
||||||
|
case TSP_STD_FID(TSP_DIV):
|
||||||
if (ns) {
|
if (ns) {
|
||||||
/*
|
/*
|
||||||
* This is a fresh request from the non-secure client.
|
* This is a fresh request from the non-secure client.
|
||||||
|
@ -412,11 +435,15 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
|
||||||
* state and send the request to the secure payload.
|
* state and send the request to the secure payload.
|
||||||
*/
|
*/
|
||||||
assert(handle == cm_get_context(mpidr, NON_SECURE));
|
assert(handle == cm_get_context(mpidr, NON_SECURE));
|
||||||
|
|
||||||
|
/* Check if we are already preempted */
|
||||||
|
if (get_std_smc_active_flag(tsp_ctx->state))
|
||||||
|
SMC_RET1(handle, SMC_UNK);
|
||||||
|
|
||||||
cm_el1_sysregs_context_save(NON_SECURE);
|
cm_el1_sysregs_context_save(NON_SECURE);
|
||||||
|
|
||||||
/* Save x1 and x2 for use by TSP_GET_ARGS call below */
|
/* Save x1 and x2 for use by TSP_GET_ARGS call below */
|
||||||
SMC_SET_GP(handle, CTX_GPREG_X1, x1);
|
store_tsp_args(tsp_ctx, x1, x2);
|
||||||
SMC_SET_GP(handle, CTX_GPREG_X2, x2);
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* We are done stashing the non-secure context. Ask the
|
* We are done stashing the non-secure context. Ask the
|
||||||
|
@ -431,17 +458,27 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
|
||||||
* from this function.
|
* from this function.
|
||||||
*/
|
*/
|
||||||
assert(&tsp_ctx->cpu_ctx == cm_get_context(mpidr, SECURE));
|
assert(&tsp_ctx->cpu_ctx == cm_get_context(mpidr, SECURE));
|
||||||
set_aapcs_args7(&tsp_ctx->cpu_ctx, smc_fid, x1, x2, 0, 0,
|
|
||||||
0, 0, 0);
|
/* Set appropriate entry for SMC.
|
||||||
cm_set_elr_el3(SECURE, (uint64_t) tsp_entry_info->fast_smc_entry);
|
* We expect the TSP to manage the PSTATE.I and PSTATE.F
|
||||||
|
* flags as appropriate.
|
||||||
|
*/
|
||||||
|
if (GET_SMC_TYPE(smc_fid) == SMC_TYPE_FAST) {
|
||||||
|
cm_set_elr_el3(SECURE, (uint64_t)
|
||||||
|
tsp_entry_info->fast_smc_entry);
|
||||||
|
} else {
|
||||||
|
set_std_smc_active_flag(tsp_ctx->state);
|
||||||
|
cm_set_elr_el3(SECURE, (uint64_t)
|
||||||
|
tsp_entry_info->std_smc_entry);
|
||||||
|
}
|
||||||
|
|
||||||
cm_el1_sysregs_context_restore(SECURE);
|
cm_el1_sysregs_context_restore(SECURE);
|
||||||
cm_set_next_eret_context(SECURE);
|
cm_set_next_eret_context(SECURE);
|
||||||
|
SMC_RET3(&tsp_ctx->cpu_ctx, smc_fid, x1, x2);
|
||||||
return smc_fid;
|
|
||||||
} else {
|
} else {
|
||||||
/*
|
/*
|
||||||
* This is the result from the secure client of an
|
* This is the result from the secure client of an
|
||||||
* earlier request. The results are in x1-x2. Copy it
|
* earlier request. The results are in x1-x3. Copy it
|
||||||
* into the non-secure context, save the secure state
|
* into the non-secure context, save the secure state
|
||||||
* and return to the non-secure state.
|
* and return to the non-secure state.
|
||||||
*/
|
*/
|
||||||
|
@ -451,17 +488,52 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
|
||||||
/* Get a reference to the non-secure context */
|
/* Get a reference to the non-secure context */
|
||||||
ns_cpu_context = cm_get_context(mpidr, NON_SECURE);
|
ns_cpu_context = cm_get_context(mpidr, NON_SECURE);
|
||||||
assert(ns_cpu_context);
|
assert(ns_cpu_context);
|
||||||
ns_gp_regs = get_gpregs_ctx(ns_cpu_context);
|
|
||||||
|
|
||||||
/* Restore non-secure state */
|
/* Restore non-secure state */
|
||||||
cm_el1_sysregs_context_restore(NON_SECURE);
|
cm_el1_sysregs_context_restore(NON_SECURE);
|
||||||
cm_set_next_eret_context(NON_SECURE);
|
cm_set_next_eret_context(NON_SECURE);
|
||||||
|
if (GET_SMC_TYPE(smc_fid) == SMC_TYPE_STD)
|
||||||
SMC_RET2(ns_gp_regs, x1, x2);
|
clr_std_smc_active_flag(tsp_ctx->state);
|
||||||
|
SMC_RET3(ns_cpu_context, x1, x2, x3);
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Request from non secure world to resume the preempted
|
||||||
|
* Standard SMC call.
|
||||||
|
*/
|
||||||
|
case TSP_FID_RESUME:
|
||||||
|
/* RESUME should be invoked only by normal world */
|
||||||
|
if (!ns) {
|
||||||
|
assert(0);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This is a resume request from the non-secure client.
|
||||||
|
* save the non-secure state and send the request to
|
||||||
|
* the secure payload.
|
||||||
|
*/
|
||||||
|
assert(handle == cm_get_context(mpidr, NON_SECURE));
|
||||||
|
|
||||||
|
/* Check if we are already preempted before resume */
|
||||||
|
if (!get_std_smc_active_flag(tsp_ctx->state))
|
||||||
|
SMC_RET1(handle, SMC_UNK);
|
||||||
|
|
||||||
|
cm_el1_sysregs_context_save(NON_SECURE);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* We are done stashing the non-secure context. Ask the
|
||||||
|
* secure payload to do the work now.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* We just need to return to the preempted point in
|
||||||
|
* TSP and the execution will resume as normal.
|
||||||
|
*/
|
||||||
|
cm_el1_sysregs_context_restore(SECURE);
|
||||||
|
cm_set_next_eret_context(SECURE);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* This is a request from the secure payload for more arguments
|
* This is a request from the secure payload for more arguments
|
||||||
* for an ongoing arithmetic operation requested by the
|
* for an ongoing arithmetic operation requested by the
|
||||||
|
@ -475,10 +547,9 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
|
||||||
/* Get a reference to the non-secure context */
|
/* Get a reference to the non-secure context */
|
||||||
ns_cpu_context = cm_get_context(mpidr, NON_SECURE);
|
ns_cpu_context = cm_get_context(mpidr, NON_SECURE);
|
||||||
assert(ns_cpu_context);
|
assert(ns_cpu_context);
|
||||||
ns_gp_regs = get_gpregs_ctx(ns_cpu_context);
|
|
||||||
|
|
||||||
SMC_RET2(handle, read_ctx_reg(ns_gp_regs, CTX_GPREG_X1),
|
get_tsp_args(tsp_ctx, x1, x2);
|
||||||
read_ctx_reg(ns_gp_regs, CTX_GPREG_X2));
|
SMC_RET2(handle, x1, x2);
|
||||||
|
|
||||||
case TOS_CALL_COUNT:
|
case TOS_CALL_COUNT:
|
||||||
/*
|
/*
|
||||||
|
@ -502,9 +573,9 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
|
||||||
SMC_RET1(handle, SMC_UNK);
|
SMC_RET1(handle, SMC_UNK);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Define a SPD runtime service descriptor */
|
/* Define a SPD runtime service descriptor for fast SMC calls */
|
||||||
DECLARE_RT_SVC(
|
DECLARE_RT_SVC(
|
||||||
spd,
|
tspd_fast,
|
||||||
|
|
||||||
OEN_TOS_START,
|
OEN_TOS_START,
|
||||||
OEN_TOS_END,
|
OEN_TOS_END,
|
||||||
|
@ -512,3 +583,14 @@ DECLARE_RT_SVC(
|
||||||
tspd_setup,
|
tspd_setup,
|
||||||
tspd_smc_handler
|
tspd_smc_handler
|
||||||
);
|
);
|
||||||
|
|
||||||
|
/* Define a SPD runtime service descriptor for standard SMC calls */
|
||||||
|
DECLARE_RT_SVC(
|
||||||
|
tspd_std,
|
||||||
|
|
||||||
|
OEN_TOS_START,
|
||||||
|
OEN_TOS_END,
|
||||||
|
SMC_TYPE_STD,
|
||||||
|
NULL,
|
||||||
|
tspd_smc_handler
|
||||||
|
);
|
||||||
|
|
|
@ -125,6 +125,12 @@
|
||||||
#include <cassert.h>
|
#include <cassert.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
|
/*
|
||||||
|
* The number of arguments to save during a SMC call for TSP.
|
||||||
|
* Currently only x1 and x2 are used by TSP.
|
||||||
|
*/
|
||||||
|
#define TSP_NUM_ARGS 0x2
|
||||||
|
|
||||||
/* AArch64 callee saved general purpose register context structure. */
|
/* AArch64 callee saved general purpose register context structure. */
|
||||||
DEFINE_REG_STRUCT(c_rt_regs, TSPD_C_RT_CTX_ENTRIES);
|
DEFINE_REG_STRUCT(c_rt_regs, TSPD_C_RT_CTX_ENTRIES);
|
||||||
|
|
||||||
|
@ -147,6 +153,8 @@ CASSERT(TSPD_C_RT_CTX_SIZE == sizeof(c_rt_regs_t), \
|
||||||
* 'c_rt_ctx' - stack address to restore C runtime context from after
|
* 'c_rt_ctx' - stack address to restore C runtime context from after
|
||||||
* returning from a synchronous entry into the SP.
|
* returning from a synchronous entry into the SP.
|
||||||
* 'cpu_ctx' - space to maintain SP architectural state
|
* 'cpu_ctx' - space to maintain SP architectural state
|
||||||
|
* 'saved_tsp_args' - space to store arguments for TSP arithmetic operations
|
||||||
|
* which will queried using the TSP_GET_ARGS SMC by TSP.
|
||||||
******************************************************************************/
|
******************************************************************************/
|
||||||
typedef struct tsp_context {
|
typedef struct tsp_context {
|
||||||
uint64_t saved_elr_el3;
|
uint64_t saved_elr_el3;
|
||||||
|
@ -155,8 +163,20 @@ typedef struct tsp_context {
|
||||||
uint64_t mpidr;
|
uint64_t mpidr;
|
||||||
uint64_t c_rt_ctx;
|
uint64_t c_rt_ctx;
|
||||||
cpu_context_t cpu_ctx;
|
cpu_context_t cpu_ctx;
|
||||||
|
uint64_t saved_tsp_args[TSP_NUM_ARGS];
|
||||||
} tsp_context_t;
|
} tsp_context_t;
|
||||||
|
|
||||||
|
/* Helper macros to store and retrieve tsp args from tsp_context */
|
||||||
|
#define store_tsp_args(tsp_ctx, x1, x2) do {\
|
||||||
|
tsp_ctx->saved_tsp_args[0] = x1;\
|
||||||
|
tsp_ctx->saved_tsp_args[1] = x2;\
|
||||||
|
} while (0)
|
||||||
|
|
||||||
|
#define get_tsp_args(tsp_ctx, x1, x2) do {\
|
||||||
|
x1 = tsp_ctx->saved_tsp_args[0];\
|
||||||
|
x2 = tsp_ctx->saved_tsp_args[1];\
|
||||||
|
} while (0)
|
||||||
|
|
||||||
/* TSPD power management handlers */
|
/* TSPD power management handlers */
|
||||||
extern const spd_pm_ops_t tspd_pm;
|
extern const spd_pm_ops_t tspd_pm;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue