From: Pavel Pisa <p...@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 <p...@cmp.felk.cvut.cz> + * + * Czech Technical University in Prague + * Zikova 1903/4 + * 166 36 Praha 6 + * Czech Republic + * + * Reuses some ideas from Rohini Kulkarni <krohini1...@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 _______________________________________________ devel mailing list devel@rtems.org http://lists.rtems.org/mailman/listinfo/devel