[Minios-devel] [UNIKRAFT early RFC PATCH 08/11] plat/kvm/arm: Implement smp boot on arm64 kvm plat

[Jia He <justin.he@xxxxxxx>]

Signed-off-by: Jia He <justin.he@xxxxxxx>
---
 lib/ukboot/Makefile.uk |   1 +
 lib/ukboot/boot.c      |  15 ++++
 plat/kvm/arm/setup.c   | 188 +++++++++++++++++++++++++++++++++++++++++
 3 files changed, 204 insertions(+)

diff --git a/lib/ukboot/Makefile.uk b/lib/ukboot/Makefile.uk
index 55f205d..c8e0b44 100644
--- a/lib/ukboot/Makefile.uk
+++ b/lib/ukboot/Makefile.uk
@@ -1,6 +1,7 @@
 $(eval $(call addlib_s,libukboot,$(CONFIG_LIBUKBOOT)))
 
 CINCLUDES-$(CONFIG_LIBUKBOOT)          += -I$(LIBUKBOOT_BASE)/include
+CINCLUDES-$(CONFIG_LIBUKBOOT)          += -I$(UK_PLAT_COMMON_BASE)/include
 CXXINCLUDES-$(CONFIG_LIBUKBOOT)        += -I$(LIBUKBOOT_BASE)/include
 
 LIBUKBOOT_SRCS-y += $(LIBUKBOOT_BASE)/boot.c
diff --git a/lib/ukboot/boot.c b/lib/ukboot/boot.c
index 4846782..d4d49c1 100644
--- a/lib/ukboot/boot.c
+++ b/lib/ukboot/boot.c
@@ -41,6 +41,10 @@
 #include <stdio.h>
 #include <errno.h>
 
+#if CONFIG_SMP
+#include <smp.h>
+#endif
+
 #if CONFIG_LIBUKALLOC && CONFIG_LIBUKALLOCBBUDDY && CONFIG_LIBUKBOOT_INITALLOC
 #include <uk/allocbbuddy.h>
 #endif
@@ -255,6 +259,17 @@ void ukplat_entry(int argc, char *argv[])
        tma.argc = argc;
        tma.argv = argv;
 
+#if CONFIG_SMP
+       uk_pr_info("before start cpu\n");
+
+       for (i = 1; i < MAXCPU; i++) {
+               if (cpu_possible_map[i] != -1)
+                       start_cpu(cpu_possible_map[i]);
+       }
+
+       release_aps();
+#endif
+
 #if CONFIG_LIBUKSCHED
        main_thread = uk_thread_create("main", main_thread_func, &tma);
        if (unlikely(!main_thread))
diff --git a/plat/kvm/arm/setup.c b/plat/kvm/arm/setup.c
index 1f6e458..4035202 100644
--- a/plat/kvm/arm/setup.c
+++ b/plat/kvm/arm/setup.c
@@ -27,6 +27,29 @@
 #include <arm/cpu.h>
 #include <uk/arch/limits.h>
 
+#include <stdbool.h>
+#include <uk/plat/bootstrap.h>
+#include <uk/sched.h>
+
+#include <smp.h>
+#include <uk/plat/memory.h>
+#include <uk/plat/io.h>
+#include <uk/plat/lcpu.h>
+#include <arm/psci.h>
+#include <ofw/fdt.h>
+#include <time.h>
+
+#ifdef CONFIG_SMP
+int aps_ready;
+int smp_started;
+int mp_ncpus;
+int smp_cpus = 1;      /* how many cpu's running */
+void mpentry(void);
+uint8_t secondary_stacks[MAXCPU - 1][__PAGE_SIZE * 4];
+int cpu_possible_map[MAXCPU];
+static int cpu0 = -1;
+#endif
+
 void *_libkvmplat_pagetable;
 void *_libkvmplat_heap_start;
 void *_libkvmplat_stack_top;
@@ -191,6 +214,160 @@ static void _libkvmplat_entry2(void *arg 
__attribute__((unused)))
        ukplat_entry_argp(NULL, (char *)cmdline, strlen(cmdline));
 }
 
+static void _init_dtb_cpu(void)
+{
+       int fdt_cpu;
+       int naddr, nsize;
+       uint64_t core_id, index;
+       int subnode;
+       int i;
+
+       /* Init the cpu_possible_map */
+       for (i = 0; i < MAXCPU; i++)
+               cpu_possible_map[i] = -1;
+
+       /* Search for assigned VM cpus in DTB */
+       fdt_cpu = fdt_path_offset(_libkvmplat_dtb, "/cpus");
+       if (fdt_cpu < 0)
+               uk_pr_warn("cpus node is not found in device tree\n");
+
+       /* Get address,size cell */
+       naddr = fdt_address_cells(_libkvmplat_dtb, fdt_cpu);
+       if (naddr < 0 || naddr >= FDT_MAX_NCELLS) {
+               UK_CRASH("Could not find cpu address!\n");
+               return;
+       }
+       nsize = fdt_size_cells(_libkvmplat_dtb, fdt_cpu);
+       if (nsize < 0 || nsize >= FDT_MAX_NCELLS) {
+               UK_CRASH("Could not find cpu size!\n");
+               return;
+       }
+
+       /* Search all the cpu nodes in DTB */
+       index = 0;
+       fdt_for_each_subnode(subnode, _libkvmplat_dtb, fdt_cpu) {
+               const struct fdt_property *prop;
+               int prop_len = 0;
+
+               index++;
+
+               prop = fdt_get_property(_libkvmplat_dtb, subnode,
+                                               "enable-method", NULL);
+               if (!prop || strcmp(prop->data, "psci")) {
+                       uk_pr_err("Only support psci method!(%s)\n",
+                                       prop->data);
+                       return;
+               }
+
+               prop = fdt_get_property(_libkvmplat_dtb, subnode,
+                                               "device_type", &prop_len);
+               if (!prop)
+                       continue;
+               if (prop_len < 4)
+                       continue;
+               if (strcmp(prop->data, "cpu"))
+                       continue;
+
+               prop = fdt_get_property(_libkvmplat_dtb, subnode,
+                                               "reg", &prop_len);
+               if (prop == NULL || prop_len <= 0) {
+                       uk_pr_err("Error when searching reg property\n");
+                       return;
+               }
+
+               core_id = fdt_reg_read_number((const fdt32_t *)prop->data,
+                                               naddr);
+               cpu_possible_map[index-1] = core_id;
+               mp_ncpus++;
+       }
+}
+
+void release_aps(void)
+{
+       int i, started;
+
+       /* Only release CPUs if they exist */
+       if (mp_ncpus == 1)
+               return;
+
+       //TODO: make aps_ready atomic
+       aps_ready = 1;
+
+       /* Wake up the other CPUs */
+       __asm __volatile(
+               "dsb ishst      \n"
+               "sev            \n"
+               ::: "memory");
+
+       uk_pr_info("Release APs...");
+
+       started = 0;
+       for (i = 0; i < 20000; i++) {
+               if (smp_started) {
+                       uk_pr_info("done\n");
+                       return;
+               }
+               /*
+                * Don't time out while we are making progress. Some large
+                * systems can take a while to start all CPUs.
+                */
+               if (smp_cpus > started) {
+                       i = 0;
+                       started = smp_cpus;
+               }
+
+               uk_pr_info("sleep for a while\n");
+               mdelay(1);
+       }
+
+       uk_pr_err("APs not started\n");
+}
+
+void init_secondary(uint64_t cpu)
+{
+       struct uk_sched *s = NULL;
+       struct uk_alloc *a = NULL;
+
+       uk_pr_info("init secondary cpu=%lu\n", cpu);
+
+       /* Spin until the BSP releases the APs */
+       while (!aps_ready)
+               __asm __volatile("wfe");
+       uk_pr_info("after wfe cpu=%lu\n", cpu);
+
+       smp_cpus += 1;
+
+       if (smp_cpus == mp_ncpus)
+               smp_started = 1;
+}
+
+void start_cpu(uint64_t target_cpu)
+{
+
+       uint32_t pa;
+       int err;
+
+       /* Check we are able to start this cpu */
+       UK_ASSERT(target_cpu < MAXCPU);
+
+       uk_pr_info("Starting CPU %lu\n", target_cpu);
+
+       /* We are already running on cpu 0 */
+       if (target_cpu == (uint64_t)cpu0)
+               return;
+
+       pa = ukplat_virt_to_phys(mpentry);
+       err = psci_cpu_on(target_cpu, pa);
+       if (err != PSCI_RET_SUCCESS) {
+               mp_ncpus--;
+
+               /* Notify the user that the CPU failed to start */
+               uk_pr_info("Failed to start CPU (%lx)\n", target_cpu);
+       }
+
+       uk_pr_info("Starting CPU %lu successfully\n", target_cpu);
+}
+
 void _libkvmplat_start(void *dtb_pointer)
 {
        _init_dtb(dtb_pointer);
@@ -214,6 +391,17 @@ void _libkvmplat_start(void *dtb_pointer)
        uk_pr_info("     heap start: %p\n", _libkvmplat_heap_start);
        uk_pr_info("      stack top: %p\n", _libkvmplat_stack_top);
 
+       _init_dtb_cpu();
+
+       if (cpu0 < 0) {
+               uint64_t mpidr_reg = SYSREG_READ32(mpidr_el1);
+
+               uk_pr_info("get mpidr_el1 0x%lx\n", mpidr_reg);
+
+               if ((mpidr_reg & 0xff00fffffful) == 0)
+                       cpu0 = 0;
+       }
+
        /*
         * Switch away from the bootstrap stack as early as possible.
         */
-- 
2.17.1


_______________________________________________
Minios-devel mailing list
Minios-devel@xxxxxxxxxxxxxxxxxxxx
https://lists.xenproject.org/mailman/listinfo/minios-devel