[arch][arm64] SMP support

Change-Id: Ieb9dec2ad64b9b04d51da15a17b9e7c4df50460b
2014-12-04 16:17:37 -08:00
parent d3eb95aaf1
commit 47e5b7101f
4 changed files with 209 additions and 4 deletions
--- a/arch/arm64/arch.c
+++ b/arch/arm64/arch.c
@@ -25,9 +25,22 @@
 #include <arch/ops.h>
 #include <arch/arm64.h>
 #include <arch/arm64/mmu.h>
+#include <arch/mp.h>
+#include <kernel/thread.h>
+#include <lk/init.h>
+#include <lk/main.h>
 #include <platform.h>
+#include <trace.h>

-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

--- a/arch/arm64/mp.c
+++ 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 <arch/mp.h>
+
+#include <assert.h>
+#include <trace.h>
+#include <err.h>
+#include <platform/interrupts.h>
+#include <arch/ops.h>
+
+#if WITH_DEV_INTERRUPT_ARM_GIC
+#include <dev/interrupt/arm_gic.h>
+#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);
+}
+
--- 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

--- 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