score: Distribute clock tick to all online CPUs

Update #2554.

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;

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;
+}

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,

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;

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)

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()
 

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

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;

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.
    */

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),

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;

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 =

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)))
 

cpukit/score/include/rtems/score/smpimpl.h

@@ -59,6 +59,16 @@ extern "C" {
  */
 #define SMP_MESSAGE_MULTICAST_ACTION 0x4UL
 
+/**
+ * @brief SMP message to request a clock tick.
+ *
+ * This message is provided for systems without a proper interrupt affinity
+ * support and may be used by the clock driver.
+ *
+ * @see _SMP_Send_message().
+ */
+#define SMP_MESSAGE_CLOCK_TICK 0x8UL
+
 /**
  * @brief SMP fatal codes.
  */
@@ -152,10 +162,13 @@ void _SMP_Multicast_actions_process( void );
 
 /**
  * @brief Interrupt handler for inter-processor interrupts.
+ *
+ * @return The received message.
  */
-static inline void _SMP_Inter_processor_interrupt_handler( void )
+static inline long unsigned _SMP_Inter_processor_interrupt_handler( void )
 {
   Per_CPU_Control *cpu_self = _Per_CPU_Get();
+  unsigned long message = 0;
 
   /*
    * In the common case the inter-processor interrupt is issued to carry out a
@@ -164,7 +177,7 @@ static inline void _SMP_Inter_processor_interrupt_handler( void )
   cpu_self->dispatch_necessary = true;
 
   if ( _Atomic_Load_ulong( &cpu_self->message, ATOMIC_ORDER_RELAXED ) != 0 ) {
-    unsigned long message = _Atomic_Exchange_ulong(
+    message = _Atomic_Exchange_ulong(
       &cpu_self->message,
       0UL,
       ATOMIC_ORDER_RELAXED
@@ -183,6 +196,8 @@ static inline void _SMP_Inter_processor_interrupt_handler( void )
       _SMP_Multicast_actions_process();
     }
   }
+
+  return message;
 }
 
 /**
@@ -197,7 +212,7 @@ static inline void _SMP_Inter_processor_interrupt_handler( void )
 bool _SMP_Should_start_processor( uint32_t cpu_index );
 
 /**
- *  @brief Sends a SMP message to a processor.
+ *  @brief Sends an SMP message to a processor.
  *
  *  The target processor may be the sending processor.
  *
@@ -207,21 +222,16 @@ bool _SMP_Should_start_processor( uint32_t cpu_index );
 void _SMP_Send_message( uint32_t cpu_index, unsigned long message );
 
 /**
- *  @brief Request of others CPUs.
+ *  @brief Sends an SMP message to all other online processors.
  *
- *  This method is invoked by RTEMS when it needs to make a request
- *  of the other CPUs.  It should be implemented using some type of
- *  interprocessor interrupt. CPUs not including the originating
- *  CPU should receive the message.
- *
- *  @param [in] message is message to send
+ *  @param[in] message The message.
  */
 void _SMP_Send_message_broadcast(
   unsigned long message
 );
 
 /**
- *  @brief Sends a SMP message to a set of processors.
+ *  @brief Sends an SMP message to a set of processors.
  *
  *  The sending processor may be part of the set.
  *
@@ -238,7 +248,7 @@ void _SMP_Send_message_multicast(
 typedef void ( *SMP_Action_handler )( void *arg );
 
 /**
- *  @brief Initiates a SMP multicast action to a set of processors.
+ *  @brief Initiates an SMP multicast action to a set of processors.
  *
  *  The current processor may be part of the set.
  *

cpukit/score/include/rtems/score/watchdogimpl.h

@@ -23,6 +23,7 @@
 #include <rtems/score/assert.h>
 #include <rtems/score/chainimpl.h>
 #include <rtems/score/isrlock.h>
+#include <rtems/score/percpu.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -139,11 +140,11 @@ RTEMS_INLINE_ROUTINE void _Watchdog_Flash(
 void _Watchdog_Handler_initialization( void );
 
 /**
- *  @brief Triggers a watchdog tick.
+ *  @brief Performs a watchdog tick.
  *
- *  This routine executes TOD, watchdog and scheduler ticks.
+ *  @param cpu The processor for this watchdog tick.
  */
-void _Watchdog_Tick( void );
+void _Watchdog_Tick( Per_CPU_Control *cpu );
 
 /**
  *  @brief Removes @a the_watchdog from the watchdog chain.

cpukit/score/src/kern_tc.c

@@ -1974,10 +1974,15 @@ tc_ticktock(int cnt)
 void
 _Timecounter_Tick(void)
 {
+	Per_CPU_Control *cpu_self = _Per_CPU_Get();
+
+	if (_Per_CPU_Is_boot_processor(cpu_self)) {
 #endif /* __rtems__ */
 	tc_windup();
 #ifdef __rtems__
-	_Watchdog_Tick();
+	};
+
+	_Watchdog_Tick(cpu_self);
 #endif /* __rtems__ */
 }
 #ifdef __rtems__
@@ -2016,7 +2021,7 @@ _Timecounter_Tick_simple(uint32_t delta, uint32_t offset,
 
 	_Timecounter_Release(lock_context);
 
-	_Watchdog_Tick();
+	_Watchdog_Tick(_Per_CPU_Get());
 }
 #endif /* __rtems__ */
 

cpukit/score/src/smp.c

@@ -189,7 +189,10 @@ void _SMP_Send_message_broadcast( unsigned long message )
   _Assert( _Debug_Is_thread_dispatching_allowed() );
 
   for ( cpu_index = 0 ; cpu_index < cpu_count ; ++cpu_index ) {
-    if ( cpu_index != cpu_index_self ) {
+    if (
+      cpu_index != cpu_index_self
+        && _Processor_mask_Is_set( _SMP_Online_processors, cpu_index )
+    ) {
       _SMP_Send_message( cpu_index, message );
     }
   }

cpukit/score/src/watchdogtick.c

@@ -22,13 +22,15 @@
 #include "config.h"
 #endif
 
-void _Watchdog_Tick( void )
+void _Watchdog_Tick( Per_CPU_Control *cpu )
 {
   _Assert( !_Thread_Dispatch_is_enabled() );
 
-  _TOD_Tickle_ticks();
+  if ( _Per_CPU_Is_boot_processor( cpu ) ) {
+    _TOD_Tickle_ticks();
 
-  _Watchdog_Tickle_ticks();
+    _Watchdog_Tickle_ticks();
 
-  _Scheduler_Tick();
+    _Scheduler_Tick();
+  }
 }