143 lines
3.9 KiB
C
143 lines
3.9 KiB
C
/*
|
|
* Copyright (c) 2019 Intel Corporation
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#include <zephyr/device.h>
|
|
#include <zephyr/init.h>
|
|
#include <zephyr/kernel.h>
|
|
#include <zephyr/kernel_structs.h>
|
|
#include <zephyr/toolchain.h>
|
|
#include <zephyr/sys/__assert.h>
|
|
#include <zephyr/sys/sys_io.h>
|
|
|
|
#include <xtensa/config/core-isa.h>
|
|
|
|
#include <zephyr/logging/log.h>
|
|
LOG_MODULE_REGISTER(soc_mp, CONFIG_SOC_LOG_LEVEL);
|
|
|
|
#include <zephyr/zsr.h>
|
|
#include <cavs-idc.h>
|
|
#include <soc.h>
|
|
#include <zephyr/cache.h>
|
|
#include <adsp_shim.h>
|
|
#include <adsp_memory.h>
|
|
#include <cpu_init.h>
|
|
|
|
struct cpustart_rec {
|
|
uint32_t cpu;
|
|
arch_cpustart_t fn;
|
|
void *arg;
|
|
};
|
|
|
|
static struct cpustart_rec start_rec;
|
|
const uint32_t *z_mp_start_cpu = &start_rec.cpu;
|
|
|
|
char *z_mp_stack_top;
|
|
|
|
/* Vestigial silliness: An old mechanism for core startup would embed
|
|
* a "manifest" of code to copy to LP-SRAM at startup (vs. the tiny
|
|
* trampoline we use here). This was constructed in the linker
|
|
* script, and the first word would encode the number of entries. As
|
|
* it happens, SOF still emits the code to copy this data, so it needs
|
|
* to see this symbol point to a zero.
|
|
*/
|
|
uint32_t _loader_storage_manifest_start;
|
|
|
|
/* Simple array of CPUs that are active and available for an IPI. The
|
|
* IDC interrupt is ALSO used to bring a CPU out of reset, so we need
|
|
* to be absolutely sure we don't try to IPI a CPU that isn't ready to
|
|
* start, or else we'll launch it into garbage and crash the DSP.
|
|
*/
|
|
bool soc_cpus_active[CONFIG_MP_MAX_NUM_CPUS];
|
|
|
|
#define NOP4 "nop; nop; nop; nop;"
|
|
#define NOP32 NOP4 NOP4 NOP4 NOP4 NOP4 NOP4 NOP4 NOP4
|
|
#define NOP128 NOP32 NOP32 NOP32 NOP32
|
|
/* Tiny assembly stub for calling z_mp_entry() on the auxiliary CPUs.
|
|
* Mask interrupts, clear the register window state and set the stack
|
|
* pointer. This represents the minimum work required to run C code
|
|
* safely.
|
|
*
|
|
* Note that alignment is absolutely required: the IDC protocol passes
|
|
* only the upper 30 bits of the address to the second CPU.
|
|
*/
|
|
__asm__(".section .text.z_soc_mp_asm_entry, \"x\" \n\t"
|
|
".align 4 \n\t"
|
|
".global z_soc_mp_asm_entry \n\t"
|
|
"z_soc_mp_asm_entry: \n\t"
|
|
" movi a0, 0x4002f \n\t" /* WOE | UM | INTLEVEL(max) */
|
|
" wsr a0, PS \n\t"
|
|
" movi a0, 0 \n\t"
|
|
" wsr a0, WINDOWBASE \n\t"
|
|
" movi a0, 1 \n\t"
|
|
" wsr a0, WINDOWSTART \n\t"
|
|
" rsync \n\t"
|
|
" movi a1, z_mp_start_cpu \n\t"
|
|
" l32i a1, a1, 0 \n\t"
|
|
" l32i a1, a1, 0 \n\t"
|
|
" rsr a2, PRID \n\t"
|
|
" sub a2, a2, a1 \n\t"
|
|
" bnez a2, soc_mp_idle \n\t"
|
|
" movi a1, z_mp_stack_top \n\t"
|
|
" l32i a1, a1, 0 \n\t"
|
|
" call4 z_mp_entry \n\t"
|
|
"soc_mp_idle: \n\t"
|
|
#ifdef CONFIG_XTENSA_WAITI_BUG
|
|
NOP128
|
|
" isync \n\t"
|
|
" extw \n\t"
|
|
#endif
|
|
" waiti 0 \n\t" /* Power-gating is allowed, we'll exit via reset */
|
|
" j soc_mp_idle \n\t");
|
|
|
|
#undef NOP128
|
|
#undef NOP32
|
|
#undef NOP4
|
|
|
|
static __imr void __used z_mp_entry(void)
|
|
{
|
|
cpu_early_init();
|
|
/* Set up the CPU pointer. */
|
|
_cpu_t *cpu = &_kernel.cpus[start_rec.cpu];
|
|
|
|
__asm__ volatile("wsr %0, " ZSR_CPU_STR :: "r"(cpu));
|
|
|
|
soc_mp_startup(start_rec.cpu);
|
|
soc_cpus_active[start_rec.cpu] = true;
|
|
start_rec.fn(start_rec.arg);
|
|
__ASSERT(false, "arch_cpu_start() handler should never return");
|
|
}
|
|
|
|
void mp_resume_entry(void)
|
|
{
|
|
start_rec.fn(start_rec.arg);
|
|
}
|
|
|
|
bool arch_cpu_active(int cpu_num)
|
|
{
|
|
return soc_cpus_active[cpu_num];
|
|
}
|
|
|
|
void arch_cpu_start(int cpu_num, k_thread_stack_t *stack, int sz,
|
|
arch_cpustart_t fn, void *arg)
|
|
{
|
|
__ASSERT_NO_MSG(!soc_cpus_active[cpu_num]);
|
|
|
|
start_rec.cpu = cpu_num;
|
|
start_rec.fn = fn;
|
|
start_rec.arg = arg;
|
|
|
|
z_mp_stack_top = K_KERNEL_STACK_BUFFER(stack) + sz;
|
|
|
|
soc_start_core(cpu_num);
|
|
}
|
|
|
|
/* Fallback stub for external SOF code */
|
|
__imr int cavs_idc_smp_init(const struct device *dev)
|
|
{
|
|
ARG_UNUSED(dev);
|
|
return 0;
|
|
}
|