bsps/arm: Add SMP support

8 files changed, 279 insertions, 37 deletions

diff --git a/c/src/lib/libbsp/arm/shared/arm-a9mpcore-smp.c b/c/src/lib/libbsp/arm/shared/arm-a9mpcore-smp.c
new file mode 100644
index 0000000000..b94bcec81c
--- /dev/null
+++ b/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)
+  );
+}
diff --git a/c/src/lib/libbsp/arm/shared/arm-gic-irq.c b/c/src/lib/libbsp/arm/shared/arm-gic-irq.c
index 84d66aa0fb..ed2d389589 100644
--- a/c/src/lib/libbsp/arm/shared/arm-gic-irq.c
+++ b/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;
-}
diff --git a/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-regs.h b/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-regs.h
index 7d8f637bad..dc7fd758ed 100644
--- a/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-regs.h
+++ b/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];
diff --git a/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-start.h b/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-start.h
new file mode 100644
index 0000000000..b42c549c91
--- /dev/null
+++ b/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 */
diff --git a/c/src/lib/libbsp/arm/shared/include/arm-cp15-start.h b/c/src/lib/libbsp/arm/shared/include/arm-cp15-start.h
index 6fb822667c..11efbcd400 100644
--- a/c/src/lib/libbsp/arm/shared/include/arm-cp15-start.h
+++ b/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;
diff --git a/c/src/lib/libbsp/arm/shared/include/arm-gic-irq.h b/c/src/lib/libbsp/arm/shared/include/arm-gic-irq.h
index c45d3545d9..5c152a179e 100644
--- a/c/src/lib/libbsp/arm/shared/include/arm-gic-irq.h
+++ b/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
 }
diff --git a/c/src/lib/libbsp/arm/shared/start/start.S b/c/src/lib/libbsp/arm/shared/start/start.S
index e79a5fc503..7b59ab4857 100644
--- a/c/src/lib/libbsp/arm/shared/start/start.S
+++ b/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 */
 
diff --git a/c/src/lib/libbsp/arm/shared/startup/linkcmds.base b/c/src/lib/libbsp/arm/shared/startup/linkcmds.base
index 27f5acb125..d1a73d009a 100644
--- a/c/src/lib/libbsp/arm/shared/startup/linkcmds.base
+++ b/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 = .;