diff --git a/arch/arm/core/aarch64/cpu_smp.c b/arch/arm/core/aarch64/cpu_smp.c index 0016d25ce240fc98e040308ecda31308152f134b..006cb42231119590cc29da27d13609ec3bc3dcc2 100644 --- a/arch/arm/core/aarch64/cpu_smp.c +++ b/arch/arm/core/aarch64/cpu_smp.c @@ -15,7 +15,6 @@ static ALWAYS_INLINE void __SEV(void) } static void sys_write64(uint64_t val, uint64_t *addr) { - printk("Info: *%p = %d (write %d)\n", addr, (int) *addr, (int) val); *addr = val; } @@ -34,8 +33,6 @@ void raspi3b_smp_init_top_wrapper(void); void arch_start_cpu(int cpu_num, k_thread_stack_t *stack, int sz, arch_cpustart_t fn, void *arg) { - printk("%s\n", __func__); - /* https://github.com/AlMazyr/raspberry-pi-os/blob/dd0375d84f2b96a200fcbb8085d298a5c3b9c1b8/src/lesson01/src/boot.S#L18 */ raspi3b_smp_init_top_stack[cpu_num] = (uint64_t) stack; raspi3b_smp_init_top_sz[cpu_num] = (uint64_t) sz; diff --git a/kernel/smp.c b/kernel/smp.c index 7ca46e2b0befed18ce3fca04729feceece377cca..f2ac83b44899cc8bb0a8fa89330b845ce9fb779a 100644 --- a/kernel/smp.c +++ b/kernel/smp.c @@ -66,12 +66,12 @@ static FUNC_NORETURN void smp_init_top(void *arg) atomic_t *cpu_start_flag = arg; struct k_thread dummy_thread; - printk("%s\n", __func__); - /* Wait for the signal to begin scheduling */ while (!atomic_get(cpu_start_flag)) { } + printk("%s\n", __func__); + z_dummy_thread_init(&dummy_thread); smp_timer_init(); z_swap_unlocked(); diff --git a/soc/arm/qemu_raspi3/intc_bcm2837.c b/soc/arm/qemu_raspi3/intc_bcm2837.c index db40de393f553f53c6614904f358e43b6ab3d1dd..e6ccbfe487d919b766889ce0f33ddc36927d9cf0 100644 --- a/soc/arm/qemu_raspi3/intc_bcm2837.c +++ b/soc/arm/qemu_raspi3/intc_bcm2837.c @@ -94,6 +94,27 @@ static void armctrl_irq_disable(unsigned int irq) { irq_enabled[irq] = false; } +/* BCM2836 ARM-local peripherals manual, section 4.10 "Core interrupt sources" */ + +static void l1_irq_enable(unsigned int l1_irq) { + __ASSERT_NO_MSG(l1_irq < L1_NUM_IRQS); + + if (0 <= l1_irq && l1_irq <= 3) { + /* Core timer interrupts: CNT_PS_IRQ = 0, CNT_PNS_IRQ = 1, CNT_HP_IRQ = 2, CNT_V_IRQ = 3 */ + int cpu_id = arch_curr_cpu()->id; + intptr_t core_timer_interrupt_control_addr = 0x40000040 + cpu_id * 4; + uint32_t val = sys_read32(core_timer_interrupt_control_addr); + val |= (1 << l1_irq); + sys_write32(val, core_timer_interrupt_control_addr); + } else { + printk("Warning: l1_irq_enable(%d) not implemented.\n", l1_irq); + } +} + +static void l1_irq_disable(unsigned int l1_irq) { + __ASSERT_NO_MSG(l1_irq < L1_NUM_IRQS); +} + void z_soc_irq_init(void) { printk("z_soc_irq_init\n"); } @@ -107,22 +128,18 @@ int z_soc_irq_is_enabled(unsigned int irq) { } void z_soc_irq_enable(unsigned int irq) { - /* TODO: Map "4.10 Core interrupt sources" to irqs 72+ and handle those - * using l1_irq_enable|disable (by writing into the core timer control - * and mailbox interrupt control registers). */ if (irq < ARMCTRL_NUM_IRQS) { armctrl_irq_enable(irq); } else { - printk("BUG: z_soc_irq_enable(%d) not implemented\n", irq); + l1_irq_enable(irq - ARMCTRL_NUM_IRQS); } } void z_soc_irq_disable(unsigned int irq) { - /* TODO: Same as enable. */ if (irq < ARMCTRL_NUM_IRQS) { armctrl_irq_disable(irq); } else { - printk("BUG: z_soc_irq_disable(%d) not implemented\n", irq); + l1_irq_disable(irq - ARMCTRL_NUM_IRQS); } } @@ -165,6 +182,9 @@ unsigned int z_soc_irq_get_active(void) { /* TODO: Check if it was a core timer / mailbox interrupt by checking * the bit in the "core interrupt soruce" register of the current * core. */ + int cpu_id = arch_curr_cpu()->id; + intptr_t core_interrupt_sources_addr = 0x40000060 + cpu_id * 4; + printk("*%p == %x\n", (void *) core_interrupt_sources_addr, sys_read32(core_interrupt_sources_addr)); __ASSERT_NO_MSG(false); CODE_UNREACHABLE;