Update #2554. --- .../libbsp/arm/shared/arm-a9mpcore-clock-config.c | 68 +++++++++++++++++++--- c/src/lib/libbsp/arm/shared/arm-gic-irq.c | 18 ++++++ c/src/lib/libbsp/arm/shared/include/arm-gic-irq.h | 5 ++ c/src/lib/libbsp/i386/pc386/clock/ckinit.c | 15 +++-- c/src/lib/libbsp/i386/shared/smp/smp-imps.c | 11 +++- .../lib/libbsp/powerpc/qoriq/clock/clock-config.c | 9 ++- c/src/lib/libbsp/powerpc/qoriq/include/irq.h | 5 ++ c/src/lib/libbsp/powerpc/qoriq/irq/irq.c | 20 ++++--- c/src/lib/libbsp/shared/clockdrv_shell.h | 12 ++++ c/src/lib/libbsp/shared/include/fatal.h | 1 + c/src/lib/libbsp/sparc/erc32/clock/ckinit.c | 5 ++ c/src/lib/libbsp/sparc/leon3/clock/ckinit.c | 11 ++++ cpukit/score/include/rtems/score/smpimpl.h | 34 +++++++---- cpukit/score/include/rtems/score/watchdogimpl.h | 7 ++- cpukit/score/src/kern_tc.c | 9 ++- cpukit/score/src/smp.c | 5 +- cpukit/score/src/watchdogtick.c | 10 ++-- 17 files changed, 199 insertions(+), 46 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 8e2e153..3dcf708 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 7623489..487ee16 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 7765c00..a8c29bb 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 04514d1..2291839 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 158a45d..819c65d 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 8da927f..17d4bde 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 1363ec3..fb7be65 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 6bc70ce..da7def3 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 9bf5d9b..af03861 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 6c390d0..25dda39 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 f2d493e..d78fb0e 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 5d645dc..601d329 100644 --- a/c/src/lib/libbsp/sparc/leon3/clock/ckinit.c +++ b/c/src/lib/libbsp/sparc/leon3/clock/ckinit.c @@ -155,6 +155,17 @@ 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; \ + 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/cpukit/score/include/rtems/score/smpimpl.h b/cpukit/score/include/rtems/score/smpimpl.h index 59a99ec..d1b7214 100644 --- a/cpukit/score/include/rtems/score/smpimpl.h +++ b/cpukit/score/include/rtems/score/smpimpl.h @@ -60,6 +60,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. */ typedef enum { @@ -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. * diff --git a/cpukit/score/include/rtems/score/watchdogimpl.h b/cpukit/score/include/rtems/score/watchdogimpl.h index 8064c77..49ac2a1 100644 --- a/cpukit/score/include/rtems/score/watchdogimpl.h +++ b/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. diff --git a/cpukit/score/src/kern_tc.c b/cpukit/score/src/kern_tc.c index 16d76a1..e56c292 100644 --- a/cpukit/score/src/kern_tc.c +++ b/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__ */ diff --git a/cpukit/score/src/smp.c b/cpukit/score/src/smp.c index 4dacd4e..9d9507d 100644 --- a/cpukit/score/src/smp.c +++ b/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 ); } } diff --git a/cpukit/score/src/watchdogtick.c b/cpukit/score/src/watchdogtick.c index cb68ea2..e89c088 100644 --- a/cpukit/score/src/watchdogtick.c +++ b/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(); + } } -- 1.8.4.5 _______________________________________________ devel mailing list devel@rtems.org http://lists.rtems.org/mailman/listinfo/devel