arm/raspberrypi: basic BCM2836 SMP implementation.

The BSP support is divided to startup/bspsmp_api.c file where
functions required by SuperCore are defined and BCM2836 hardware
initialization part in startup/bspsmp_init.c.

Separation is done to prevent smpfatal08 test build failure.

8 files changed, 230 insertions, 4 deletions

diff --git a/c/src/lib/libbsp/arm/raspberrypi/Makefile.am b/c/src/lib/libbsp/arm/raspberrypi/Makefile.am
index 01f4ad226d..dfcc3e5db3 100644
--- a/c/src/lib/libbsp/arm/raspberrypi/Makefile.am
+++ b/c/src/lib/libbsp/arm/raspberrypi/Makefile.am
@@ -100,6 +100,10 @@ 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_api.c
+libbsp_a_SOURCES += startup/bspsmp_init.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 5241979f49..ecd5b6af08 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 e654d077c9..48f781075b 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_api.c b/c/src/lib/libbsp/arm/raspberrypi/startup/bspsmp_api.c
new file mode 100644
index 0000000000..c3e5451442
--- /dev/null
+++ b/c/src/lib/libbsp/arm/raspberrypi/startup/bspsmp_api.c
@@ -0,0 +1,83 @@
+/**
+ * @file
+ *
+ * @ingroup raspberrypi
+ *
+ * @brief Raspberry pi SMP management functions provided to SuperCore
+ */
+
+/*
+ * Copyright (c) 2016 Pavel Pisa <pisa@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@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;
+}
+
+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/bspsmp_init.c b/c/src/lib/libbsp/arm/raspberrypi/startup/bspsmp_init.c
new file mode 100644
index 0000000000..8c8cd74712
--- /dev/null
+++ b/c/src/lib/libbsp/arm/raspberrypi/startup/bspsmp_init.c
@@ -0,0 +1,81 @@
+/**
+ * @file
+ *
+ * @ingroup raspberrypi
+ *
+ * @brief Raspberry pi secondary CPU and IPI HW initialization
+ */
+
+/*
+ * Copyright (c) 2016 Pavel Pisa <pisa@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@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>
+
+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();
+}
diff --git a/c/src/lib/libbsp/arm/raspberrypi/startup/bspstarthooks.c b/c/src/lib/libbsp/arm/raspberrypi/startup/bspstarthooks.c
index 5b14e100ad..165a555880 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 d88c6d74b8..829716c11c 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 50e19d85e1..27fa76b355 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 =