[rtems commit] bsp/riscv: Add basic SMP startup
Sebastian Huber
sebh at rtems.org
Wed Jul 25 08:12:03 UTC 2018
Module: rtems
Branch: master
Commit: 447fd894aeb245959627893e138b2b9640486040
Changeset: http://git.rtems.org/rtems/commit/?id=447fd894aeb245959627893e138b2b9640486040
Author: Sebastian Huber <sebastian.huber at embedded-brains.de>
Date: Fri Jul 20 13:11:04 2018 +0200
bsp/riscv: Add basic SMP startup
Update #3433.
---
bsps/include/bsp/fatal.h | 4 +-
bsps/riscv/riscv/clock/clockdrv.c | 10 +---
bsps/riscv/riscv/include/bsp/riscv.h | 12 +++++
bsps/riscv/riscv/irq/irq.c | 56 ++++++++++++++++++++
bsps/riscv/riscv/start/bspsmp.c | 77 +++++++++++++++++++++++++++
bsps/riscv/riscv/start/bspstart.c | 91 ++++++++++++++++++++++++++++++++
bsps/riscv/riscv/start/start.S | 75 +++++++++++++++++++-------
c/src/lib/libbsp/riscv/riscv/Makefile.am | 2 +-
8 files changed, 299 insertions(+), 28 deletions(-)
diff --git a/bsps/include/bsp/fatal.h b/bsps/include/bsp/fatal.h
index 71edcb0..2679af1 100644
--- a/bsps/include/bsp/fatal.h
+++ b/bsps/include/bsp/fatal.h
@@ -145,7 +145,9 @@ typedef enum {
RISCV_FATAL_NO_NS16550_CLOCK_FREQUENCY_IN_DEVICE_TREE,
RISCV_FATAL_UNEXPECTED_INTERRUPT_EXCEPTION,
RISCV_FATAL_CLOCK_IRQ_INSTALL,
- RISCV_FATAL_NO_CLINT_REG_IN_DEVICE_TREE
+ RISCV_FATAL_NO_CLINT_REG_IN_DEVICE_TREE,
+ RISCV_FATAL_INVALID_HART_REG_IN_DEVICE_TREE,
+ RISCV_FATAL_INVALID_CLINT_IRQS_EXTENDED_IN_DEVICE_TREE
} bsp_fatal_code;
RTEMS_NO_RETURN static inline void
diff --git a/bsps/riscv/riscv/clock/clockdrv.c b/bsps/riscv/riscv/clock/clockdrv.c
index 677823c..4220952 100644
--- a/bsps/riscv/riscv/clock/clockdrv.c
+++ b/bsps/riscv/riscv/clock/clockdrv.c
@@ -119,19 +119,13 @@ static void riscv_clock_initialize(void)
{
const char *fdt;
riscv_timecounter *tc;
- int node;
uint32_t tb_freq;
uint64_t us_per_tick;
fdt = bsp_fdt_get();
- tc = &riscv_clock_tc;
-
- node = fdt_node_offset_by_compatible(fdt, -1, "riscv,clint0");
- tc->clint = riscv_fdt_get_address(fdt, node);
- if (tc->clint == NULL) {
- bsp_fatal(RISCV_FATAL_NO_CLINT_REG_IN_DEVICE_TREE);
- }
+ tc = &riscv_clock_tc;
+ tc->clint = riscv_clint;
tb_freq = riscv_clock_get_timebase_frequency(fdt);
us_per_tick = rtems_configuration_get_microseconds_per_tick();
diff --git a/bsps/riscv/riscv/include/bsp/riscv.h b/bsps/riscv/riscv/include/bsp/riscv.h
index b85b250..6701bb6 100644
--- a/bsps/riscv/riscv/include/bsp/riscv.h
+++ b/bsps/riscv/riscv/include/bsp/riscv.h
@@ -28,12 +28,24 @@
#include <bsp.h>
+#include <rtems/score/cpuimpl.h>
+
#ifdef __cplusplus
extern "C" {
#endif
+extern volatile RISCV_CLINT_regs *riscv_clint;
+
void *riscv_fdt_get_address(const void *fdt, int node);
+#ifdef RTEMS_SMP
+extern uint32_t riscv_hart_count;
+
+extern uint32_t riscv_hart_phandles[CPU_MAXIMUM_PROCESSORS];
+
+uint32_t riscv_get_hart_index_by_phandle(uint32_t phandle);
+#endif
+
#if RISCV_ENABLE_HTIF_SUPPORT != 0
void htif_poweroff(void);
#endif
diff --git a/bsps/riscv/riscv/irq/irq.c b/bsps/riscv/riscv/irq/irq.c
index 61cf131..a3a17f5 100644
--- a/bsps/riscv/riscv/irq/irq.c
+++ b/bsps/riscv/riscv/irq/irq.c
@@ -36,9 +36,17 @@
#include <bsp/irq.h>
#include <bsp/fatal.h>
+#include <bsp/fdt.h>
#include <bsp/irq-generic.h>
+#include <bsp/riscv.h>
#include <rtems/score/percpu.h>
+#include <rtems/score/riscv-utility.h>
+#include <rtems/score/smpimpl.h>
+
+#include <libfdt.h>
+
+volatile RISCV_CLINT_regs *riscv_clint;
void _RISCV_Interrupt_dispatch(uintptr_t mcause, Per_CPU_Control *cpu_self)
{
@@ -53,14 +61,62 @@ void _RISCV_Interrupt_dispatch(uintptr_t mcause, Per_CPU_Control *cpu_self)
} else if (mcause == (RISCV_INTERRUPT_EXTERNAL_MACHINE << 1)) {
/* TODO: Handle PLIC interrupt */
} else if (mcause == (RISCV_INTERRUPT_SOFTWARE_MACHINE << 1)) {
+#ifdef RTEMS_SMP
+ clear_csr(mip, MIP_MSIP);
+ _SMP_Inter_processor_interrupt_handler(cpu_self);
+#else
bsp_interrupt_handler_dispatch(RISCV_INTERRUPT_VECTOR_SOFTWARE);
+#endif
} else {
bsp_fatal(RISCV_FATAL_UNEXPECTED_INTERRUPT_EXCEPTION);
}
}
+static void riscv_clint_init(const void *fdt)
+{
+ volatile RISCV_CLINT_regs *clint;
+ int node;
+#ifdef RTEMS_SMP
+ const uint32_t *val;
+ int len;
+ int i;
+#endif
+
+ node = fdt_node_offset_by_compatible(fdt, -1, "riscv,clint0");
+
+ clint = riscv_fdt_get_address(fdt, node);
+ if (clint == NULL) {
+ bsp_fatal(RISCV_FATAL_NO_CLINT_REG_IN_DEVICE_TREE);
+ }
+
+ riscv_clint = clint;
+
+#ifdef RTEMS_SMP
+ val = fdt_getprop(fdt, node, "interrupts-extended", &len);
+
+ for (i = 0; i < len; i += 16) {
+ uint32_t hart_index;
+ Per_CPU_Control *cpu;
+
+ hart_index = riscv_get_hart_index_by_phandle(fdt32_to_cpu(val[i / 4]));
+ if (hart_index >= rtems_configuration_get_maximum_processors()) {
+ continue;
+ }
+
+ cpu = _Per_CPU_Get_by_index(hart_index);
+ cpu->cpu_per_cpu.clint_msip = &clint->msip[i / 16];
+ cpu->cpu_per_cpu.clint_mtimecmp = &clint->mtimecmp[i / 16];
+ }
+#endif
+}
+
rtems_status_code bsp_interrupt_facility_initialize(void)
{
+ const void *fdt;
+
+ fdt = bsp_fdt_get();
+ riscv_clint_init(fdt);
+
return RTEMS_SUCCESSFUL;
}
diff --git a/bsps/riscv/riscv/start/bspsmp.c b/bsps/riscv/riscv/start/bspsmp.c
new file mode 100644
index 0000000..8e5540d
--- /dev/null
+++ b/bsps/riscv/riscv/start/bspsmp.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2018 embedded brains GmbH
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <bsp/bootcard.h>
+#include <bsp/irq.h>
+#include <bsp/riscv.h>
+
+#include <rtems/score/riscv-utility.h>
+#include <rtems/score/smpimpl.h>
+
+void bsp_start_on_secondary_processor(Per_CPU_Control *cpu_self)
+{
+ uint32_t cpu_index_self;
+
+ cpu_index_self = _Per_CPU_Get_index(cpu_self);
+
+ if (
+ cpu_index_self < rtems_configuration_get_maximum_processors()
+ && _SMP_Should_start_processor(cpu_index_self)
+ ) {
+ set_csr(mie, MIP_MSIP);
+ _SMP_Start_multitasking_on_secondary_processor(cpu_self);
+ } else {
+ _CPU_Thread_Idle_body(0);
+ }
+}
+
+uint32_t _CPU_SMP_Initialize(void)
+{
+ return riscv_hart_count;
+}
+
+bool _CPU_SMP_Start_processor(uint32_t cpu_index)
+{
+ return true;
+}
+
+void _CPU_SMP_Finalize_initialization(uint32_t cpu_count)
+{
+ (void) cpu_count;
+ set_csr(mie, MIP_MSIP);
+}
+
+void _CPU_SMP_Prepare_start_multitasking(void)
+{
+ /* Do nothing */
+}
+
+void _CPU_SMP_Send_interrupt(uint32_t target_processor_index)
+{
+ Per_CPU_Control *cpu;
+
+ cpu = _Per_CPU_Get_by_index(target_processor_index);
+ *cpu->cpu_per_cpu.clint_msip = 0x1;
+}
diff --git a/bsps/riscv/riscv/start/bspstart.c b/bsps/riscv/riscv/start/bspstart.c
index 6b57977..2cb453f 100644
--- a/bsps/riscv/riscv/start/bspstart.c
+++ b/bsps/riscv/riscv/start/bspstart.c
@@ -24,6 +24,8 @@
*/
#include <bsp/bootcard.h>
+#include <bsp/fatal.h>
+#include <bsp/fdt.h>
#include <bsp/irq-generic.h>
#include <bsp/riscv.h>
@@ -69,7 +71,96 @@ void *riscv_fdt_get_address(const void *fdt, int node)
return (void *)(uintptr_t) addr;
}
+#ifdef RTEMS_SMP
+uint32_t riscv_hart_count;
+
+uint32_t riscv_hart_phandles[CPU_MAXIMUM_PROCESSORS];
+
+static void riscv_find_harts(void)
+{
+ const void *fdt;
+ int node;
+ uint32_t max_hart_index;
+
+ fdt = bsp_fdt_get();
+ max_hart_index = 0;
+ node = -1;
+
+ while ((node = fdt_node_offset_by_compatible(fdt, node, "riscv")) >= 0) {
+ int subnode;
+ const uint32_t *val;
+ int len;
+ uint32_t phandle;
+ uint32_t hart_index;
+
+ val = fdt_getprop(fdt, node, "reg", &len);
+ if (val == NULL || len != 4) {
+ bsp_fatal(RISCV_FATAL_INVALID_HART_REG_IN_DEVICE_TREE);
+ }
+
+ hart_index = fdt32_to_cpu(val[0]);
+
+ if (hart_index >= CPU_MAXIMUM_PROCESSORS) {
+ continue;
+ }
+
+ if (hart_index > max_hart_index) {
+ max_hart_index = hart_index;
+ }
+
+ phandle = 0;
+
+ fdt_for_each_subnode(subnode, fdt, node) {
+ int propoff;
+ bool interrupt_controller;
+ uint32_t potential_phandle;
+
+ interrupt_controller = false;
+ potential_phandle = 0;
+
+ fdt_for_each_property_offset(propoff, fdt, subnode) {
+ const char *name;
+
+ val = fdt_getprop_by_offset(fdt, propoff, &name, &len);
+ if (val != NULL) {
+ if (strcmp(name, "interrupt-controller") == 0) {
+ interrupt_controller = true;
+ } else if (len == 4 && strcmp(name, "phandle") == 0) {
+ potential_phandle = fdt32_to_cpu(val[0]);
+ }
+ }
+ }
+
+ if (interrupt_controller) {
+ phandle = potential_phandle;
+ break;
+ }
+ }
+
+ riscv_hart_phandles[hart_index] = phandle;
+ }
+
+ riscv_hart_count = max_hart_index + 1;
+}
+
+uint32_t riscv_get_hart_index_by_phandle(uint32_t phandle)
+{
+ uint32_t hart_index;
+
+ for (hart_index = 0; hart_index < riscv_hart_count; ++hart_index) {
+ if (riscv_hart_phandles[hart_index] == phandle) {
+ return hart_index;
+ }
+ }
+
+ return UINT32_MAX;
+}
+#endif
+
void bsp_start(void)
{
+#ifdef RTEMS_SMP
+ riscv_find_harts();
+#endif
bsp_interrupt_initialize();
}
diff --git a/bsps/riscv/riscv/start/start.S b/bsps/riscv/riscv/start/start.S
index 83926a9..290c95a 100644
--- a/bsps/riscv/riscv/start/start.S
+++ b/bsps/riscv/riscv/start/start.S
@@ -48,26 +48,34 @@ SYM(_start):
LADDR gp, __global_pointer$
.option pop
+ /* Init FPU unit if it's there */
+ li t0, MSTATUS_FS
+ csrs mstatus, t0
+
+ /* Set exception handler */
+ LADDR t0, _RISCV_Exception_handler
+ csrw mtvec, t0
+
+ /* Load stack pointer and branch to secondary processor start if necessary */
#ifdef RTEMS_SMP
+ LADDR sp, _Configuration_Interrupt_stack_area_begin
+ LADDR t2, _Configuration_Interrupt_stack_size
csrr s0, mhartid
LADDR t0, _Per_CPU_Information
slli t1, s0, PER_CPU_CONTROL_SIZE_LOG2
- add t0, t0, t1
- csrw mscratch, t0
- bnez s0, .Lwait_for_go
-#endif
-
- /* load stack and frame pointers */
+ add s1, t0, t1
+ csrw mscratch, s1
+ bnez s0, .Lstart_on_secondary_processor
+ add sp, sp, t2
+#else
LADDR sp, _Configuration_Interrupt_stack_area_end
+#endif
#ifdef BSP_START_COPY_FDT_FROM_U_BOOT
mv a0, a1
call bsp_fdt_copy
#endif
- LADDR t0, _RISCV_Exception_handler
- csrw mtvec, t0
-
/* Clear .bss */
LADDR a0, bsp_section_bss_begin
li a1, 0
@@ -81,24 +89,55 @@ SYM(_start):
amoswap.w zero, zero, 0(t0)
#endif
- /* Init FPU unit if it's there */
- li t0, MSTATUS_FS
- csrs mstatus, t0
-
j boot_card
#ifdef RTEMS_SMP
+
+.Lstart_on_secondary_processor:
+
+ /* Adjust stack pointer */
+#ifdef __riscv_mul
+ addi t0, s0, 1
+ mul t2, t2, t0
+#else
+ mv t0, s0
+ mv t3, t2
+
+.Ladd_more:
+
+ add t2, t2, t3
+ addi t0, t0, -1
+ bnez t0, .Ladd_more
+#endif
+ add sp, sp, t2
+
/* Wait for go issued by the boot processor (mhartid == 0) */
-.Lwait_for_go:
LADDR t0, .Lsecondary_processor_go
+
.Lwait_for_go_again:
+
lw t1, 0(t0)
fence iorw, iorw
- sext.w t1, t1
bnez t1, .Lwait_for_go_again
-.Lloop_forever:
- j .Lloop_forever
+
+ mv a0, s1
+ call bsp_start_on_secondary_processor
+
+#if __riscv_xlen == 32
+ .align 2
+#elif __riscv_xlen == 64
+ .align 3
+#endif
.Lsecondary_processor_go:
- .word 0xdeadbeef
+
+ /*
+ * These are ebreak instructions, just in case we end up here executing
+ * code.
+ */
+ .word 0x00100073
+#if __riscv_xlen == 64
+ .word 0x00100073
#endif
+
+#endif /* RTEMS_SMP */
diff --git a/c/src/lib/libbsp/riscv/riscv/Makefile.am b/c/src/lib/libbsp/riscv/riscv/Makefile.am
index b9a9a63..23c574f 100644
--- a/c/src/lib/libbsp/riscv/riscv/Makefile.am
+++ b/c/src/lib/libbsp/riscv/riscv/Makefile.am
@@ -61,7 +61,7 @@ librtemsbsp_a_SOURCES += ../../../../../../bsps/riscv/riscv/console/console-conf
librtemsbsp_a_SOURCES += ../../../../../../bsps/riscv/riscv/console/htif.c
if HAS_SMP
-librtemsbsp_a_SOURCES += ../../../../../../bsps/shared/start/bspsmp-dummy.c
+librtemsbsp_a_SOURCES += ../../../../../../bsps/riscv/riscv/start/bspsmp.c
endif
include $(top_srcdir)/../../../../automake/local.am
More information about the vc
mailing list