bsps/arm: Add SMP support

c/src/lib/libbsp/arm/realview-pbx-a9/Makefile.am

@@ -35,6 +35,7 @@ include_bsp_HEADERS += ../../shared/tod.h
 include_bsp_HEADERS += ../shared/include/start.h
 include_bsp_HEADERS += ../shared/include/arm-a9mpcore-irq.h
 include_bsp_HEADERS += ../shared/include/arm-a9mpcore-regs.h
+include_bsp_HEADERS += ../shared/include/arm-a9mpcore-start.h
 include_bsp_HEADERS += ../shared/include/arm-cp15-start.h
 include_bsp_HEADERS += ../shared/include/arm-gic.h
 include_bsp_HEADERS += ../shared/include/arm-gic-irq.h
@@ -59,6 +60,8 @@ project_lib_DATA = start.$(OBJEXT)
 project_lib_DATA += startup/linkcmds
 
 EXTRA_DIST =
+EXTRA_DIST += startup/linkcmds.realview_pbx_a9_qemu
+EXTRA_DIST += startup/linkcmds.realview_pbx_a9_qemu_smp
 
 ###############################################################################
 #                  LibBSP                                                     #
@@ -121,6 +124,10 @@ libbsp_a_CPPFLAGS += -I$(srcdir)/../../../libcpu/arm/shared/include
 # Start hooks
 libbsp_a_SOURCES += startup/bspstarthooks.c
 
+if HAS_SMP
+libbsp_a_SOURCES += ../shared/arm-a9mpcore-smp.c
+endif
+
 ###############################################################################
 #                  Special Rules                                              #
 ###############################################################################

c/src/lib/libbsp/arm/realview-pbx-a9/README

@@ -11,3 +11,5 @@ make install
 export PATH="$PATH:/opt/qemu/bin"
 
 qemu-system-arm -S -s -no-reboot -net none -nographic -M realview-pbx-a9 -m 256M -kernel ticker.exe
+
+qemu-system-arm -S -s -no-reboot -net none -nographic -smp 2 -icount auto -M realview-pbx-a9 -m 256M -kernel ticker.exe

c/src/lib/libbsp/arm/realview-pbx-a9/configure.ac

@@ -33,6 +33,9 @@ RTEMS_BSPOPTS_HELP([CLOCK_DRIVER_USE_FAST_IDLE],
 occurs while the IDLE thread is executing.  This can significantly reduce
 simulation times.])
 
+RTEMS_CHECK_SMP
+AM_CONDITIONAL(HAS_SMP,[test "$rtems_cv_HAS_SMP" = "yes"])
+
 RTEMS_BSP_CLEANUP_OPTIONS(0, 1)
 RTEMS_BSP_LINKCMDS
 

c/src/lib/libbsp/arm/realview-pbx-a9/include/bsp.h

@@ -31,8 +31,14 @@
 extern "C" {
 #endif /* __cplusplus */
 
+#define BSP_ARM_A9MPCORE_SCU_BASE 0x1f000000
+
+#define BSP_ARM_GIC_CPUIF_BASE 0x1f000100
+
 #define BSP_ARM_A9MPCORE_PT_BASE 0x1f000600
 
+#define BSP_ARM_GIC_DIST_BASE 0x1f001000
+
 typedef enum {
   BSP_ARM_A9MPCORE_FATAL_CLOCK_IRQ_INSTALL,
   BSP_ARM_A9MPCORE_FATAL_CLOCK_IRQ_REMOVE

c/src/lib/libbsp/arm/realview-pbx-a9/include/irq.h

@@ -77,10 +77,6 @@ extern "C" {
 #define BSP_INTERRUPT_VECTOR_MIN 0
 #define BSP_INTERRUPT_VECTOR_MAX 89
 
-#define BSP_ARM_GIC_CPUIF_BASE 0x1f000100
-
-#define BSP_ARM_GIC_DIST_BASE 0x1f001000
-
 #ifdef __cplusplus
 }
 #endif /* __cplusplus */

c/src/lib/libbsp/arm/realview-pbx-a9/make/custom/realview_pbx_a9_qemu_smp.cfg

@@ -0,0 +1 @@
+include $(RTEMS_ROOT)/make/custom/realview_pbx_a9_qemu.cfg

c/src/lib/libbsp/arm/realview-pbx-a9/preinstall.am

@@ -94,6 +94,10 @@ $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-regs.h: ../shared/include/arm-a9mpcore-regs.
 	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-regs.h
 PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-regs.h
 
+$(PROJECT_INCLUDE)/bsp/arm-a9mpcore-start.h: ../shared/include/arm-a9mpcore-start.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
+	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-start.h
+PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-start.h
+
 $(PROJECT_INCLUDE)/bsp/arm-cp15-start.h: ../shared/include/arm-cp15-start.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
 	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/arm-cp15-start.h
 PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/arm-cp15-start.h

c/src/lib/libbsp/arm/realview-pbx-a9/startup/bspstarthooks.c

@@ -15,12 +15,14 @@
 #include <bsp.h>
 #include <bsp/start.h>
 #include <bsp/arm-cp15-start.h>
+#include <bsp/arm-a9mpcore-start.h>
 #include <bsp/linker-symbols.h>
 
-BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
+#ifdef RTEMS_SMP
-{
+  #define MMU_DATA_READ_WRITE ARMV7_MMU_DATA_READ_WRITE_SHAREABLE
-  /* Do nothing */
+#else
-}
+  #define MMU_DATA_READ_WRITE ARMV7_MMU_DATA_READ_WRITE_CACHED
+#endif
 
 BSP_START_DATA_SECTION static const arm_cp15_start_section_config
 rvpbxa9_mmu_config_table[] = {
@@ -31,7 +33,7 @@ rvpbxa9_mmu_config_table[] = {
   }, {
     .begin = (uint32_t) bsp_section_fast_data_begin,
     .end = (uint32_t) bsp_section_fast_data_end,
-    .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+    .flags = MMU_DATA_READ_WRITE
   }, {
     .begin = (uint32_t) bsp_section_start_begin,
     .end = (uint32_t) bsp_section_start_end,
@@ -39,7 +41,7 @@ rvpbxa9_mmu_config_table[] = {
   }, {
     .begin = (uint32_t) bsp_section_vector_begin,
     .end = (uint32_t) bsp_section_vector_end,
-    .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+    .flags = MMU_DATA_READ_WRITE
   }, {
     .begin = (uint32_t) bsp_section_text_begin,
     .end = (uint32_t) bsp_section_text_end,
@@ -51,19 +53,19 @@ rvpbxa9_mmu_config_table[] = {
   }, {
     .begin = (uint32_t) bsp_section_data_begin,
     .end = (uint32_t) bsp_section_data_end,
-    .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+    .flags = MMU_DATA_READ_WRITE
   }, {
     .begin = (uint32_t) bsp_section_bss_begin,
     .end = (uint32_t) bsp_section_bss_end,
-    .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+    .flags = MMU_DATA_READ_WRITE
   }, {
     .begin = (uint32_t) bsp_section_work_begin,
     .end = (uint32_t) bsp_section_work_end,
-    .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+    .flags = MMU_DATA_READ_WRITE
   }, {
     .begin = (uint32_t) bsp_section_stack_begin,
     .end = (uint32_t) bsp_section_stack_end,
-    .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+    .flags = MMU_DATA_READ_WRITE
   }, {
     .begin = 0x10000000U,
     .end = 0x10020000U,
@@ -91,6 +93,11 @@ BSP_START_TEXT_SECTION static void setup_mmu_and_cache(void)
   );
 }
 
+BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
+{
+  arm_a9mpcore_start_hook_0();
+}
+
 BSP_START_TEXT_SECTION void bsp_start_hook_1(void)
 {
   bsp_start_copy_sections();

c/src/lib/libbsp/arm/realview-pbx-a9/startup/linkcmds.realview_pbx_a9_qemu_smp

@@ -0,0 +1,33 @@
+MEMORY {
+	RAM : ORIGIN = 0x00000000, LENGTH = 256M - 16k
+	RAM_MMU : ORIGIN = 0x0fffc000, LENGTH = 16k
+}
+
+REGION_ALIAS ("REGION_START", RAM);
+REGION_ALIAS ("REGION_VECTOR", RAM);
+REGION_ALIAS ("REGION_TEXT", RAM);
+REGION_ALIAS ("REGION_TEXT_LOAD", RAM);
+REGION_ALIAS ("REGION_RODATA", RAM);
+REGION_ALIAS ("REGION_RODATA_LOAD", RAM);
+REGION_ALIAS ("REGION_DATA", RAM);
+REGION_ALIAS ("REGION_DATA_LOAD", RAM);
+REGION_ALIAS ("REGION_FAST_TEXT", RAM);
+REGION_ALIAS ("REGION_FAST_TEXT_LOAD", RAM);
+REGION_ALIAS ("REGION_FAST_DATA", RAM);
+REGION_ALIAS ("REGION_FAST_DATA_LOAD", RAM);
+REGION_ALIAS ("REGION_BSS", RAM);
+REGION_ALIAS ("REGION_WORK", RAM);
+REGION_ALIAS ("REGION_STACK", RAM);
+
+bsp_stack_irq_size = DEFINED (bsp_stack_irq_size) ? bsp_stack_irq_size : 4096;
+bsp_stack_abt_size = DEFINED (bsp_stack_abt_size) ? bsp_stack_abt_size : 1024;
+
+bsp_section_rwbarrier_align = DEFINED (bsp_section_rwbarrier_align) ? bsp_section_rwbarrier_align : 1M;
+
+bsp_vector_table_in_start_section = 1;
+
+bsp_translation_table_base = ORIGIN (RAM_MMU);
+
+bsp_processor_count = DEFINED (bsp_processor_count) ? bsp_processor_count : 8;
+
+INCLUDE linkcmds.armv4

c/src/lib/libbsp/arm/shared/arm-a9mpcore-smp.c

@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2013 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <info@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <assert.h>
+
+#include <rtems/bspsmp.h>
+
+#include <libcpu/arm-cp15.h>
+
+#include <bsp/irq.h>
+
+int bsp_smp_processor_id(void)
+{
+  return (int) arm_cortex_a9_get_multiprocessor_cpu_id();
+}
+
+static void ipi_handler(void *arg)
+{
+  rtems_smp_process_interrupt();
+}
+
+uint32_t bsp_smp_initialize(uint32_t configured_cpu_count)
+{
+  rtems_status_code sc;
+  uint32_t max_cpu_count = arm_gic_irq_processor_count();
+  uint32_t used_cpu_count = configured_cpu_count < max_cpu_count ?
+    configured_cpu_count : max_cpu_count;
+
+  sc = rtems_interrupt_handler_install(
+    ARM_GIC_IRQ_SGI_0,
+    "IPI",
+    RTEMS_INTERRUPT_UNIQUE,
+    ipi_handler,
+    NULL
+  );
+  assert(sc == RTEMS_SUCCESSFUL);
+
+  return used_cpu_count;
+}
+
+void bsp_smp_broadcast_interrupt(void)
+{
+  /*
+   * FIXME: This broadcasts the interrupt also to processors not used by RTEMS.
+   */
+  rtems_status_code sc = arm_gic_irq_generate_software_irq(
+    ARM_GIC_IRQ_SGI_0,
+    ARM_GIC_IRQ_SOFTWARE_IRQ_TO_ALL_EXCEPT_SELF,
+    0xff
+  );
+}
+
+void bsp_smp_interrupt_cpu(int cpu)
+{
+  rtems_status_code sc = arm_gic_irq_generate_software_irq(
+    ARM_GIC_IRQ_SGI_0,
+    ARM_GIC_IRQ_SOFTWARE_IRQ_TO_ALL_IN_LIST,
+    (uint8_t) (1U << cpu)
+  );
+}

c/src/lib/libbsp/arm/shared/arm-gic-irq.c

@@ -18,14 +18,12 @@
 
 #include <libcpu/arm-cp15.h>
 
-#include <bsp.h>
 #include <bsp/irq.h>
 #include <bsp/irq-generic.h>
+#include <bsp/start.h>
 
 #define GIC_CPUIF ((volatile gic_cpuif *) BSP_ARM_GIC_CPUIF_BASE)
 
-#define GIC_DIST ((volatile gic_dist *) BSP_ARM_GIC_DIST_BASE)
-
 #define PRIORITY_DEFAULT 128
 
 void bsp_interrupt_dispatch(void)
@@ -51,7 +49,7 @@ rtems_status_code bsp_interrupt_vector_enable(rtems_vector_number vector)
   rtems_status_code sc = RTEMS_SUCCESSFUL;
 
   if (bsp_interrupt_is_valid_vector(vector)) {
-    volatile gic_dist *dist = GIC_DIST;
+    volatile gic_dist *dist = ARM_GIC_DIST;
 
     gic_id_enable(dist, vector);
   } else {
@@ -66,7 +64,7 @@ rtems_status_code bsp_interrupt_vector_disable(rtems_vector_number vector)
   rtems_status_code sc = RTEMS_SUCCESSFUL;
 
   if (bsp_interrupt_is_valid_vector(vector)) {
-    volatile gic_dist *dist = GIC_DIST;
+    volatile gic_dist *dist = ARM_GIC_DIST;
 
     gic_id_disable(dist, vector);
   } else {
@@ -89,10 +87,15 @@ static inline uint32_t get_id_count(volatile gic_dist *dist)
 rtems_status_code bsp_interrupt_facility_initialize(void)
 {
   volatile gic_cpuif *cpuif = GIC_CPUIF;
-  volatile gic_dist *dist = GIC_DIST;
+  volatile gic_dist *dist = ARM_GIC_DIST;
   uint32_t id_count = get_id_count(dist);
   uint32_t id;
 
+  arm_cp15_set_exception_handler(
+    ARM_EXCEPTION_IRQ,
+    _ARMV4_Exception_interrupt
+  );
+
   for (id = 0; id < id_count; ++id) {
     gic_id_set_priority(dist, id, PRIORITY_DEFAULT);
   }
@@ -107,14 +110,25 @@ rtems_status_code bsp_interrupt_facility_initialize(void)
 
   dist->icddcr = GIC_DIST_ICDDCR_ENABLE;
 
-  arm_cp15_set_exception_handler(
-    ARM_EXCEPTION_IRQ,
-    _ARMV4_Exception_interrupt
-  );
-
   return RTEMS_SUCCESSFUL;
 }
 
+#ifdef RTEMS_SMP
+BSP_START_TEXT_SECTION void arm_gic_irq_initialize_secondary_cpu(void)
+{
+  volatile gic_cpuif *cpuif = GIC_CPUIF;
+  volatile gic_dist *dist = ARM_GIC_DIST;
+
+  while ((dist->icddcr & GIC_DIST_ICDDCR_ENABLE) == 0) {
+    /* Wait */
+  }
+
+  cpuif->iccpmr = GIC_CPUIF_ICCPMR_PRIORITY(0xff);
+  cpuif->iccbpr = GIC_CPUIF_ICCBPR_BINARY_POINT(0x0);
+  cpuif->iccicr = GIC_CPUIF_ICCICR_ENABLE;
+}
+#endif
+
 rtems_status_code arm_gic_irq_set_priority(
   rtems_vector_number vector,
   uint8_t priority
@@ -123,7 +137,7 @@ rtems_status_code arm_gic_irq_set_priority(
   rtems_status_code sc = RTEMS_SUCCESSFUL;
 
   if (bsp_interrupt_is_valid_vector(vector)) {
-    volatile gic_dist *dist = GIC_DIST;
+    volatile gic_dist *dist = ARM_GIC_DIST;
 
     gic_id_set_priority(dist, vector, priority);
   } else {
@@ -141,7 +155,7 @@ rtems_status_code arm_gic_irq_get_priority(
   rtems_status_code sc = RTEMS_SUCCESSFUL;
 
   if (bsp_interrupt_is_valid_vector(vector)) {
-    volatile gic_dist *dist = GIC_DIST;
+    volatile gic_dist *dist = ARM_GIC_DIST;
 
     *priority = gic_id_get_priority(dist, vector);
   } else {
@@ -150,24 +164,3 @@ rtems_status_code arm_gic_irq_get_priority(
 
   return sc;
 }
-
-rtems_status_code arm_gic_irq_generate_software_irq(
-  rtems_vector_number vector,
-  arm_gic_irq_software_irq_target_filter filter,
-  uint8_t targets
-)
-{
-  rtems_status_code sc = RTEMS_SUCCESSFUL;
-
-  if (vector < 16) {
-    volatile gic_dist *dist = GIC_DIST;
-
-    dist->icdsgir = GIC_DIST_ICDSGIR_TARGET_LIST_FILTER(filter)
-      | GIC_DIST_ICDSGIR_CPU_TARGET_LIST(targets)
-      | GIC_DIST_ICDSGIR_SGIINTID(vector);
-  } else {
-    sc = RTEMS_INVALID_ID;
-  }
-
-  return sc;
-}

c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-regs.h

@@ -19,7 +19,23 @@
 
 typedef struct {
   uint32_t ctrl;
+#define A9MPCORE_SCU_CTRL_SCU_EN BSP_BIT32(0)
+#define A9MPCORE_SCU_CTRL_ADDR_FLT_EN BSP_BIT32(1)
+#define A9MPCORE_SCU_CTRL_RAM_PAR_EN BSP_BIT32(2)
+#define A9MPCORE_SCU_CTRL_SCU_SPEC_LINE_FILL_EN BSP_BIT32(3)
+#define A9MPCORE_SCU_CTRL_FORCE_PORT_0_EN BSP_BIT32(4)
+#define A9MPCORE_SCU_CTRL_SCU_STANDBY_EN BSP_BIT32(5)
+#define A9MPCORE_SCU_CTRL_IC_STANDBY_EN BSP_BIT32(6)
   uint32_t cfg;
+#define A9MPCORE_SCU_CFG_CPU_COUNT(val) BSP_FLD32(val, 0, 1)
+#define A9MPCORE_SCU_CFG_CPU_COUNT_GET(reg) BSP_FLD32GET(reg, 0, 1)
+#define A9MPCORE_SCU_CFG_CPU_COUNT_SET(reg, val) BSP_FLD32SET(reg, val, 0, 1)
+#define A9MPCORE_SCU_CFG_SMP_MODE(val) BSP_FLD32(val, 4, 7)
+#define A9MPCORE_SCU_CFG_SMP_MODE_GET(reg) BSP_FLD32GET(reg, 4, 7)
+#define A9MPCORE_SCU_CFG_SMP_MODE_SET(reg, val) BSP_FLD32SET(reg, val, 4, 7)
+#define A9MPCORE_SCU_CFG_TAG_RAM_SIZE(val) BSP_FLD32(val, 8, 15)
+#define A9MPCORE_SCU_CFG_TAG_RAM_SIZE_GET(reg) BSP_FLD32GET(reg, 8, 15)
+#define A9MPCORE_SCU_CFG_TAG_RAM_SIZE_SET(reg, val) BSP_FLD32SET(reg, val, 8, 15)
   uint32_t pwrst;
   uint32_t invss;
   uint32_t reserved_10[12];

c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-start.h

@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2013 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <info@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#ifndef LIBBSP_ARM_SHARED_ARM_A9MPCORE_START_H
+#define LIBBSP_ARM_SHARED_ARM_A9MPCORE_START_H
+
+#include <rtems/bspsmp.h>
+
+#include <libcpu/arm-cp15.h>
+
+#include <bsp.h>
+#include <bsp/start.h>
+#include <bsp/arm-a9mpcore-regs.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+BSP_START_TEXT_SECTION static inline uint32_t
+arm_cp15_get_auxiliary_control(void);
+
+BSP_START_TEXT_SECTION static inline void
+arm_cp15_set_auxiliary_control(uint32_t val);
+
+BSP_START_TEXT_SECTION static inline arm_a9mpcore_start_hook_0(void)
+{
+#ifdef RTEMS_SMP
+  volatile a9mpcore_scu *scu = (volatile a9mpcore_scu *) BSP_ARM_A9MPCORE_SCU_BASE;
+  uint32_t cpu_id;
+  uint32_t actlr;
+
+  /* Enable Snoop Control Unit (SCU) */
+  scu->ctrl |= A9MPCORE_SCU_CTRL_SCU_EN;
+
+  /* Enable cache coherency support for this processor */
+  actlr = arm_cp15_get_auxiliary_control();
+  actlr |= ARM_CORTEX_A9_ACTL_SMP;
+  arm_cp15_set_auxiliary_control(actlr);
+
+  cpu_id = arm_cortex_a9_get_multiprocessor_cpu_id();
+  if (cpu_id != 0) {
+    if (cpu_id < rtems_configuration_get_maximum_processors()) {
+      uint32_t ctrl;
+
+      arm_gic_irq_initialize_secondary_cpu();
+
+      ctrl = arm_cp15_start_setup_mmu_and_cache(
+        0,
+        ARM_CP15_CTRL_AFE | ARM_CP15_CTRL_Z
+      );
+
+      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);
+
+      ctrl |= ARM_CP15_CTRL_I | ARM_CP15_CTRL_C | ARM_CP15_CTRL_M;
+      arm_cp15_set_control(ctrl);
+
+      rtems_smp_secondary_cpu_initialize();
+    } else {
+      /* FIXME: Shutdown processor */
+      while (1) {
+        __asm__ volatile ("wfi");
+      }
+    }
+  }
+#endif
+}
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* LIBBSP_ARM_SHARED_ARM_A9MPCORE_START_H */

c/src/lib/libbsp/arm/shared/include/arm-cp15-start.h

@@ -41,6 +41,12 @@ arm_cp15_cache_invalidate(void);
 BSP_START_TEXT_SECTION static inline void
 arm_cp15_tlb_invalidate(void);
 
+BSP_START_TEXT_SECTION static inline uint32_t
+arm_cp15_get_multiprocessor_affinity(void);
+
+BSP_START_TEXT_SECTION static inline uint32_t
+arm_cortex_a9_get_multiprocessor_cpu_id(void);
+
 typedef struct {
   uint32_t begin;
   uint32_t end;

c/src/lib/libbsp/arm/shared/include/arm-gic-irq.h

@@ -15,7 +15,8 @@
 #ifndef LIBBSP_ARM_SHARED_ARM_GIC_IRQ_H
 #define LIBBSP_ARM_SHARED_ARM_GIC_IRQ_H
 
-#include <rtems.h>
+#include <bsp.h>
+#include <bsp/arm-gic.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -37,6 +38,8 @@ extern "C" {
 #define ARM_GIC_IRQ_SGI_14 14
 #define ARM_GIC_IRQ_SGI_15 15
 
+#define ARM_GIC_DIST ((volatile gic_dist *) BSP_ARM_GIC_DIST_BASE)
+
 rtems_status_code arm_gic_irq_set_priority(
   rtems_vector_number vector,
   uint8_t priority
@@ -53,11 +56,35 @@ typedef enum {
   ARM_GIC_IRQ_SOFTWARE_IRQ_TO_SELF
 } arm_gic_irq_software_irq_target_filter;
 
-rtems_status_code arm_gic_irq_generate_software_irq(
+static inline rtems_status_code arm_gic_irq_generate_software_irq(
   rtems_vector_number vector,
   arm_gic_irq_software_irq_target_filter filter,
   uint8_t targets
-);
+)
+{
+  rtems_status_code sc = RTEMS_SUCCESSFUL;
+
+  if (vector <= ARM_GIC_IRQ_SGI_15) {
+    volatile gic_dist *dist = ARM_GIC_DIST;
+
+    dist->icdsgir = GIC_DIST_ICDSGIR_TARGET_LIST_FILTER(filter)
+      | GIC_DIST_ICDSGIR_CPU_TARGET_LIST(targets)
+      | GIC_DIST_ICDSGIR_SGIINTID(vector);
+  } else {
+    sc = RTEMS_INVALID_ID;
+  }
+
+  return sc;
+}
+
+static inline uint32_t arm_gic_irq_processor_count(void)
+{
+  volatile gic_dist *dist = ARM_GIC_DIST;
+
+  return GIC_DIST_ICDICTR_CPU_NUMBER_GET(dist->icdictr) + 1;
+}
+
+void arm_gic_irq_initialize_secondary_cpu(void);
 
 #ifdef __cplusplus
 }

c/src/lib/libbsp/arm/shared/start/start.S

@@ -31,6 +31,14 @@
 	.extern	boot_card
 	.extern	bsp_start_hook_0
 	.extern	bsp_start_hook_1
+	.extern bsp_stack_irq_end
+	.extern bsp_stack_fiq_end
+	.extern bsp_stack_abt_end
+	.extern bsp_stack_und_end
+	.extern bsp_stack_svc_end
+#ifdef RTEMS_SMP
+	.extern bsp_stack_all_size
+#endif
 	.extern	_ARMV4_Exception_undef_default
 	.extern	_ARMV4_Exception_swi_default
 	.extern	_ARMV4_Exception_data_abort_default
@@ -119,6 +127,16 @@ _start:
 	 * loader.
 	 */
 
+#ifdef RTEMS_SMP
+	/* Read MPIDR */
+	mrc	p15, 0, r0, c0, c0, 5
+
+	/* Calculate stack offset */
+	and	r0, #0xff
+	ldr	r1, =bsp_stack_all_size
+	mul	r1, r0
+#endif
+
 	/*
 	 * Set SVC mode, disable interrupts and enable ARM instructions.
 	 */
@@ -131,26 +149,41 @@ _start:
 	mov	r0, #(ARM_PSR_M_IRQ | ARM_PSR_I | ARM_PSR_F)
 	msr	cpsr, r0
 	ldr	sp, =bsp_stack_irq_end
+#ifdef RTEMS_SMP
+	add	sp, r1
+#endif
 
 	/* Enter FIQ mode and set up the FIQ stack pointer */
 	mov	r0, #(ARM_PSR_M_FIQ | ARM_PSR_I | ARM_PSR_F)
 	msr	cpsr, r0
 	ldr	sp, =bsp_stack_fiq_end
+#ifdef RTEMS_SMP
+	add	sp, r1
+#endif
 
 	/* Enter ABT mode and set up the ABT stack pointer */
 	mov	r0, #(ARM_PSR_M_ABT | ARM_PSR_I | ARM_PSR_F)
 	msr	cpsr, r0
 	ldr	sp, =bsp_stack_abt_end
+#ifdef RTEMS_SMP
+	add	sp, r1
+#endif
 
 	/* Enter UND mode and set up the UND stack pointer */
 	mov	r0, #(ARM_PSR_M_UND | ARM_PSR_I | ARM_PSR_F)
 	msr	cpsr, r0
 	ldr	sp, =bsp_stack_und_end
+#ifdef RTEMS_SMP
+	add	sp, r1
+#endif
 
 	/* Enter SVC mode and set up the SVC stack pointer */
 	mov	r0, #(ARM_PSR_M_SVC | ARM_PSR_I | ARM_PSR_F)
 	msr	cpsr, r0
 	ldr	sp, =bsp_stack_svc_end
+#ifdef RTEMS_SMP
+	add	sp, r1
+#endif
 
 	/* Stay in SVC mode */
 

c/src/lib/libbsp/arm/shared/startup/linkcmds.base

@@ -56,6 +56,10 @@ bsp_stack_und_size = ALIGN (bsp_stack_und_size, bsp_stack_align);
 bsp_stack_main_size = DEFINED (bsp_stack_main_size) ? bsp_stack_main_size : 0;
 bsp_stack_main_size = ALIGN (bsp_stack_main_size, bsp_stack_align);
 
+bsp_stack_all_size = bsp_stack_abt_size + bsp_stack_fiq_size + bsp_stack_irq_size + bsp_stack_svc_size + bsp_stack_und_size + bsp_stack_main_size;
+
+bsp_processor_count = DEFINED (bsp_processor_count) ? bsp_processor_count : 1;
+
 MEMORY {
 	UNEXPECTED_SECTIONS : ORIGIN = 0xffffffff, LENGTH = 0
 }
@@ -324,6 +328,10 @@ SECTIONS {
 		. = . + bsp_stack_main_size;
 		bsp_stack_main_end = .;
 
+		bsp_stack_secondary_processors_begin = .;
+		. = . + (bsp_processor_count - 1) * bsp_stack_all_size;
+		bsp_stack_secondary_processors_end = .;
+
 		*(.bsp_vector)
 
 		bsp_section_vector_end = .;

c/src/lib/libbsp/arm/xilinx-zynq/Makefile.am

@@ -35,6 +35,7 @@ include_bsp_HEADERS += ../../shared/tod.h
 include_bsp_HEADERS += ../shared/include/start.h
 include_bsp_HEADERS += ../shared/include/arm-a9mpcore-irq.h
 include_bsp_HEADERS += ../shared/include/arm-a9mpcore-regs.h
+include_bsp_HEADERS += ../shared/include/arm-a9mpcore-start.h
 include_bsp_HEADERS += ../shared/include/arm-cp15-start.h
 include_bsp_HEADERS += ../shared/include/arm-gic.h
 include_bsp_HEADERS += ../shared/include/arm-gic-irq.h
@@ -59,6 +60,8 @@ project_lib_DATA = start.$(OBJEXT)
 project_lib_DATA += startup/linkcmds
 
 EXTRA_DIST =
+EXTRA_DIST += startup/linkcmds.xilinx_zynq_a9_qemu
+EXTRA_DIST += startup/linkcmds.xilinx_zynq_a9_qemu_smp
 
 ###############################################################################
 #                  LibBSP                                                     #
@@ -121,6 +124,10 @@ libbsp_a_CPPFLAGS += -I$(srcdir)/../../../libcpu/arm/shared/include
 # Start hooks
 libbsp_a_SOURCES += startup/bspstarthooks.c
 
+if HAS_SMP
+libbsp_a_SOURCES += ../shared/arm-a9mpcore-smp.c
+endif
+
 ###############################################################################
 #                  Special Rules                                              #
 ###############################################################################

c/src/lib/libbsp/arm/xilinx-zynq/configure.ac

@@ -33,6 +33,9 @@ RTEMS_BSPOPTS_HELP([CLOCK_DRIVER_USE_FAST_IDLE],
 occurs while the IDLE thread is executing.  This can significantly reduce
 simulation times.])
 
+RTEMS_CHECK_SMP
+AM_CONDITIONAL(HAS_SMP,[test "$rtems_cv_HAS_SMP" = "yes"])
+
 RTEMS_BSP_CLEANUP_OPTIONS(0, 1)
 RTEMS_BSP_LINKCMDS
 

c/src/lib/libbsp/arm/xilinx-zynq/include/bsp.h

@@ -31,8 +31,14 @@
 extern "C" {
 #endif /* __cplusplus */
 
+#define BSP_ARM_A9MPCORE_SCU_BASE 0xf8f00000
+
+#define BSP_ARM_GIC_CPUIF_BASE 0xf8f00100
+
 #define BSP_ARM_A9MPCORE_PT_BASE 0xf8f00600
 
+#define BSP_ARM_GIC_DIST_BASE 0xf8f01000
+
 typedef enum {
   BSP_ARM_A9MPCORE_FATAL_CLOCK_IRQ_INSTALL,
   BSP_ARM_A9MPCORE_FATAL_CLOCK_IRQ_REMOVE

c/src/lib/libbsp/arm/xilinx-zynq/include/irq.h

@@ -91,10 +91,6 @@ extern "C" {
 #define BSP_INTERRUPT_VECTOR_MIN 0
 #define BSP_INTERRUPT_VECTOR_MAX 92
 
-#define BSP_ARM_GIC_CPUIF_BASE 0xf8f00100
-
-#define BSP_ARM_GIC_DIST_BASE 0xf8f01000
-
 #ifdef __cplusplus
 }
 #endif /* __cplusplus */

c/src/lib/libbsp/arm/xilinx-zynq/make/custom/xilinx_zynq_a9_qemu_smp.cfg

@@ -0,0 +1 @@
+include $(RTEMS_ROOT)/make/custom/xilinx_zynq_a9_qemu.cfg

c/src/lib/libbsp/arm/xilinx-zynq/preinstall.am

@@ -94,6 +94,10 @@ $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-regs.h: ../shared/include/arm-a9mpcore-regs.
 	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-regs.h
 PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-regs.h
 
+$(PROJECT_INCLUDE)/bsp/arm-a9mpcore-start.h: ../shared/include/arm-a9mpcore-start.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
+	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-start.h
+PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-start.h
+
 $(PROJECT_INCLUDE)/bsp/arm-cp15-start.h: ../shared/include/arm-cp15-start.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
 	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/arm-cp15-start.h
 PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/arm-cp15-start.h

c/src/lib/libbsp/arm/xilinx-zynq/startup/bspstarthooks.c

@@ -15,12 +15,14 @@
 #include <bsp.h>
 #include <bsp/start.h>
 #include <bsp/arm-cp15-start.h>
+#include <bsp/arm-a9mpcore-start.h>
 #include <bsp/linker-symbols.h>
 
-BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
+#ifdef RTEMS_SMP
-{
+  #define MMU_DATA_READ_WRITE ARMV7_MMU_DATA_READ_WRITE_SHAREABLE
-  /* Do nothing */
+#else
-}
+  #define MMU_DATA_READ_WRITE ARMV7_MMU_DATA_READ_WRITE_CACHED
+#endif
 
 BSP_START_DATA_SECTION static const arm_cp15_start_section_config
 zynq_mmu_config_table[] = {
@@ -31,7 +33,7 @@ zynq_mmu_config_table[] = {
   }, {
     .begin = (uint32_t) bsp_section_fast_data_begin,
     .end = (uint32_t) bsp_section_fast_data_end,
-    .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+    .flags = MMU_DATA_READ_WRITE
   }, {
     .begin = (uint32_t) bsp_section_start_begin,
     .end = (uint32_t) bsp_section_start_end,
@@ -39,7 +41,7 @@ zynq_mmu_config_table[] = {
   }, {
     .begin = (uint32_t) bsp_section_vector_begin,
     .end = (uint32_t) bsp_section_vector_end,
-    .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+    .flags = MMU_DATA_READ_WRITE
   }, {
     .begin = (uint32_t) bsp_section_text_begin,
     .end = (uint32_t) bsp_section_text_end,
@@ -51,19 +53,19 @@ zynq_mmu_config_table[] = {
   }, {
     .begin = (uint32_t) bsp_section_data_begin,
     .end = (uint32_t) bsp_section_data_end,
-    .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+    .flags = MMU_DATA_READ_WRITE
   }, {
     .begin = (uint32_t) bsp_section_bss_begin,
     .end = (uint32_t) bsp_section_bss_end,
-    .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+    .flags = MMU_DATA_READ_WRITE
   }, {
     .begin = (uint32_t) bsp_section_work_begin,
     .end = (uint32_t) bsp_section_work_end,
-    .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+    .flags = MMU_DATA_READ_WRITE
   }, {
     .begin = (uint32_t) bsp_section_stack_begin,
     .end = (uint32_t) bsp_section_stack_end,
-    .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+    .flags = MMU_DATA_READ_WRITE
   }, {
     .begin = 0xe0000000U,
     .end = 0xe0200000U,
@@ -91,6 +93,11 @@ BSP_START_TEXT_SECTION static void setup_mmu_and_cache(void)
   );
 }
 
+BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
+{
+  arm_a9mpcore_start_hook_0();
+}
+
 BSP_START_TEXT_SECTION void bsp_start_hook_1(void)
 {
   bsp_start_copy_sections();

c/src/lib/libbsp/arm/xilinx-zynq/startup/linkcmds.xilinx_zynq_a9_qemu_smp

@@ -0,0 +1,33 @@
+MEMORY {
+	RAM : ORIGIN = 0x00000000, LENGTH = 256M - 16k
+	RAM_MMU : ORIGIN = 0x0fffc000, LENGTH = 16k
+}
+
+REGION_ALIAS ("REGION_START", RAM);
+REGION_ALIAS ("REGION_VECTOR", RAM);
+REGION_ALIAS ("REGION_TEXT", RAM);
+REGION_ALIAS ("REGION_TEXT_LOAD", RAM);
+REGION_ALIAS ("REGION_RODATA", RAM);
+REGION_ALIAS ("REGION_RODATA_LOAD", RAM);
+REGION_ALIAS ("REGION_DATA", RAM);
+REGION_ALIAS ("REGION_DATA_LOAD", RAM);
+REGION_ALIAS ("REGION_FAST_TEXT", RAM);
+REGION_ALIAS ("REGION_FAST_TEXT_LOAD", RAM);
+REGION_ALIAS ("REGION_FAST_DATA", RAM);
+REGION_ALIAS ("REGION_FAST_DATA_LOAD", RAM);
+REGION_ALIAS ("REGION_BSS", RAM);
+REGION_ALIAS ("REGION_WORK", RAM);
+REGION_ALIAS ("REGION_STACK", RAM);
+
+bsp_stack_irq_size = DEFINED (bsp_stack_irq_size) ? bsp_stack_irq_size : 4096;
+bsp_stack_abt_size = DEFINED (bsp_stack_abt_size) ? bsp_stack_abt_size : 1024;
+
+bsp_section_rwbarrier_align = DEFINED (bsp_section_rwbarrier_align) ? bsp_section_rwbarrier_align : 1M;
+
+bsp_vector_table_in_start_section = 1;
+
+bsp_translation_table_base = ORIGIN (RAM_MMU);
+
+bsp_processor_count = DEFINED (bsp_processor_count) ? bsp_processor_count : 8;
+
+INCLUDE linkcmds.armv4

c/src/lib/libcpu/arm/shared/include/arm-cp15.h

@@ -842,6 +842,63 @@ static inline void arm_cp15_wait_for_interrupt(void)
   );
 }
 
+static inline uint32_t arm_cp15_get_multiprocessor_affinity(void)
+{
+  ARM_SWITCH_REGISTERS;
+  uint32_t mpidr;
+
+  __asm__ volatile (
+    ARM_SWITCH_TO_ARM
+	  "mrc p15, 0, %[mpidr], c0, c0, 5\n"
+    ARM_SWITCH_BACK
+    : [mpidr] "=&r" (mpidr) ARM_SWITCH_ADDITIONAL_OUTPUT
+  );
+
+  return mpidr & 0xff;
+}
+
+static inline uint32_t arm_cortex_a9_get_multiprocessor_cpu_id(void)
+{
+  return arm_cp15_get_multiprocessor_affinity() & 0xff;
+}
+
+#define ARM_CORTEX_A9_ACTL_FW (1U << 0)
+#define ARM_CORTEX_A9_ACTL_L2_PREFETCH_HINT_ENABLE (1U << 1)
+#define ARM_CORTEX_A9_ACTL_L1_PREFETCH_ENABLE (1U << 2)
+#define ARM_CORTEX_A9_ACTL_WRITE_FULL_LINE_OF_ZEROS_MODE (1U << 3)
+#define ARM_CORTEX_A9_ACTL_SMP (1U << 6)
+#define ARM_CORTEX_A9_ACTL_EXCL (1U << 7)
+#define ARM_CORTEX_A9_ACTL_ALLOC_IN_ONE_WAY (1U << 8)
+#define ARM_CORTEX_A9_ACTL_PARITY_ON (1U << 9)
+
+static inline uint32_t arm_cp15_get_auxiliary_control(void)
+{
+  ARM_SWITCH_REGISTERS;
+  uint32_t val;
+
+  __asm__ volatile (
+    ARM_SWITCH_TO_ARM
+    "mrc p15, 0, %[val], c1, c0, 1\n"
+    ARM_SWITCH_BACK
+    : [val] "=&r" (val) ARM_SWITCH_ADDITIONAL_OUTPUT
+  );
+
+  return val;
+}
+
+static inline void arm_cp15_set_auxiliary_control(uint32_t val)
+{
+  ARM_SWITCH_REGISTERS;
+
+  __asm__ volatile (
+    ARM_SWITCH_TO_ARM
+    "mcr p15, 0, %[val], c1, c0, 1\n"
+    ARM_SWITCH_BACK
+    : ARM_SWITCH_OUTPUT
+    : [val] "r" (val)
+  );
+}
+
 /**
  * @brief Sets the @a section_flags for the address range [@a begin, @a end).
  *