[PATCH 08/10] bsps/arm: basic BCM2836 SMP implementation.
pisa at cmp.felk.cvut.cz
pisa at cmp.felk.cvut.cz
Sat Sep 3 12:31:24 UTC 2016
From: Pavel Pisa <pisa at cmp.felk.cvut.cz>
---
c/src/lib/libbsp/arm/raspberrypi/Makefile.am | 3 +
c/src/lib/libbsp/arm/raspberrypi/include/bsp.h | 3 +
c/src/lib/libbsp/arm/raspberrypi/irq/irq.c | 18 +++
c/src/lib/libbsp/arm/raspberrypi/startup/bspsmp.c | 122 +++++++++++++++++++++
.../libbsp/arm/raspberrypi/startup/bspstarthooks.c | 35 +++++-
.../lib/libbsp/arm/raspberrypi/startup/linkcmds.in | 2 +-
.../arm/raspberrypi/startup/mm_config_table.c | 8 ++
7 files changed, 187 insertions(+), 4 deletions(-)
create mode 100644 c/src/lib/libbsp/arm/raspberrypi/startup/bspsmp.c
diff --git a/c/src/lib/libbsp/arm/raspberrypi/Makefile.am b/c/src/lib/libbsp/arm/raspberrypi/Makefile.am
index 01f4ad2..1a2f0aa 100644
--- a/c/src/lib/libbsp/arm/raspberrypi/Makefile.am
+++ b/c/src/lib/libbsp/arm/raspberrypi/Makefile.am
@@ -100,6 +100,9 @@ libbsp_a_SOURCES += ../../shared/bspreset_loop.c
libbsp_a_SOURCES += startup/bspstart.c
libbsp_a_SOURCES += startup/cmdline.c
libbsp_a_SOURCES += startup/bspgetworkarea.c
+if HAS_SMP
+libbsp_a_SOURCES += startup/bspsmp.c
+endif
# IRQ
libbsp_a_SOURCES += ../shared/arm-cp15-set-exception-handler.c
diff --git a/c/src/lib/libbsp/arm/raspberrypi/include/bsp.h b/c/src/lib/libbsp/arm/raspberrypi/include/bsp.h
index 5241979..ecd5b6a 100644
--- a/c/src/lib/libbsp/arm/raspberrypi/include/bsp.h
+++ b/c/src/lib/libbsp/arm/raspberrypi/include/bsp.h
@@ -50,6 +50,9 @@ void rpi_video_init(void);
void rpi_fb_outch (char);
int rpi_video_is_initialized(void);
+void rpi_ipi_initialize(void);
+void rpi_start_rtems_on_secondary_processor(void);
+
#ifdef __cplusplus
}
#endif /* __cplusplus */
diff --git a/c/src/lib/libbsp/arm/raspberrypi/irq/irq.c b/c/src/lib/libbsp/arm/raspberrypi/irq/irq.c
index e654d07..48f7810 100644
--- a/c/src/lib/libbsp/arm/raspberrypi/irq/irq.c
+++ b/c/src/lib/libbsp/arm/raspberrypi/irq/irq.c
@@ -32,6 +32,11 @@
#include <rtems/bspIo.h>
#include <strings.h>
+#ifdef RTEMS_SMP
+#include <rtems/score/smp.h>
+#include <rtems/score/smpimpl.h>
+#endif
+
typedef struct {
unsigned long enable_reg_addr;
unsigned long disable_reg_addr;
@@ -97,6 +102,19 @@ void bsp_interrupt_dispatch(void)
rtems_vector_number vector = 255;
+#ifdef RTEMS_SMP
+ uint32_t cpu_index_self = _SMP_Get_current_processor();
+ uint32_t local_source = BCM2835_REG(BCM2836_IRQ_SOURCE_REG(cpu_index_self));
+
+ if ( local_source & BCM2836_IRQ_SOURCE_MBOX3 ) {
+ /* reset mailbox 3 contents to zero */
+ BCM2835_REG(BCM2836_MAILBOX_3_READ_CLEAR_BASE + 0x10 * cpu_index_self) = 0xffffffff;
+ _SMP_Inter_processor_interrupt_handler();
+ }
+ if ( cpu_index_self != 0 )
+ return;
+#endif /* RTEMS_SMP */
+
pend = BCM2835_REG(BCM2835_IRQ_BASIC);
if ( pend & BCM2835_IRQ_BASIC_SPEEDUP_USED_BITS ) {
pend_bit = ffs(pend) - 1;
diff --git a/c/src/lib/libbsp/arm/raspberrypi/startup/bspsmp.c b/c/src/lib/libbsp/arm/raspberrypi/startup/bspsmp.c
new file mode 100644
index 0000000..3f9fd6a
--- /dev/null
+++ b/c/src/lib/libbsp/arm/raspberrypi/startup/bspsmp.c
@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) 2016 Pavel Pisa <pisa at cmp.felk.cvut.cz>
+ *
+ * Czech Technical University in Prague
+ * Zikova 1903/4
+ * 166 36 Praha 6
+ * Czech Republic
+ *
+ * Reuses some ideas from Rohini Kulkarni <krohini1593 at gmail.com>
+ * GSoC 2015 project and Altera Cyclone-V SMP code
+ * by embedded brains GmbH
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.org/license/LICENSE.
+ */
+
+#include <rtems/score/smpimpl.h>
+
+#include <bsp/start.h>
+#include <bsp/raspberrypi.h>
+#include <bsp.h>
+#include <bsp/arm-cp15-start.h>
+#include <libcpu/arm-cp15.h>
+#include <rtems.h>
+#include <bsp/irq-generic.h>
+#include <assert.h>
+
+bool _CPU_SMP_Start_processor( uint32_t cpu_index )
+{
+ bool started;
+ uint32_t cpu_index_self = _SMP_Get_current_processor();
+
+ if (cpu_index != cpu_index_self) {
+
+ BCM2835_REG(BCM2836_MAILBOX_3_WRITE_SET_BASE + 0x10 * cpu_index) = (uint32_t)_start;
+
+ /*
+ * Wait for secondary processor to complete its basic initialization so
+ * that we can enable the unified L2 cache.
+ */
+ started = _Per_CPU_State_wait_for_non_initial_state(cpu_index, 0);
+ } else {
+ started = false;
+ }
+
+ return started;
+}
+
+
+void rpi_ipi_initialize(void)
+{
+ uint32_t cpu_index_self = _SMP_Get_current_processor();
+
+ /*
+ * Includes support only for mailbox 3 interrupt.
+ * Further interrupt support has to be added. This will have to be integrated
+ * with existing interrupt support for Raspberry Pi
+ */
+
+ /* reset mailbox 3 contents to zero */
+ BCM2835_REG(BCM2836_MAILBOX_3_READ_CLEAR_BASE + 0x10 * cpu_index_self) = 0xffffffff;
+
+ BCM2835_REG(BCM2836_MAILBOX_IRQ_CTRL(cpu_index_self)) =
+ BCM2836_MAILBOX_IRQ_CTRL_MBOX3_IRQ;
+}
+
+void rpi_start_rtems_on_secondary_processor(void)
+{
+ uint32_t ctrl;
+
+ ctrl = arm_cp15_start_setup_mmu_and_cache(
+ 0,
+ ARM_CP15_CTRL_AFE | ARM_CP15_CTRL_Z
+ );
+
+ rpi_ipi_initialize();
+
+ arm_cp15_set_domain_access_control(
+ ARM_CP15_DAC_DOMAIN(ARM_MMU_DEFAULT_CLIENT_DOMAIN, ARM_CP15_DAC_CLIENT)
+ );
+
+ /* FIXME: Sharing the translation table between processors is brittle */
+ arm_cp15_set_translation_table_base(
+ (uint32_t *) bsp_translation_table_base
+ );
+
+ arm_cp15_tlb_invalidate();
+
+ ctrl |= ARM_CP15_CTRL_I | ARM_CP15_CTRL_C | ARM_CP15_CTRL_M;
+ ctrl &= ~ARM_CP15_CTRL_V;
+ arm_cp15_set_control(ctrl);
+
+ _SMP_Start_multitasking_on_secondary_processor();
+}
+
+uint32_t _CPU_SMP_Initialize(void)
+{
+ uint32_t cpu_count = (uint32_t)bsp_processor_count;
+
+ if ( cpu_count > 4 )
+ cpu_count = 4;
+
+ return cpu_count;
+}
+
+void _CPU_SMP_Finalize_initialization( uint32_t cpu_count )
+{
+ /* Do nothing */
+}
+
+void _CPU_SMP_Prepare_start_multitasking( void )
+{
+ /* Do nothing */
+}
+
+void _CPU_SMP_Send_interrupt( uint32_t target_cpu_index )
+{
+ /* Generates IPI */
+ BCM2835_REG(BCM2836_MAILBOX_3_WRITE_SET_BASE +
+ 0x10 * target_cpu_index) = 0x1;
+}
diff --git a/c/src/lib/libbsp/arm/raspberrypi/startup/bspstarthooks.c b/c/src/lib/libbsp/arm/raspberrypi/startup/bspstarthooks.c
index 5b14e10..165a555 100644
--- a/c/src/lib/libbsp/arm/raspberrypi/startup/bspstarthooks.c
+++ b/c/src/lib/libbsp/arm/raspberrypi/startup/bspstarthooks.c
@@ -29,10 +29,16 @@
#include <libcpu/arm-cp15.h>
#include <bsp.h>
+#ifdef RTEMS_SMP
+#include <rtems/score/smp.h>
+#endif
void BSP_START_TEXT_SECTION bsp_start_hook_0(void)
{
uint32_t sctlr_val;
+#ifdef RTEMS_SMP
+ uint32_t cpu_index_self = _SMP_Get_current_processor();
+#endif /* RTEMS_SMP */
sctlr_val = arm_cp15_get_control();
@@ -48,14 +54,29 @@ void BSP_START_TEXT_SECTION bsp_start_hook_0(void)
* If the data cache is on then ensure that it is clean
* before switching off to be extra carefull.
*/
- rtems_cache_flush_entire_data();
- rtems_cache_invalidate_entire_data();
+#ifdef RTEMS_SMP
+ if (cpu_index_self != 0) {
+ arm_cp15_data_cache_clean_level(0);
+ arm_cp15_cache_invalidate_level(0, 0);
+ } else
+#endif /* RTEMS_SMP */
+ {
+ rtems_cache_flush_entire_data();
+ rtems_cache_invalidate_entire_data();
+ }
}
arm_cp15_flush_prefetch_buffer();
sctlr_val &= ~(ARM_CP15_CTRL_I | ARM_CP15_CTRL_C | ARM_CP15_CTRL_M | ARM_CP15_CTRL_A);
arm_cp15_set_control(sctlr_val);
}
- rtems_cache_invalidate_entire_data();
+#ifdef RTEMS_SMP
+ if (cpu_index_self != 0) {
+ arm_cp15_cache_invalidate_level(0, 0);
+ } else
+#endif /* RTEMS_SMP */
+ {
+ rtems_cache_invalidate_entire_data();
+ }
rtems_cache_invalidate_entire_instruction();
arm_cp15_branch_predictor_invalidate_all();
arm_cp15_tlb_invalidate();
@@ -66,6 +87,14 @@ void BSP_START_TEXT_SECTION bsp_start_hook_0(void)
/* Clear Secure or Non-secure Vector Base Address Register */
arm_cp15_set_vector_base_address(0);
+
+#ifdef RTEMS_SMP
+ if (cpu_index_self == 0) {
+ rpi_ipi_initialize();
+ } else {
+ rpi_start_rtems_on_secondary_processor();
+ }
+#endif
}
void BSP_START_TEXT_SECTION bsp_start_hook_1(void)
diff --git a/c/src/lib/libbsp/arm/raspberrypi/startup/linkcmds.in b/c/src/lib/libbsp/arm/raspberrypi/startup/linkcmds.in
index d88c6d7..829716c 100644
--- a/c/src/lib/libbsp/arm/raspberrypi/startup/linkcmds.in
+++ b/c/src/lib/libbsp/arm/raspberrypi/startup/linkcmds.in
@@ -61,7 +61,7 @@ REGION_ALIAS ("REGION_STACK", RAM);
REGION_ALIAS ("REGION_NOCACHE", RAM);
REGION_ALIAS ("REGION_NOCACHE_LOAD", RAM);
-bsp_stack_irq_size = DEFINED (bsp_stack_irq_size) ? bsp_stack_irq_size : 4096;
+bsp_stack_irq_size = DEFINED (bsp_stack_irq_size) ? bsp_stack_irq_size : 3008;
bsp_stack_abt_size = DEFINED (bsp_stack_abt_size) ? bsp_stack_abt_size : 1024;
bsp_section_robarrier_align = DEFINED (bsp_section_robarrier_align) ? bsp_section_robarrier_align : 1M;
diff --git a/c/src/lib/libbsp/arm/raspberrypi/startup/mm_config_table.c b/c/src/lib/libbsp/arm/raspberrypi/startup/mm_config_table.c
index 50e19d8..27fa76b 100644
--- a/c/src/lib/libbsp/arm/raspberrypi/startup/mm_config_table.c
+++ b/c/src/lib/libbsp/arm/raspberrypi/startup/mm_config_table.c
@@ -82,6 +82,14 @@ const arm_cp15_start_section_config arm_cp15_start_mmu_config_table[] = {
.end = RPI_PERIPHERAL_BASE + RPI_PERIPHERAL_SIZE,
.flags = ARMV7_MMU_DEVICE
}
+#if (BSP_IS_RPI2 == 1)
+ /* Core local peripherals area - timer, mailboxes */
+ , {
+ .begin = BCM2836_CORE_LOCAL_PERIPH_BASE,
+ .end = BCM2836_CORE_LOCAL_PERIPH_BASE + BCM2836_CORE_LOCAL_PERIPH_SIZE,
+ .flags = ARMV7_MMU_DEVICE
+ }
+#endif
};
const size_t arm_cp15_start_mmu_config_table_size =
--
1.9.1
More information about the devel
mailing list