score: Distribute clock tick to all online CPUs

Update #2554.

13 files changed, 175 insertions, 24 deletions

diff --git a/c/src/lib/libbsp/arm/shared/arm-a9mpcore-clock-config.c b/c/src/lib/libbsp/arm/shared/arm-a9mpcore-clock-config.c
index 8e2e153b46..3dcf708bcc 100644
--- a/c/src/lib/libbsp/arm/shared/arm-a9mpcore-clock-config.c
+++ b/c/src/lib/libbsp/arm/shared/arm-a9mpcore-clock-config.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2013, 2016 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -15,9 +15,11 @@
 #include <bsp.h>
 #include <bsp/fatal.h>
 #include <bsp/irq.h>
+#include <bsp/irq-generic.h>
 #include <bsp/arm-a9mpcore-regs.h>
 #include <bsp/arm-a9mpcore-clock.h>
 #include <rtems/timecounter.h>
+#include <rtems/score/smpimpl.h>
 
 #define A9MPCORE_GT ((volatile a9mpcore_gt *) BSP_ARM_A9MPCORE_GT_BASE)
 
@@ -77,6 +79,60 @@ static uint32_t a9mpcore_clock_get_timecount(struct timecounter *tc)
   return gt->cntrlower;
 }
 
+static void a9mpcore_clock_gt_init(
+  volatile a9mpcore_gt *gt,
+  uint64_t cmpval,
+  uint32_t interval
+)
+{
+  gt->cmpvallower = (uint32_t) cmpval;
+  gt->cmpvalupper = (uint32_t) (cmpval >> 32);
+  gt->autoinc = interval;
+  gt->ctrl = A9MPCORE_GT_CTRL_AUTOINC_EN
+    | A9MPCORE_GT_CTRL_IRQ_EN
+    | A9MPCORE_GT_CTRL_COMP_EN
+    | A9MPCORE_GT_CTRL_TMR_EN;
+}
+
+#ifdef RTEMS_SMP
+typedef struct {
+  uint64_t cmpval;
+  uint32_t interval;
+} a9mpcore_clock_init_data;
+
+static void a9mpcore_clock_secondary_action(void *arg)
+{
+  volatile a9mpcore_gt *gt = A9MPCORE_GT;
+  a9mpcore_clock_init_data *init_data = arg;
+
+  a9mpcore_clock_gt_init(gt, init_data->cmpval, init_data->interval);
+  bsp_interrupt_vector_enable(A9MPCORE_IRQ_GT);
+}
+#endif
+
+static void a9mpcore_clock_secondary_initialization(
+  volatile a9mpcore_gt *gt,
+  uint64_t cmpval,
+  uint32_t interval
+)
+{
+#ifdef RTEMS_SMP
+  a9mpcore_clock_init_data init_data = {
+    .cmpval = cmpval,
+    .interval = interval
+  };
+
+  _SMP_Before_multitasking_action_broadcast(
+    a9mpcore_clock_secondary_action,
+    &init_data
+  );
+
+  if (cmpval - a9mpcore_clock_get_counter(gt) >= interval) {
+    bsp_fatal(BSP_ARM_A9MPCORE_FATAL_CLOCK_SMP_INIT);
+  }
+#endif
+}
+
 static void a9mpcore_clock_initialize(void)
 {
   volatile a9mpcore_gt *gt = A9MPCORE_GT;
@@ -91,14 +147,8 @@ static void a9mpcore_clock_initialize(void)
   cmpval = a9mpcore_clock_get_counter(gt);
   cmpval += interval;
 
-  gt->cmpvallower = (uint32_t) cmpval;
-  gt->cmpvalupper = (uint32_t) (cmpval >> 32);
-  gt->autoinc = interval;
-
-  gt->ctrl = A9MPCORE_GT_CTRL_AUTOINC_EN
-    | A9MPCORE_GT_CTRL_IRQ_EN
-    | A9MPCORE_GT_CTRL_COMP_EN
-    | A9MPCORE_GT_CTRL_TMR_EN;
+  a9mpcore_clock_gt_init(gt, cmpval, interval);
+  a9mpcore_clock_secondary_initialization(gt, cmpval, interval);
 
   a9mpcore_tc.tc_get_timecount = a9mpcore_clock_get_timecount;
   a9mpcore_tc.tc_counter_mask = 0xffffffff;
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 7623489c35..487ee16f72 100644
--- a/c/src/lib/libbsp/arm/shared/arm-gic-irq.c
+++ b/c/src/lib/libbsp/arm/shared/arm-gic-irq.c
@@ -164,3 +164,21 @@ rtems_status_code arm_gic_irq_get_priority(
 
   return sc;
 }
+
+rtems_status_code arm_gic_irq_set_affinity(
+  rtems_vector_number vector,
+  uint8_t targets
+)
+{
+  rtems_status_code sc = RTEMS_SUCCESSFUL;
+
+  if (bsp_interrupt_is_valid_vector(vector)) {
+    volatile gic_dist *dist = ARM_GIC_DIST;
+
+    gic_id_set_targets(dist, vector, targets);
+  } else {
+    sc = RTEMS_INVALID_ID;
+  }
+
+  return sc;
+}
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 7765c003ae..a8c29bb7c5 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
@@ -58,6 +58,11 @@ rtems_status_code arm_gic_irq_get_priority(
   uint8_t *priority
 );
 
+rtems_status_code arm_gic_irq_set_affinity(
+  rtems_vector_number vector,
+  uint8_t targets
+);
+
 typedef enum {
   ARM_GIC_IRQ_SOFTWARE_IRQ_TO_ALL_IN_LIST,
   ARM_GIC_IRQ_SOFTWARE_IRQ_TO_ALL_EXCEPT_SELF,
diff --git a/c/src/lib/libbsp/i386/pc386/clock/ckinit.c b/c/src/lib/libbsp/i386/pc386/clock/ckinit.c
index 04514d19fa..2291839dd5 100644
--- a/c/src/lib/libbsp/i386/pc386/clock/ckinit.c
+++ b/c/src/lib/libbsp/i386/pc386/clock/ckinit.c
@@ -65,11 +65,10 @@ extern volatile uint32_t Clock_driver_ticks;
   } while (0)
 
 
-/*
- *  Hooks which get swapped based upon which nanoseconds since last
- *  tick method is preferred.
- */
-#define Clock_driver_support_at_tick()
+#ifdef RTEMS_SMP
+#define Clock_driver_support_at_tick() \
+  _SMP_Send_message_broadcast(SMP_MESSAGE_CLOCK_TICK)
+#endif
 
 #define Clock_driver_support_install_isr( _new, _old ) \
   do { \
@@ -203,6 +202,12 @@ void Clock_driver_install_handler(void)
   clockOn();
 }
 
+#define Clock_driver_support_set_interrupt_affinity(online_processors) \
+  do { \
+    /* FIXME: Is there a way to do this on x86? */ \
+    (void) online_processors; \
+  } while (0)
+
 void Clock_driver_support_initialize_hardware(void)
 {
   bool use_tsc = false;
diff --git a/c/src/lib/libbsp/i386/shared/smp/smp-imps.c b/c/src/lib/libbsp/i386/shared/smp/smp-imps.c
index ca1a0f134a..1ed504ccf9 100644
--- a/c/src/lib/libbsp/i386/shared/smp/smp-imps.c
+++ b/c/src/lib/libbsp/i386/shared/smp/smp-imps.c
@@ -744,13 +744,22 @@ static void smp_apic_ack(void)
   IMPS_LAPIC_WRITE(LAPIC_EOI, 0 );     /* ACK the interrupt */
 }
 
+/* FIXME: There should be a header file for this */
+void Clock_isr(void *arg);
+
 static void bsp_inter_processor_interrupt(void *arg)
 {
+  unsigned long message;
+
   (void) arg;
 
   smp_apic_ack();
 
-  _SMP_Inter_processor_interrupt_handler();
+  message = _SMP_Inter_processor_interrupt_handler();
+
+  if ((message & SMP_MESSAGE_CLOCK_TICK) != 0) {
+    Clock_isr(NULL);
+  }
 }
 
 static void ipi_install_irq(void)
diff --git a/c/src/lib/libbsp/powerpc/qoriq/clock/clock-config.c b/c/src/lib/libbsp/powerpc/qoriq/clock/clock-config.c
index 8da927f1f3..17d4bde3cb 100644
--- a/c/src/lib/libbsp/powerpc/qoriq/clock/clock-config.c
+++ b/c/src/lib/libbsp/powerpc/qoriq/clock/clock-config.c
@@ -7,7 +7,7 @@
  */
 
 /*
- * Copyright (c) 2011-2015 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2011, 2016 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -55,6 +55,7 @@ static void qoriq_clock_handler_install(rtems_isr_entry *old_isr)
 
   *old_isr = NULL;
 
+#if defined(RTEMS_MULTIPROCESSING) && !defined(RTEMS_SMP)
   sc = qoriq_pic_set_affinity(
     CLOCK_INTERRUPT,
     ppc_processor_id()
@@ -62,6 +63,7 @@ static void qoriq_clock_handler_install(rtems_isr_entry *old_isr)
   if (sc != RTEMS_SUCCESSFUL) {
     rtems_fatal_error_occurred(0xdeadbeef);
   }
+#endif
 
   sc = qoriq_pic_set_priority(
     CLOCK_INTERRUPT,
@@ -126,8 +128,13 @@ static void qoriq_clock_cleanup(void)
 
 #define Clock_driver_support_initialize_hardware() \
   qoriq_clock_initialize()
+
 #define Clock_driver_support_install_isr(clock_isr, old_isr) \
   qoriq_clock_handler_install(&old_isr)
+
+#define Clock_driver_support_set_interrupt_affinity(online_processors) \
+  qoriq_pic_set_affinities(CLOCK_INTERRUPT, online_processors[0])
+
 #define Clock_driver_support_shutdown_hardware() \
   qoriq_clock_cleanup()
 
diff --git a/c/src/lib/libbsp/powerpc/qoriq/include/irq.h b/c/src/lib/libbsp/powerpc/qoriq/include/irq.h
index 1363ec3580..fb7be65cca 100644
--- a/c/src/lib/libbsp/powerpc/qoriq/include/irq.h
+++ b/c/src/lib/libbsp/powerpc/qoriq/include/irq.h
@@ -377,6 +377,11 @@ rtems_status_code qoriq_pic_set_affinity(
   uint32_t processor_index
 );
 
+rtems_status_code qoriq_pic_set_affinities(
+  rtems_vector_number vector,
+  uint32_t processor_affinities
+);
+
 /** @} */
 
 #ifdef __cplusplus
diff --git a/c/src/lib/libbsp/powerpc/qoriq/irq/irq.c b/c/src/lib/libbsp/powerpc/qoriq/irq/irq.c
index 6bc70ce6d0..da7def3913 100644
--- a/c/src/lib/libbsp/powerpc/qoriq/irq/irq.c
+++ b/c/src/lib/libbsp/powerpc/qoriq/irq/irq.c
@@ -147,21 +147,17 @@ rtems_status_code qoriq_pic_set_priority(
 	return sc;
 }
 
-rtems_status_code qoriq_pic_set_affinity(
+rtems_status_code qoriq_pic_set_affinities(
 	rtems_vector_number vector,
-	uint32_t processor_index
+	uint32_t processor_affinities
 )
 {
 	rtems_status_code sc = RTEMS_SUCCESSFUL;
 
 	if (bsp_interrupt_is_valid_vector(vector)) {
-		if (processor_index <= 1) {
-			volatile qoriq_pic_src_cfg *src_cfg = get_src_cfg(vector);
+		volatile qoriq_pic_src_cfg *src_cfg = get_src_cfg(vector);
 
-			src_cfg->dr = BSP_BIT32(processor_index);
-		} else {
-			sc = RTEMS_INVALID_NUMBER;
-		}
+		src_cfg->dr = processor_affinities;
 	} else {
 		sc = RTEMS_INVALID_ID;
 	}
@@ -169,6 +165,14 @@ rtems_status_code qoriq_pic_set_affinity(
 	return sc;
 }
 
+rtems_status_code qoriq_pic_set_affinity(
+	rtems_vector_number vector,
+	uint32_t processor_index
+)
+{
+	return qoriq_pic_set_affinities(vector, BSP_BIT32(processor_index));
+}
+
 static rtems_status_code pic_vector_enable(rtems_vector_number vector, uint32_t msk)
 {
 	rtems_status_code sc = RTEMS_SUCCESSFUL;
diff --git a/c/src/lib/libbsp/shared/clockdrv_shell.h b/c/src/lib/libbsp/shared/clockdrv_shell.h
index 9bf5d9b73a..af038610a6 100644
--- a/c/src/lib/libbsp/shared/clockdrv_shell.h
+++ b/c/src/lib/libbsp/shared/clockdrv_shell.h
@@ -20,6 +20,7 @@
 #include <bsp.h>
 #include <rtems/clockdrv.h>
 #include <rtems/score/percpu.h>
+#include <rtems/score/smpimpl.h>
 
 #ifdef Clock_driver_nanoseconds_since_last_tick
 #error "Update driver to use the timecounter instead of nanoseconds extension"
@@ -51,6 +52,13 @@
   #define Clock_driver_support_at_tick()
 #endif
 
+/**
+ * @brief Do nothing by default.
+ */
+#ifndef Clock_driver_support_set_interrupt_affinity
+  #define Clock_driver_support_set_interrupt_affinity(online_processors)
+#endif
+
 /*
  * A specialized clock driver may use for example rtems_timecounter_tick_simple()
  * instead of the default.
@@ -199,6 +207,10 @@ rtems_device_driver Clock_initialize(
   (void) Old_ticker;
   Clock_driver_support_install_isr( Clock_isr, Old_ticker );
 
+  #ifdef RTEMS_SMP
+    Clock_driver_support_set_interrupt_affinity( _SMP_Online_processors );
+  #endif
+
   /*
    *  Now initialize the hardware that is the source of the tick ISR.
    */
diff --git a/c/src/lib/libbsp/shared/include/fatal.h b/c/src/lib/libbsp/shared/include/fatal.h
index 6c390d0834..25dda3933a 100644
--- a/c/src/lib/libbsp/shared/include/fatal.h
+++ b/c/src/lib/libbsp/shared/include/fatal.h
@@ -47,6 +47,7 @@ typedef enum {
   BSP_ARM_PL111_FATAL_REGISTER_DEV,
   BSP_ARM_PL111_FATAL_SEM_CREATE,
   BSP_ARM_PL111_FATAL_SEM_RELEASE,
+  BSP_ARM_A9MPCORE_FATAL_CLOCK_SMP_INIT,
 
   /* LEON3 fatal codes */
   LEON3_FATAL_NO_IRQMP_CONTROLLER = BSP_FATAL_CODE_BLOCK(2),
diff --git a/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c b/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c
index f2d493e73d..d78fb0ea5d 100644
--- a/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c
+++ b/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c
@@ -41,6 +41,11 @@
     _old = set_vector( _new, CLOCK_VECTOR, 1 ); \
   } while(0)
 
+#define Clock_driver_support_set_interrupt_affinity( _online_processors ) \
+  do { \
+    (void) _online_processors; \
+  } while (0)
+
 extern int CLOCK_SPEED;
 
 static rtems_timecounter_simple erc32_tc;
diff --git a/c/src/lib/libbsp/sparc/leon3/clock/ckinit.c b/c/src/lib/libbsp/sparc/leon3/clock/ckinit.c
index 5d645dcd36..4ef29d33ef 100644
--- a/c/src/lib/libbsp/sparc/leon3/clock/ckinit.c
+++ b/c/src/lib/libbsp/sparc/leon3/clock/ckinit.c
@@ -155,6 +155,18 @@ static void bsp_clock_handler_install(rtems_isr *new)
   }
 }
 
+#define Clock_driver_support_set_interrupt_affinity(online_processors) \
+  do { \
+    uint32_t cpu_count = _SMP_Processor_count; \
+    uint32_t cpu_index; \
+    LEON_Enable_interrupt_broadcast(clkirq); \
+    for (cpu_index = 0; cpu_index < cpu_count; ++cpu_index) { \
+      if (_Processor_mask_Is_set(online_processors, cpu_index)) { \
+        BSP_Cpu_Unmask_interrupt(clkirq, cpu_index); \
+      } \
+    } \
+  } while (0)
+
 static void leon3_clock_initialize(void)
 {
   volatile struct irqmp_timestamp_regs *irqmp_ts =
diff --git a/c/src/lib/libbsp/sparc/leon3/include/leon.h b/c/src/lib/libbsp/sparc/leon3/include/leon.h
index 5ac1d71872..bbf94d90b3 100644
--- a/c/src/lib/libbsp/sparc/leon3/include/leon.h
+++ b/c/src/lib/libbsp/sparc/leon3/include/leon.h
@@ -194,6 +194,24 @@ extern rtems_interrupt_lock LEON3_IrqCtrl_Lock;
     LEON3_IrqCtrl_Regs->iforce = (1 << (_source)); \
   } while (0)
 
+#define LEON_Enable_interrupt_broadcast( _source ) \
+  do { \
+    rtems_interrupt_lock_context _lock_context; \
+    uint32_t _mask = 1U << ( _source ); \
+    LEON3_IRQCTRL_ACQUIRE( &_lock_context ); \
+    LEON3_IrqCtrl_Regs->bcast |= _mask; \
+    LEON3_IRQCTRL_RELEASE( &_lock_context ); \
+  } while (0)
+
+#define LEON_Disable_interrupt_broadcast( _source ) \
+  do { \
+    rtems_interrupt_lock_context _lock_context; \
+    uint32_t _mask = 1U << ( _source ); \
+    LEON3_IRQCTRL_ACQUIRE( &_lock_context ); \
+    LEON3_IrqCtrl_Regs->bcast &= ~_mask; \
+    LEON3_IRQCTRL_RELEASE( &_lock_context ); \
+  } while (0)
+
 #define LEON_Is_interrupt_pending( _source ) \
   (LEON3_IrqCtrl_Regs->ipend & (1 << (_source)))