bool thread_init_stack(uint32_t thread_id, vaddr_t sp)
{
switch (thread_id) {
case THREAD_TMP_STACK: {
struct thread_core_local *l = get_core_local();
l->tmp_stack_va_end = sp;
l->curr_thread = -1;
thread_set_irq_sp(sp);
thread_set_fiq_sp(sp);
break;
}
case THREAD_ABT_STACK:
thread_set_abt_sp(sp);
break;
default:
if (thread_id >= NUM_THREADS)
return false;
if (threads[thread_id].state != THREAD_STATE_FREE)
return false;
threads[thread_id].stack_va_end = sp;
}
return true;
}
struct thread_ctx_regs *thread_get_ctx_regs(void)
{
struct thread_core_local *l = get_core_local();
assert(l->curr_thread != -1);
return &threads[l->curr_thread].regs;
}
static void thread_alloc_and_run(struct thread_smc_args *args)
{
size_t n;
struct thread_core_local *l = get_core_local();
bool found_thread = false;
assert(l->curr_thread == -1);
lock_global();
if (!have_one_active_thread() && !have_one_preempted_thread()) {
for (n = 0; n < NUM_THREADS; n++) {
if (threads[n].state == THREAD_STATE_FREE) {
threads[n].state = THREAD_STATE_ACTIVE;
found_thread = true;
break;
}
}
}
unlock_global();
if (!found_thread) {
args->a0 = TEESMC_RETURN_EBUSY;
args->a1 = 0;
args->a2 = 0;
args->a3 = 0;
return;
}
l->curr_thread = n;
threads[n].regs.pc = (uint32_t)thread_stdcall_entry;
/* Stdcalls starts in SVC mode with masked IRQ and unmasked FIQ */
threads[n].regs.cpsr = CPSR_MODE_SVC | CPSR_I;
threads[n].flags = 0;
/* Enable thumb mode if it's a thumb instruction */
if (threads[n].regs.pc & 1)
threads[n].regs.cpsr |= CPSR_T;
/* Reinitialize stack pointer */
threads[n].regs.svc_sp = threads[n].stack_va_end;
/*
* Copy arguments into context. This will make the
* arguments appear in r0-r7 when thread is started.
*/
threads[n].regs.r0 = args->a0;
threads[n].regs.r1 = args->a1;
threads[n].regs.r2 = args->a2;
threads[n].regs.r3 = args->a3;
threads[n].regs.r4 = args->a4;
threads[n].regs.r5 = args->a5;
threads[n].regs.r6 = args->a6;
threads[n].regs.r7 = args->a7;
/* Save Hypervisor Client ID */
threads[n].hyp_clnt_id = args->a7;
thread_resume(&threads[n].regs);
}
int thread_state_suspend(uint32_t flags, uint32_t cpsr, uint32_t pc)
{
struct thread_core_local *l = get_core_local();
int ct = l->curr_thread;
assert(ct != -1);
check_canaries();
lock_global();
assert(threads[ct].state == THREAD_STATE_ACTIVE);
threads[ct].flags |= flags;
threads[ct].regs.cpsr = cpsr;
threads[ct].regs.pc = pc;
threads[ct].state = THREAD_STATE_SUSPENDED;
threads[ct].have_user_map = !tee_mmu_is_kernel_mapping();
if (threads[ct].have_user_map) {
tee_mmu_get_map(&threads[ct].user_map);
tee_mmu_set_map(NULL);
}
l->curr_thread = -1;
unlock_global();
return ct;
}
void thread_set_tsd(void *tsd, thread_tsd_free_t free_func)
{
struct thread_core_local *l = get_core_local();
assert(l->curr_thread != -1);
assert(threads[l->curr_thread].state == THREAD_STATE_ACTIVE);
threads[l->curr_thread].tsd = tsd;
threads[l->curr_thread].tsd_free = free_func;
}
void *thread_get_tsd(void)
{
struct thread_core_local *l = get_core_local();
int ct = l->curr_thread;
if (ct == -1 || threads[ct].state != THREAD_STATE_ACTIVE)
return NULL;
else
return threads[ct].tsd;
}
static void thread_resume_from_rpc(struct thread_smc_args *args)
{
size_t n = args->a3; /* thread id */
struct thread_core_local *l = get_core_local();
uint32_t rv = 0;
assert(l->curr_thread == -1);
lock_global();
if (have_one_active_thread()) {
rv = TEESMC_RETURN_EBUSY;
} else if (n < NUM_THREADS &&
threads[n].state == THREAD_STATE_SUSPENDED &&
args->a7 == threads[n].hyp_clnt_id) {
threads[n].state = THREAD_STATE_ACTIVE;
} else {
rv = TEESMC_RETURN_ERESUME;
}
unlock_global();
if (rv) {
args->a0 = rv;
args->a1 = 0;
args->a2 = 0;
args->a3 = 0;
return;
}
l->curr_thread = n;
if (threads[n].have_user_map)
tee_mmu_set_map(&threads[n].user_map);
/*
* Return from RPC to request service of an IRQ must not
* get parameters from non-secure world.
*/
if (threads[n].flags & THREAD_FLAGS_COPY_ARGS_ON_RETURN) {
/*
* Update returned values from RPC, values will appear in
* r0-r3 when thread is resumed.
*/
threads[n].regs.r0 = args->a0;
threads[n].regs.r1 = args->a1;
threads[n].regs.r2 = args->a2;
threads[n].regs.r3 = args->a3;
threads[n].flags &= ~THREAD_FLAGS_COPY_ARGS_ON_RETURN;
}
thread_resume(&threads[n].regs);
}
void thread_restore_irq(void)
{
struct thread_core_local *l;
uint32_t cpsr = read_cpsr();
/* get_core_local() requires IRQs to be disabled */
write_cpsr(cpsr | CPSR_I);
l = get_core_local();
assert(l->curr_thread != -1);
if (threads[l->curr_thread].flags & THREAD_FLAGS_IRQ_ENABLE)
write_cpsr(cpsr & ~CPSR_I);
}
void thread_state_free(void)
{
struct thread_core_local *l = get_core_local();
assert(l->curr_thread != -1);
lock_global();
assert(threads[l->curr_thread].state == THREAD_STATE_ACTIVE);
threads[l->curr_thread].state = THREAD_STATE_FREE;
threads[l->curr_thread].flags = 0;
l->curr_thread = -1;
unlock_global();
}
void thread_set_tsd(void *tsd)
{
uint32_t cpsr = read_cpsr();
struct thread_core_local *l;
int ct;
/* get_core_local() requires IRQs to be disabled */
write_cpsr(cpsr | CPSR_I);
l = get_core_local();
ct = l->curr_thread;
assert(ct != -1);
assert(threads[ct].state == THREAD_STATE_ACTIVE);
threads[ct].tsd = tsd;
write_cpsr(cpsr);
}
void *thread_get_tsd(void)
{
uint32_t cpsr = read_cpsr();
struct thread_core_local *l;
int ct;
void *tsd;
/* get_core_local() requires IRQs to be disabled */
write_cpsr(cpsr | CPSR_I);
l = get_core_local();
ct = l->curr_thread;
if (ct == -1 || threads[ct].state != THREAD_STATE_ACTIVE)
tsd = NULL;
else
tsd = threads[ct].tsd;
write_cpsr(cpsr);
return tsd;
}
void thread_set_irq(bool enable)
{
struct thread_core_local *l;
uint32_t cpsr = read_cpsr();
/* get_core_local() requires IRQs to be disabled */
write_cpsr(cpsr | CPSR_I);
l = get_core_local();
assert(l->curr_thread != -1);
if (enable) {
threads[l->curr_thread].flags |= THREAD_FLAGS_IRQ_ENABLE;
write_cpsr(cpsr & ~CPSR_I);
} else {
/*
* No need to disable IRQ here since it's already disabled
* above.
*/
threads[l->curr_thread].flags &= ~THREAD_FLAGS_IRQ_ENABLE;
}
}
void *thread_get_tmp_sp(void)
{
struct thread_core_local *l = get_core_local();
return (void *)l->tmp_stack_va_end;
}
