Loading...
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 | /* * Copyright (c) 2019 Intel Corporation * * SPDX-License-Identifier: Apache-2.0 */ #include <device.h> #include <init.h> #include <kernel.h> #include <kernel_structs.h> #include <toolchain.h> #include <sys/__assert.h> #include <sys/sys_io.h> #include <xtensa/config/core-isa.h> #include <logging/log.h> LOG_MODULE_REGISTER(soc_mp, CONFIG_SOC_LOG_LEVEL); #include <soc.h> #include <adsp/io.h> #include <soc/shim.h> #include <drivers/ipm.h> #include <ipm/ipm_cavs_idc.h> #if CONFIG_MP_NUM_CPUS > 1 && !defined(CONFIG_IPM_CAVS_IDC) && defined(CONFIG_SMP) #error Need to enable the IPM driver for multiprocessing #endif /* ROM wake version parsed by ROM during core wake up. */ #define IDC_ROM_WAKE_VERSION 0x2 /* IDC message type. */ #define IDC_TYPE_SHIFT 24 #define IDC_TYPE_MASK 0x7f #define IDC_TYPE(x) (((x) & IDC_TYPE_MASK) << IDC_TYPE_SHIFT) /* IDC message header. */ #define IDC_HEADER_MASK 0xffffff #define IDC_HEADER(x) ((x) & IDC_HEADER_MASK) /* IDC message extension. */ #define IDC_EXTENSION_MASK 0x3fffffff #define IDC_EXTENSION(x) ((x) & IDC_EXTENSION_MASK) /* IDC power up message. */ #define IDC_MSG_POWER_UP \ (IDC_TYPE(0x1) | IDC_HEADER(IDC_ROM_WAKE_VERSION)) #define IDC_MSG_POWER_UP_EXT(x) IDC_EXTENSION((x) >> 2) #ifdef CONFIG_IPM_CAVS_IDC static const struct device *idc; #endif extern void __start(void); struct cpustart_rec { uint32_t cpu; arch_cpustart_t fn; char *stack_top; void *arg; uint32_t vecbase; uint32_t alive; /* padding to cache line */ uint8_t padding[XCHAL_DCACHE_LINESIZE - 6 * 4]; }; static __aligned(XCHAL_DCACHE_LINESIZE) struct cpustart_rec start_rec; static void *mp_top; static void mp_entry2(void) { volatile int ie; uint32_t idc_reg; /* We don't know what the boot ROM might have touched and we * don't care. Make sure it's not in our local cache. */ xthal_dcache_all_writeback_inv(); /* Copy over VECBASE from the main CPU for an initial value * (will need to revisit this if we ever allow a user API to * change interrupt vectors at runtime). */ ie = 0; __asm__ volatile("wsr.INTENABLE %0" : : "r"(ie)); __asm__ volatile("wsr.VECBASE %0" : : "r"(start_rec.vecbase)); __asm__ volatile("rsync"); /* Set up the CPU pointer. */ _cpu_t *cpu = &_kernel.cpus[start_rec.cpu]; __asm__ volatile( "wsr." CONFIG_XTENSA_KERNEL_CPU_PTR_SR " %0" : : "r"(cpu)); /* Clear busy bit set by power up message */ idc_reg = idc_read(IPC_IDCTFC(0), start_rec.cpu) | IPC_IDCTFC_BUSY; idc_write(IPC_IDCTFC(0), start_rec.cpu, idc_reg); #ifdef CONFIG_IPM_CAVS_IDC /* Interrupt must be enabled while running on current core */ irq_enable(XTENSA_IRQ_NUMBER(DT_IRQN(DT_INST(0, intel_cavs_idc)))); #endif /* CONFIG_IPM_CAVS_IDC */ start_rec.alive = 1; SOC_DCACHE_FLUSH(&start_rec, sizeof(start_rec)); start_rec.fn(start_rec.arg); #if CONFIG_MP_NUM_CPUS == 1 /* CPU#1 can be under manual control running custom functions * instead of participating in general thread execution. * Put the CPU into idle after those functions return * so this won't return. */ for (;;) { k_cpu_idle(); } #endif } /* Defines a locally callable "function" named mp_stack_switch(). The * first argument (in register a2 post-ENTRY) is the new stack pointer * to go into register a1. The second (a3) is the entry point. * Because this never returns, a0 is used as a scratch register then * set to zero for the called function (a null return value is the * signal for "top of stack" to the debugger). */ void mp_stack_switch(void *stack, void *entry); __asm__("\n" ".align 4 \n" "mp_stack_switch: \n\t" "entry a1, 16 \n\t" "movi a0, 0 \n\t" "jx a3 \n\t"); /* Carefully constructed to use no stack beyond compiler-generated ABI * instructions. Stack pointer is pointing to __stack at this point. */ void z_mp_entry(void) { *(uint32_t *)CONFIG_SRAM_BASE_ADDRESS = 0xDEADBEEF; SOC_DCACHE_FLUSH((uint32_t *)CONFIG_SRAM_BASE_ADDRESS, 64); mp_stack_switch(mp_top, mp_entry2); } void arch_start_cpu(int cpu_num, k_thread_stack_t *stack, int sz, arch_cpustart_t fn, void *arg) { uint32_t vecbase; uint32_t idc_reg; __ASSERT(cpu_num == 1, "Only supports only two CPUs!"); /* Setup data to boot core #1 */ __asm__ volatile("rsr.VECBASE %0\n\t" : "=r"(vecbase)); start_rec.cpu = cpu_num; start_rec.fn = fn; start_rec.stack_top = Z_THREAD_STACK_BUFFER(stack) + sz; start_rec.arg = arg; start_rec.vecbase = vecbase; start_rec.alive = 0; mp_top = Z_THREAD_STACK_BUFFER(stack) + sz; SOC_DCACHE_FLUSH(&start_rec, sizeof(start_rec)); #ifdef CONFIG_IPM_CAVS_IDC idc = device_get_binding(DT_LABEL(DT_INST(0, intel_cavs_idc))); #endif /* Enable IDC interrupt on the other core */ idc_reg = idc_read(IPC_IDCCTL, cpu_num); idc_reg |= IPC_IDCCTL_IDCTBIE(0); idc_write(IPC_IDCCTL, cpu_num, idc_reg); sys_set_bit(DT_REG_ADDR(DT_NODELABEL(cavs0)) + 0x04 + CAVS_ICTL_INT_CPU_OFFSET(cpu_num), 8); /* Send power up message to the other core */ idc_write(IPC_IDCIETC(cpu_num), 0, IDC_MSG_POWER_UP_EXT(RAM_BASE)); idc_write(IPC_IDCITC(cpu_num), 0, IDC_MSG_POWER_UP | IPC_IDCITC_BUSY); /* Disable IDC interrupt on other core so IPI won't cause * them to jump to ISR until the core is fully initialized. */ idc_reg = idc_read(IPC_IDCCTL, cpu_num); idc_reg &= ~IPC_IDCCTL_IDCTBIE(0); idc_write(IPC_IDCCTL, cpu_num, idc_reg); sys_clear_bit(DT_REG_ADDR(DT_NODELABEL(cavs0)) + 0x04 + CAVS_ICTL_INT_CPU_OFFSET(cpu_num), 8); do { SOC_DCACHE_INVALIDATE(&start_rec, sizeof(start_rec)); } while (start_rec.alive == 0); /* Clear done bit from responding the power up message */ idc_reg = idc_read(IPC_IDCIETC(cpu_num), 0) | IPC_IDCIETC_DONE; idc_write(IPC_IDCIETC(cpu_num), 0, idc_reg); } #ifdef CONFIG_SCHED_IPI_SUPPORTED FUNC_ALIAS(soc_sched_ipi, arch_sched_ipi, void); void soc_sched_ipi(void) { if (idc != NULL) { ipm_send(idc, 0, IPM_CAVS_IDC_MSG_SCHED_IPI_ID, IPM_CAVS_IDC_MSG_SCHED_IPI_DATA, 0); } } #endif |