diff --git a/arch/arm64/arch.c b/arch/arm64/arch.c index f468a62c..56478e08 100644 --- a/arch/arm64/arch.c +++ b/arch/arm64/arch.c @@ -25,9 +25,22 @@ #include #include #include +#include +#include +#include +#include #include +#include -void arch_early_init(void) +#define LOCAL_TRACE 0 + +#if WITH_SMP +/* smp boot lock */ +static spin_lock_t arm_boot_cpu_lock = 1; +static volatile int secondaries_to_init = 0; +#endif + +static void arm64_cpu_early_init(void) { /* set the vector base */ ARM64_WRITE_SYSREG(VBAR_EL1, (uint64_t)&arm64_exception_base); @@ -37,12 +50,38 @@ void arch_early_init(void) if (current_el > 1) { arm64_el3_to_el1(); } +} +void arch_early_init(void) +{ + arm64_cpu_early_init(); platform_init_mmu_mappings(); } void arch_init(void) { + arch_mp_init_percpu(); + +#if WITH_SMP + LTRACEF("midr_el1 0x%llx\n", ARM64_READ_SYSREG(midr_el1)); + + secondaries_to_init = SMP_MAX_CPUS - 1; /* TODO: get count from somewhere else, or add cpus as they boot */ + + lk_init_secondary_cpus(secondaries_to_init); + + LTRACEF("releasing %d secondary cpus\n", secondaries_to_init); + + /* release the secondary cpus */ + spin_unlock(&arm_boot_cpu_lock); + + /* flush the release of the lock, since the secondary cpus are running without cache on */ + arch_clean_cache_range((addr_t)&arm_boot_cpu_lock, sizeof(arm_boot_cpu_lock)); + + /* wait for all of the secondary cpus to boot */ + while (secondaries_to_init > 0) { + __asm__ volatile("wfe"); + } +#endif } void arch_quiesce(void) @@ -59,4 +98,26 @@ void arch_chain_load(void *entry, ulong arg0, ulong arg1, ulong arg2, ulong arg3 PANIC_UNIMPLEMENTED; } +#if WITH_SMP +void arm64_secondary_entry(void) +{ + arm64_cpu_early_init(); + + spin_lock(&arm_boot_cpu_lock); + spin_unlock(&arm_boot_cpu_lock); + + /* run early secondary cpu init routines up to the threading level */ + lk_init_level(LK_INIT_FLAG_SECONDARY_CPUS, LK_INIT_LEVEL_EARLIEST, LK_INIT_LEVEL_THREADING - 1); + + arch_mp_init_percpu(); + + LTRACEF("cpu num %d\n", arch_curr_cpu_num()); + + /* we're done, tell the main cpu we're up */ + atomic_add(&secondaries_to_init, -1); + __asm__ volatile("sev"); + + lk_secondary_cpu_entry(); +} +#endif diff --git a/arch/arm64/mp.c b/arch/arm64/mp.c new file mode 100644 index 00000000..0760ed32 --- /dev/null +++ b/arch/arm64/mp.c @@ -0,0 +1,81 @@ +/* + * Copyright (c) 2014 Travis Geiselbrecht + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files + * (the "Software"), to deal in the Software without restriction, + * including without limitation the rights to use, copy, modify, merge, + * publish, distribute, sublicense, and/or sell copies of the Software, + * and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. + * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY + * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, + * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE + * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ +#include + +#include +#include +#include +#include +#include + +#if WITH_DEV_INTERRUPT_ARM_GIC +#include +#else +#error need other implementation of interrupt controller that can ipi +#endif + +#define LOCAL_TRACE 0 + +#define GIC_IPI_BASE (14) + +status_t arch_mp_send_ipi(mp_cpu_mask_t target, mp_ipi_t ipi) +{ + LTRACEF("target 0x%x, ipi %u\n", target, ipi); + +#if WITH_DEV_INTERRUPT_ARM_GIC + uint gic_ipi_num = ipi + GIC_IPI_BASE; + + /* filter out targets outside of the range of cpus we care about */ + target &= ((1UL << SMP_MAX_CPUS) - 1); + if (target != 0) { + LTRACEF("target 0x%x, gic_ipi %u\n", target, gic_ipi_num); + arm_gic_sgi(gic_ipi_num, ARM_GIC_SGI_FLAG_NS, target); + } +#endif + + return NO_ERROR; +} + +enum handler_return arm_ipi_generic_handler(void *arg) +{ + LTRACEF("cpu %u, arg %p\n", arch_curr_cpu_num(), arg); + + return INT_NO_RESCHEDULE; +} + +enum handler_return arm_ipi_reschedule_handler(void *arg) +{ + LTRACEF("cpu %u, arg %p\n", arch_curr_cpu_num(), arg); + + return mp_mbx_reschedule_irq(); +} + +void arch_mp_init_percpu(void) +{ + register_int_handler(MP_IPI_GENERIC + GIC_IPI_BASE, &arm_ipi_generic_handler, 0); + register_int_handler(MP_IPI_RESCHEDULE + GIC_IPI_BASE, &arm_ipi_reschedule_handler, 0); + + //unmask_interrupt(MP_IPI_GENERIC); + //unmask_interrupt(MP_IPI_RESCHEDULE); +} + diff --git a/arch/arm64/rules.mk b/arch/arm64/rules.mk index 2399c854..273511d6 100644 --- a/arch/arm64/rules.mk +++ b/arch/arm64/rules.mk @@ -27,8 +27,22 @@ MODULE_SRCS += \ $(LOCAL_DIR)/arm/dcc.S GLOBAL_DEFINES += \ - ARCH_DEFAULT_STACK_SIZE=8192 \ - SMP_MAX_CPUS=1 + ARCH_DEFAULT_STACK_SIZE=8192 + +# if its requested we build with SMP, arm generically supports 4 cpus +ifeq ($(WITH_SMP),1) +SMP_MAX_CPUS ?= 4 + +GLOBAL_DEFINES += \ + WITH_SMP=1 \ + SMP_MAX_CPUS=$(SMP_MAX_CPUS) + +MODULE_SRCS += \ + $(LOCAL_DIR)/mp.c +else +GLOBAL_DEFINES += \ + SMP_MAX_CPUS=1 +endif ARCH_OPTFLAGS := -O2 diff --git a/arch/arm64/start.S b/arch/arm64/start.S index 517031af..3e64781d 100644 --- a/arch/arm64/start.S +++ b/arch/arm64/start.S @@ -17,6 +17,7 @@ page_table .req x13 new_page_table .req x14 phys_offset .req x15 +cpuid .req x19 page_table0 .req x20 page_table1 .req x21 mmu_initial_mapping .req x22 @@ -46,6 +47,12 @@ FUNCTION(_start) adrp page_table0, tt_trampoline add page_table0, page_table0, #:lo12:tt_trampoline +#if WITH_SMP + mrs cpuid, mpidr_el1 + bic cpuid, cpuid, #0xff000000 + cbnz cpuid, .Lmmu_enable_secondary +#endif + mov tmp, #0 /* walk through all the entries in the translation table, setting them up */ @@ -197,6 +204,19 @@ FUNCTION(_start) str tmp2, [page_table0, tmp, lsl #3] /* tt_trampoline[paddr index] = pt entry */ +#if WITH_SMP + adr tmp, page_tables_not_ready + str wzr, [tmp] + b .Lpage_tables_ready + +.Lmmu_enable_secondary: + adr tmp, page_tables_not_ready +.Lpage_tables_not_ready: + ldr tmp2, [tmp] + cbnz tmp2, .Lpage_tables_not_ready +.Lpage_tables_ready: +#endif + /* set up the mmu */ /* Invalidate TLB */ @@ -248,6 +268,9 @@ FUNCTION(_start) tlbi vmalle1 isb +#if WITH_SMP + cbnz cpuid, .Lsecondary_boot +#endif #endif /* WITH_KERNEL_VM */ ldr tmp, =__stack_end @@ -270,12 +293,38 @@ FUNCTION(_start) bl lk_main b . +#if WITH_SMP +.Lsecondary_boot: + cmp cpuid, #SMP_MAX_CPUS + bge .Lunsupported_cpu_trap + + /* Set up the stack */ + ldr tmp, =__stack_end + mov tmp2, #ARCH_DEFAULT_STACK_SIZE + mul tmp2, tmp2, cpuid + sub sp, tmp, tmp2 + + bl arm64_secondary_entry + +.Lunsupported_cpu_trap: + wfe + b .Lunsupported_cpu_trap +#endif + .ltorg +#if WITH_SMP +.data +DATA(page_tables_not_ready) + .long 1 +DATA(secondary_cpu_allocated_stack) + .quad 0 +#endif + .section .bss.prebss.stack .align 4 DATA(__stack) - .skip 0x2000 + .skip ARCH_DEFAULT_STACK_SIZE * SMP_MAX_CPUS DATA(__stack_end) #if WITH_KERNEL_VM