From e16f5f81dfb2e97a554ec19b941ec05a7942fd2d Mon Sep 17 00:00:00 2001 From: Kimplul Date: Fri, 4 Aug 2023 20:50:41 +0300 Subject: add initial smp bringup --- arch/riscv64/kernel/arch.c | 43 +++++++++++++ arch/riscv64/kernel/arch.h | 48 ++++++++++++++ arch/riscv64/kernel/asm.h | 30 +++++++++ arch/riscv64/kernel/core_bringup.S | 23 +++++++ arch/riscv64/kernel/cpu.c | 14 ++-- arch/riscv64/kernel/entry.S | 9 +-- arch/riscv64/kernel/sbi.h | 29 +++++++++ arch/riscv64/kernel/smp.c | 129 +++++++++++++++++++++++++++++++++++++ arch/riscv64/kernel/vmem.c | 34 ++++++---- 9 files changed, 329 insertions(+), 30 deletions(-) create mode 100644 arch/riscv64/kernel/arch.c create mode 100644 arch/riscv64/kernel/arch.h create mode 100644 arch/riscv64/kernel/asm.h create mode 100644 arch/riscv64/kernel/core_bringup.S create mode 100644 arch/riscv64/kernel/smp.c (limited to 'arch/riscv64/kernel') diff --git a/arch/riscv64/kernel/arch.c b/arch/riscv64/kernel/arch.c new file mode 100644 index 0000000..c4009e7 --- /dev/null +++ b/arch/riscv64/kernel/arch.c @@ -0,0 +1,43 @@ +/* SPDX-License-Identifier: copyleft-next-0.3.1 */ +/* Copyright 2023, Kim Kuparinen < kimi.h.kuparinen@gmail.com > */ + +/** + * @file arch.c + * Misc riscv specific stuff implementations. + */ + +#include +#include +#include "csr.h" +#include "arch.h" + +id_t __cpuid_to_hartid[MAX_CPUS]; + +id_t hartid_to_cpuid(id_t hart) +{ + for (size_t i = 0; i < MAX_CPUS; ++i) + if (cpuid_to_hartid(i) == hart) + return i; + + error("failed to match hart id %d to cpu id\n", hart); + /* default to zero, though this should maybe be a panic? */ + return 0; +} + +pm_t branch_to_satp(struct vmem *branch, enum mm_mode mode) +{ + /* Sv57 && Sv64 in the future? */ + branch = (struct vmem *)__pa(branch); + pm_t pn = (pm_t)(branch) >> page_shift(); + + pm_t m = DEFAULT_Sv_MODE; + + switch (mode) { + case Sv32: m = SATP_MODE_Sv32; break; + case Sv39: m = SATP_MODE_Sv39; break; + case Sv48: m = SATP_MODE_Sv48; break; + default: bug("unknown satp mode\n"); break; + } + + return m | pn; +} diff --git a/arch/riscv64/kernel/arch.h b/arch/riscv64/kernel/arch.h new file mode 100644 index 0000000..d38dad0 --- /dev/null +++ b/arch/riscv64/kernel/arch.h @@ -0,0 +1,48 @@ +/* SPDX-License-Identifier: copyleft-next-0.3.1 */ +/* Copyright 2023, Kim Kuparinen < kimi.h.kuparinen@gmail.com > */ + +/** + * @file arch.h + * + * Random tools common to riscv. + */ + +#ifndef KMI_RISCV_ARCH_H +#define KMI_RISCV_ARCH_H + +#include + +/** Actual map of cpu id to hart id. */ +extern id_t __cpuid_to_hartid[MAX_CPUS]; + +/** + * Map cpu id to hart id. + * Defined as a macro to allow for stuff like + * @code + * cpuid_to_hartid(0) = 20; + * @endcode + * + * @param x Hart ID. + */ +#define cpuid_to_hartid(x) __cpuid_to_hartid[x] + +/** + * Find the cpu id that corresponds to hart id. + * + * @param hart Hart to find corresponding cpu id for. + * @return Corresponding cpu id. 0 if not found, though this should maybe be a + * panic situation. + */ +id_t hartid_to_cpuid(id_t hart); + +/** + * Format branch and mode information into something that can be written to + * SATP. + * + * @param branch Top branch of virtual memory. + * @param mode Mode to use. Sv32/Sv39/etc. + * @return Corresponding SATP value. + */ +pm_t branch_to_satp(struct vmem *branch, enum mm_mode mode); + +#endif /* KMI_RISCV_ARCH_H */ diff --git a/arch/riscv64/kernel/asm.h b/arch/riscv64/kernel/asm.h new file mode 100644 index 0000000..f8ad202 --- /dev/null +++ b/arch/riscv64/kernel/asm.h @@ -0,0 +1,30 @@ +/* SPDX-License-Identifier: copyleft-next-0.3.1 */ +/* Copyright 2023, Kim Kuparinen < kimi.h.kuparinen@gmail.com > */ + +/** + * @file asm.h + * Stuff to make assembly easier to write on both riscv64 and riscv32. + */ + +#if __riscv_xlen == 64 + +/** Store register. */ +#define sr sd + +/** Load register. */ +#define lr ld + +/** Shift count to get a register sized value. */ +#define RW_SHIFT 3 + +#else + +/** Store register. */ +#define sr sw + +/** Load register. */ +#define lr lw + +/** Shift count to get register sized value. */ +#define RW_SHIFT 2 +#endif diff --git a/arch/riscv64/kernel/core_bringup.S b/arch/riscv64/kernel/core_bringup.S new file mode 100644 index 0000000..aff9b0b --- /dev/null +++ b/arch/riscv64/kernel/core_bringup.S @@ -0,0 +1,23 @@ +#include "asm.h" + +/* pic forces core_bringup to be placed in .got that we can access both from + * direct mapped and virtual memory. Which is kind of strange, as core_bringup() + * is fixed in place by the VM, so it could be possible to just load the address + * directly into a register, but apparently that's annoying to do */ +.option pic +.global riscv_bringup +riscv_bringup: + /* immediately switch to virtual memory through argument in a1 */ + csrw satp, a1 + sfence.vma + + /* fetch the stack allocated to us at our hart index in smp_init_stacks */ + la t0, smp_init_stacks + slli t1, a0, RW_SHIFT + add t0, t0, t1 + lr sp, 0(t0) + mv tp, sp + + /* jump to C to handle rest of bringup */ + la t0, core_bringup + jr t0 diff --git a/arch/riscv64/kernel/cpu.c b/arch/riscv64/kernel/cpu.c index dc87a13..0cb3d4c 100644 --- a/arch/riscv64/kernel/cpu.c +++ b/arch/riscv64/kernel/cpu.c @@ -13,18 +13,16 @@ #include "sbi.h" -/** Keeps track of initialized cpus. */ -static atomic_long cpus = 0; - void cpu_assign(struct tcb *t) { - /* bounce cpu id forward, or if first time here, get a cpu id */ + /* bounce cpu id forward */ struct tcb *c = cur_tcb(); - if (likely(c)) - t->cpu_id = c->cpu_id; - else - t->cpu_id = cpus++; + t->cpu_id = c->cpu_id; + __asm__ volatile ("mv tp, %0\n" : : "r" (t) :); +} +void tcb_assign(struct tcb *t) +{ __asm__ volatile ("mv tp, %0\n" : : "r" (t) :); } diff --git a/arch/riscv64/kernel/entry.S b/arch/riscv64/kernel/entry.S index 698dae7..2959e2f 100644 --- a/arch/riscv64/kernel/entry.S +++ b/arch/riscv64/kernel/entry.S @@ -3,14 +3,7 @@ #include "../kernel/gen/asm-offsets.h" #include "../kernel/csr.h" - -#if __riscv_xlen == 64 -#define sr sd -#define lr ld -#else -#define sr sw -#define lr lw -#endif +#include "asm.h" /* very much based on linux, but why change it if works, eh? */ .section .text diff --git a/arch/riscv64/kernel/sbi.h b/arch/riscv64/kernel/sbi.h index 8abff25..fe75c85 100644 --- a/arch/riscv64/kernel/sbi.h +++ b/arch/riscv64/kernel/sbi.h @@ -133,6 +133,14 @@ static inline struct sbiret sbi_send_ipi(unsigned long hart_mask, /** Hart start function ID. */ #define FID_HSM_START 0 +/** + * Start hart. + * + * @param hartid Hart to start. + * @param start_addr Address to jump to in S mode. + * @param opaque OS-specific argument passed to hart. + * @return SBI call return. \see sbiret. + */ static inline struct sbiret sbi_hart_start(unsigned long hartid, unsigned long start_addr, unsigned long opaque) @@ -141,4 +149,25 @@ static inline struct sbiret sbi_hart_start(unsigned long hartid, 0, 0, 0); } +/** Hart status function ID. */ +#define FID_HSM_STATUS 2 + +/** + * Get hart status. + * + * @param hartid Hart ID. + * @return SBI call return. \see sbiret. + */ +static inline struct sbiret sbi_hart_status(unsigned long hartid) +{ + return sbi_ecall(EID_HSM, FID_HSM_STATUS, hartid, + 0, 0, 0, 0, 0); +} + +/** + * Status code \ref sbi_hart_status() returns for a started hart. + * The only one we're currently interested in. + */ +#define SBI_HART_STARTED 0 + #endif /* KMI_RISCV_SBI_H */ diff --git a/arch/riscv64/kernel/smp.c b/arch/riscv64/kernel/smp.c new file mode 100644 index 0000000..88a796e --- /dev/null +++ b/arch/riscv64/kernel/smp.c @@ -0,0 +1,129 @@ +/* SPDX-License-Identifier: copyleft-next-0.3.1 */ +/* Copyright 2023, Kim Kuparinen < kimi.h.kuparinen@gmail.com > */ + +/** + * @file smp.c + * riscv multicore bringup implementation. + */ + +#include +#include +#include +#include +#include "arch.h" +#include "sbi.h" + +/** + * Array of stacks for bringing up harts. + * Note not static since we want to access it from core_bringup.S. + */ +void *smp_init_stacks[MAX_CPUS]; + +/** + * Check if FDT node is a cpu and that its status is 'okay'. + * Helper for smp_bringup(), cpu nodes that are marked 'okay' in the FDT should + * be brought up. + * + * @param fdt Flattened device tree. + * @param node Node to check. + * @return true if node is a cpu node and it should be brought up, false + * otherwise. + */ +static bool riscv_cpu_okay(void *fdt, int node) +{ + const char *s = fdt_getprop(fdt, node, "device_type", NULL); + if (!s || strcmp(s, "cpu") != 0) + return false; + + s = fdt_getprop(fdt, node, "status", NULL); + if (!s) + return false; + + if (strcmp(s, "okay") == 0) + return true; + + return false; +} + +/** Counter for how many cores system has. */ +static size_t cpus = 1; + +/* called from main to start other cores */ +void smp_bringup(struct vmem *b, void *fdt) +{ + /* defined in core_bringup.S */ + extern void riscv_bringup(void); + + /* mark first hart available */ + cpuid_to_hartid(0) = -1; + + /* assume we're in default Sv mode, in the future this will have to be + * fixed if we start implementing Sv48 etc. */ + pm_t satp = branch_to_satp(b, DEFAULT_Sv_MODE); + int cpu_offset = fdt_path_offset(fdt, "/cpus"); + struct cell_info ci = get_cellinfo(fdt, cpu_offset); + + /* almost directly lifted from netbsd */ + int node; + fdt_for_each_subnode(node, fdt, cpu_offset) { + if (!riscv_cpu_okay(fdt, node)) + continue; + + const void *reg = fdt_getprop(fdt, node, "reg", NULL); + id_t hartid = fdt_load_reg_addr(ci, reg, 0); + + struct sbiret r = sbi_hart_status(hartid); + if (r.error) { + warn("failed getting hart %d status: %ld\n", + hartid, + r.error); + continue; + } + + if (r.value == SBI_HART_STARTED) { + /* there should ever only be one started hart */ + catastrophic_assert(cpuid_to_hartid(0) == -1); + cpuid_to_hartid(0) = hartid; + continue; + } + + /** @todo should check that cpus doesn't go over MAX_CPUS */ + cpuid_to_hartid(cpus++) = hartid; + + /** @todo try to remember to free these as well */ + smp_init_stacks[hartid] = (void *)alloc_page(BASE_PAGE) + + BASE_PAGE_SIZE; + r = sbi_hart_start(hartid, + (unsigned long)__pa(riscv_bringup), satp); + + if (r.error) { + warn("failed bringing up hart %d: %ld\n", + hartid, + r.error); + continue; + } + } +} + +/** + * Called from secondary_bringup.S to finish bringing up core we're running on. + * + * @param hartid Hart that's being brought up. + */ +void core_bringup(long hartid) +{ + /* assume smp_bringup assigned our cpuid correctly */ + id_t cpuid = hartid_to_cpuid(hartid); + info("core %d online\n", cpuid); + + /* add us as a thread to init program that cpu 0 is hopefully running by + * now */ + struct tcb *t = create_thread(cpu_tcb(0)); + t->cpu_id = cpuid; + tcb_assign(t); + use_tcb(t); + + /* eventually we should jump to init and start running stuff, but for + * now take it easy */ + while (1); +} diff --git a/arch/riscv64/kernel/vmem.c b/arch/riscv64/kernel/vmem.c index 9edd246..e2d0fa3 100644 --- a/arch/riscv64/kernel/vmem.c +++ b/arch/riscv64/kernel/vmem.c @@ -13,6 +13,7 @@ #include #include #include "pages.h" +#include "arch.h" #include "csr.h" /** @@ -386,28 +387,33 @@ void flush_tlb_all() */ static void __use_vmem(struct vmem *branch, enum mm_mode m) { - branch = (struct vmem *)__pa(branch); - pm_t pn = (pm_t)(branch) >> page_shift(); - - pm_t mode = DEFAULT_Sv_MODE; - - if (m == Sv32) - mode = SATP_MODE_Sv32; - else if (m == Sv39) - mode = SATP_MODE_Sv39; - else if (m == Sv48) - mode = SATP_MODE_Sv48; - - csr_write(CSR_SATP, mode | pn); + pm_t satp = branch_to_satp(branch, m); + csr_write(CSR_SATP, satp); flush_tlb_full(); - /* Sv57 && Sv64 in the future? */ /** @todo ASID table for maybe faster context switches? */ } +/** + * Populate \p branch with direct mapping. + * Mainly intended for bringup and init stuff. + * Arguably works only for riscv64 and is sort of shared with init.c, so could + * still be improved. + * + * @param branch Branch to populate. + */ +static void __populate_dmap(struct vmem *branch) +{ + size_t flags = VM_V | VM_R | VM_W | VM_X | VM_G | VM_D | VM_A; + for (size_t i = 0; i < CSTACK_PAGE; ++i) + branch->leaf[i] = + (struct vmem *)to_pte(TOP_PAGE_SIZE * i, flags); +} + struct vmem *init_vmem(void *fdt) { UNUSED(fdt); struct vmem *b = create_vmem(); + __populate_dmap(b); /* update which memory branch to use */ use_vmem(b); return b; -- cgit v1.3