score: Replace watchdog handler implementation

Use a red-black tree instead of delta chains.

Close #2344.
Update #2554.
Update #2555.
Close #2606.

24 files changed, 621 insertions, 871 deletions

diff --git a/cpukit/score/Makefile.am b/cpukit/score/Makefile.am
index 3d3f1c1532..ea440b384e 100644
--- a/cpukit/score/Makefile.am
+++ b/cpukit/score/Makefile.am
@@ -327,14 +327,13 @@ libscore_a_SOURCES += src/timespecaddto.c src/timespecfromticks.c \
 
 ## TOD_C_FILES
 libscore_a_SOURCES += src/coretod.c src/coretodset.c \
-    src/coretodtickle.c \
     src/coretodtickspersec.c \
     src/coretodadjust.c
 libscore_a_SOURCES += src/coretodabsolutetimeout.c
 
 ## WATCHDOG_C_FILES
-libscore_a_SOURCES += src/watchdog.c src/watchdogadjust.c \
-    src/watchdoginsert.c src/watchdogremove.c
+libscore_a_SOURCES += src/watchdoginsert.c
+libscore_a_SOURCES += src/watchdogremove.c
 libscore_a_SOURCES += src/watchdogtick.c
 libscore_a_SOURCES += src/watchdogtickssinceboot.c
 
diff --git a/cpukit/score/include/rtems/score/mrsp.h b/cpukit/score/include/rtems/score/mrsp.h
index 08f96ac4ac..cb3de67e26 100644
--- a/cpukit/score/include/rtems/score/mrsp.h
+++ b/cpukit/score/include/rtems/score/mrsp.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2014, 2016 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -128,6 +128,11 @@ typedef struct {
    * MRSP_TIMEOUT.  State changes are protected by the MrsP control lock.
    */
   volatile MRSP_Status status;
+
+  /**
+   * @brief Watchdog for timeouts.
+   */
+  Watchdog_Control Watchdog;
 } MRSP_Rival;
 
 /**
diff --git a/cpukit/score/include/rtems/score/mrspimpl.h b/cpukit/score/include/rtems/score/mrspimpl.h
index bc9ed4b511..7638fb5975 100644
--- a/cpukit/score/include/rtems/score/mrspimpl.h
+++ b/cpukit/score/include/rtems/score/mrspimpl.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2014, 2016 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -159,18 +159,13 @@ RTEMS_INLINE_ROUTINE void _MRSP_Set_ceiling_priority(
   mrsp->ceiling_priorities[ scheduler_index ] = ceiling_priority;
 }
 
-RTEMS_INLINE_ROUTINE void _MRSP_Timeout(
-  Objects_Id  id,
-  void       *arg
-)
+RTEMS_INLINE_ROUTINE void _MRSP_Timeout( Watchdog_Control *watchdog )
 {
-  MRSP_Rival *rival = arg;
+  MRSP_Rival *rival = RTEMS_CONTAINER_OF( watchdog, MRSP_Rival, Watchdog );
   MRSP_Control *mrsp = rival->resource;
   Thread_Control *thread = rival->thread;
   ISR_lock_Context lock_context;
 
-  (void) id;
-
   _ISR_lock_ISR_disable_and_acquire( &mrsp->Lock, &lock_context );
 
   if ( rival->status == MRSP_WAIT_FOR_OWNERSHIP ) {
@@ -209,6 +204,7 @@ RTEMS_INLINE_ROUTINE MRSP_Status _MRSP_Wait_for_ownership(
   bool initial_life_protection;
   Per_CPU_Control *cpu_self;
   ISR_lock_Context giant_lock_context;
+  ISR_Level level;
 
   rival.thread = executing;
   rival.resource = mrsp;
@@ -236,13 +232,11 @@ RTEMS_INLINE_ROUTINE MRSP_Status _MRSP_Wait_for_ownership(
   _Thread_Raise_priority( executing, ceiling_priority );
 
   if ( timeout > 0 ) {
-    _Watchdog_Initialize(
-      &executing->Timer,
-      _MRSP_Timeout,
-      0,
-      &rival
-    );
-    _Watchdog_Insert_ticks( &executing->Timer, timeout );
+    _Watchdog_Preinitialize( &rival.Watchdog, cpu_self );
+    _Watchdog_Initialize( &rival.Watchdog, _MRSP_Timeout );
+    _ISR_Disable_without_giant( level );
+    _Watchdog_Per_CPU_insert_relative( &rival.Watchdog, cpu_self, timeout );
+    _ISR_Enable_without_giant( level );
   }
 
   initial_life_protection = _Thread_Set_life_protection( true );
@@ -258,7 +252,13 @@ RTEMS_INLINE_ROUTINE MRSP_Status _MRSP_Wait_for_ownership(
   _Thread_Set_life_protection( initial_life_protection );
 
   if ( timeout > 0 ) {
-    _Watchdog_Remove_ticks( &executing->Timer );
+    _ISR_Disable_without_giant( level );
+    _Watchdog_Per_CPU_remove(
+      &rival.Watchdog,
+      cpu_self,
+      &cpu_self->Watchdog.Header[ PER_CPU_WATCHDOG_RELATIVE ]
+    );
+    _ISR_Enable_without_giant( level );
 
     if ( status == MRSP_TIMEOUT ) {
       _MRSP_Restore_priority( executing, initial_priority );
diff --git a/cpukit/score/include/rtems/score/percpu.h b/cpukit/score/include/rtems/score/percpu.h
index 39be1e3367..22017889b3 100644
--- a/cpukit/score/include/rtems/score/percpu.h
+++ b/cpukit/score/include/rtems/score/percpu.h
@@ -23,10 +23,11 @@
   #include <rtems/asm.h>
 #else
   #include <rtems/score/assert.h>
-  #include <rtems/score/isrlevel.h>
+  #include <rtems/score/isrlock.h>
   #include <rtems/score/smp.h>
   #include <rtems/score/smplock.h>
   #include <rtems/score/timestamp.h>
+  #include <rtems/score/watchdog.h>
 #endif
 
 #ifdef __cplusplus
@@ -41,7 +42,7 @@ extern "C" {
    * processor.
    */
   #if defined( RTEMS_PROFILING )
-    #define PER_CPU_CONTROL_SIZE_LOG2 8
+    #define PER_CPU_CONTROL_SIZE_LOG2 9
   #else
     #define PER_CPU_CONTROL_SIZE_LOG2 7
   #endif
@@ -226,6 +227,32 @@ typedef struct {
 } Per_CPU_Stats;
 
 /**
+ * @brief Per-CPU watchdog header index.
+ */
+typedef enum {
+  /**
+   * @brief Index for relative per-CPU watchdog header.
+   *
+   * The reference time point for this header is current ticks value
+   * during insert.  Time is measured in clock ticks.
+   */
+  PER_CPU_WATCHDOG_RELATIVE,
+
+  /**
+   * @brief Index for absolute per-CPU watchdog header.
+   *
+   * The reference time point for this header is the POSIX Epoch.  Time is
+   * measured in nanoseconds since POSIX Epoch.
+   */
+  PER_CPU_WATCHDOG_ABSOLUTE,
+
+  /**
+   * @brief Count of per-CPU watchdog headers.
+   */
+  PER_CPU_WATCHDOG_COUNT
+} Per_CPU_Watchdog_index;
+
+/**
  *  @brief Per CPU Core Structure
  *
  *  This structure is used to hold per core state information.
@@ -309,6 +336,28 @@ typedef struct Per_CPU_Control {
   /** This is the time of the last context switch on this CPU. */
   Timestamp_Control time_of_last_context_switch;
 
+  /**
+   * @brief Watchdog state for this processor.
+   */
+  struct {
+    /**
+     * @brief Protects all watchdog operations on this processor.
+     */
+    ISR_LOCK_MEMBER( Lock )
+
+    /**
+     * @brief Watchdog ticks on this processor used for relative watchdogs.
+     */
+    uint64_t ticks;
+
+    /**
+     * @brief Header for watchdogs.
+     *
+     * @see Per_CPU_Watchdog_index.
+     */
+    Watchdog_Header Header[ PER_CPU_WATCHDOG_COUNT ];
+  } Watchdog;
+
   #if defined( RTEMS_SMP )
     /**
      * @brief This lock protects some parts of the low-level thread dispatching.
diff --git a/cpukit/score/include/rtems/score/schedulerimpl.h b/cpukit/score/include/rtems/score/schedulerimpl.h
index cadebfd02f..d50c36a7b9 100644
--- a/cpukit/score/include/rtems/score/schedulerimpl.h
+++ b/cpukit/score/include/rtems/score/schedulerimpl.h
@@ -452,19 +452,13 @@ RTEMS_INLINE_ROUTINE void _Scheduler_Release_job(
  * scheduler which support standard RTEMS features, this includes
  * time-slicing management.
  */
-RTEMS_INLINE_ROUTINE void _Scheduler_Tick( void )
+RTEMS_INLINE_ROUTINE void _Scheduler_Tick( const Per_CPU_Control *cpu )
 {
-  uint32_t cpu_count = _SMP_Get_processor_count();
-  uint32_t cpu_index;
+  const Scheduler_Control *scheduler = _Scheduler_Get_by_CPU( cpu );
+  Thread_Control *executing = cpu->executing;
 
-  for ( cpu_index = 0 ; cpu_index < cpu_count ; ++cpu_index ) {
-    const Per_CPU_Control *cpu = _Per_CPU_Get_by_index( cpu_index );
-    const Scheduler_Control *scheduler = _Scheduler_Get_by_CPU( cpu );
-    Thread_Control *executing = cpu->executing;
-
-    if ( scheduler != NULL && executing != NULL ) {
-      ( *scheduler->Operations.tick )( scheduler, executing );
-    }
+  if ( scheduler != NULL && executing != NULL ) {
+    ( *scheduler->Operations.tick )( scheduler, executing );
   }
 }
 
diff --git a/cpukit/score/include/rtems/score/thread.h b/cpukit/score/include/rtems/score/thread.h
index 73776b3ec1..44b706d7ff 100644
--- a/cpukit/score/include/rtems/score/thread.h
+++ b/cpukit/score/include/rtems/score/thread.h
@@ -347,6 +347,15 @@ typedef struct {
 }   Thread_Wait_information;
 
 /**
+ * @brief Information required to manage a thread timer.
+ */
+typedef struct {
+  ISR_LOCK_MEMBER( Lock )
+  Watchdog_Header *header;
+  Watchdog_Control Watchdog;
+} Thread_Timer_information;
+
+/**
  *  The following defines the control block used to manage
  *  each thread proxy.
  *
@@ -400,7 +409,7 @@ typedef struct {
   /** This field is the blocking information for this proxy. */
   Thread_Wait_information  Wait;
   /** This field is the Watchdog used to manage proxy delays and timeouts. */
-  Watchdog_Control         Timer;
+  Thread_Timer_information Timer;
 #if defined(RTEMS_MULTIPROCESSING)
   /** This field is the received response packet in an MP system. */
   MP_packet_Prefix        *receive_packet;
@@ -728,7 +737,7 @@ struct _Thread_Control {
   /** This field is the blocking information for this thread. */
   Thread_Wait_information  Wait;
   /** This field is the Watchdog used to manage thread delays and timeouts. */
-  Watchdog_Control         Timer;
+  Thread_Timer_information Timer;
 #if defined(RTEMS_MULTIPROCESSING)
   /** This field is the received response packet in an MP system. */
   MP_packet_Prefix        *receive_packet;
diff --git a/cpukit/score/include/rtems/score/threadimpl.h b/cpukit/score/include/rtems/score/threadimpl.h
index 1377543118..ec9851dba0 100644
--- a/cpukit/score/include/rtems/score/threadimpl.h
+++ b/cpukit/score/include/rtems/score/threadimpl.h
@@ -33,6 +33,7 @@
 #include <rtems/score/threadqimpl.h>
 #include <rtems/score/todimpl.h>
 #include <rtems/score/freechain.h>
+#include <rtems/score/watchdogimpl.h>
 #include <rtems/config.h>
 
 #ifdef __cplusplus
@@ -1472,10 +1473,64 @@ RTEMS_INLINE_ROUTINE void _Thread_Wait_set_timeout_code(
 /**
  * @brief General purpose thread wait timeout.
  *
- * @param[in] id Unused.
- * @param[in] arg The thread.
+ * @param[in] watchdog The thread timer watchdog.
  */
-void _Thread_Timeout( Objects_Id id, void *arg );
+void _Thread_Timeout( Watchdog_Control *watchdog );
+
+RTEMS_INLINE_ROUTINE void _Thread_Timer_insert_relative(
+  Thread_Control                 *the_thread,
+  Per_CPU_Control                *cpu,
+  Watchdog_Service_routine_entry  routine,
+  Watchdog_Interval               ticks
+)
+{
+  ISR_lock_Context lock_context;
+
+  _ISR_lock_ISR_disable_and_acquire( &the_thread->Timer.Lock, &lock_context );
+
+  the_thread->Timer.header = &cpu->Watchdog.Header[ PER_CPU_WATCHDOG_RELATIVE ];
+  the_thread->Timer.Watchdog.routine = routine;
+  _Watchdog_Per_CPU_insert_relative( &the_thread->Timer.Watchdog, cpu, ticks );
+
+  _ISR_lock_Release_and_ISR_enable( &the_thread->Timer.Lock, &lock_context );
+}
+
+RTEMS_INLINE_ROUTINE void _Thread_Timer_insert_absolute(
+  Thread_Control                 *the_thread,
+  Per_CPU_Control                *cpu,
+  Watchdog_Service_routine_entry  routine,
+  uint64_t                        expire
+)
+{
+  ISR_lock_Context lock_context;
+
+  _ISR_lock_ISR_disable_and_acquire( &the_thread->Timer.Lock, &lock_context );
+
+  the_thread->Timer.header = &cpu->Watchdog.Header[ PER_CPU_WATCHDOG_ABSOLUTE ];
+  the_thread->Timer.Watchdog.routine = routine;
+  _Watchdog_Per_CPU_insert_absolute( &the_thread->Timer.Watchdog, cpu, expire );
+
+  _ISR_lock_Release_and_ISR_enable( &the_thread->Timer.Lock, &lock_context );
+}
+
+RTEMS_INLINE_ROUTINE void _Thread_Timer_remove( Thread_Control *the_thread )
+{
+  ISR_lock_Context lock_context;
+
+  _ISR_lock_ISR_disable_and_acquire( &the_thread->Timer.Lock, &lock_context );
+
+  _Watchdog_Per_CPU_remove(
+    &the_thread->Timer.Watchdog,
+#if defined(RTEMS_SMP)
+    the_thread->Timer.Watchdog.cpu,
+#else
+    _Per_CPU_Get(),
+#endif
+    the_thread->Timer.header
+  );
+
+  _ISR_lock_Release_and_ISR_enable( &the_thread->Timer.Lock, &lock_context );
+}
 
 RTEMS_INLINE_ROUTINE void _Thread_Debug_set_real_processor(
   Thread_Control  *the_thread,
diff --git a/cpukit/score/include/rtems/score/todimpl.h b/cpukit/score/include/rtems/score/todimpl.h
index b61651cdba..b1f8a6d6b6 100644
--- a/cpukit/score/include/rtems/score/todimpl.h
+++ b/cpukit/score/include/rtems/score/todimpl.h
@@ -132,15 +132,6 @@ extern "C" {
  */
 typedef struct {
   /**
-   * @brief Time of day seconds trigger.
-   *
-   * This value specifies the nanoseconds since the last time of day second.
-   * It is updated and evaluated in _TOD_Tickle_ticks().  It is set in
-   * _TOD_Set_with_timestamp().
-   */
-  uint32_t seconds_trigger;
-
-  /**
    *  @brief Indicates if the time of day is set.
    *
    *  This is true if the application has set the current
@@ -273,14 +264,6 @@ static inline uint32_t _TOD_Seconds_since_epoch( void )
 }
 
 /**
- *  @brief Increments time of day at each clock tick.
- *
- *  This routine increments the ticks field of the current time of
- *  day at each clock tick.
- */
-void _TOD_Tickle_ticks( void );
-
-/**
  *  @brief Gets number of ticks in a second.
  *
  *  This method returns the number of ticks in a second.
diff --git a/cpukit/score/include/rtems/score/watchdog.h b/cpukit/score/include/rtems/score/watchdog.h
index bad7269051..c582dbd1ae 100644
--- a/cpukit/score/include/rtems/score/watchdog.h
+++ b/cpukit/score/include/rtems/score/watchdog.h
@@ -20,7 +20,11 @@
 #ifndef _RTEMS_SCORE_WATCHDOG_H
 #define _RTEMS_SCORE_WATCHDOG_H
 
-#include <rtems/score/object.h>
+#include <rtems/score/basedefs.h>
+#include <rtems/score/chain.h>
+#include <rtems/score/rbtree.h>
+
+struct Per_CPU_Control;
 
 #ifdef __cplusplus
 extern "C" {
@@ -39,6 +43,8 @@ extern "C" {
  */
 /**@{*/
 
+typedef struct Watchdog_Control Watchdog_Control;
+
 /**
  *  @brief Type is used to specify the length of intervals.
  *
@@ -58,10 +64,8 @@ typedef void Watchdog_Service_routine;
  *
  *  This type define a pointer to a watchdog service routine.
  */
-typedef Watchdog_Service_routine ( *Watchdog_Service_routine_entry )(
-                 Objects_Id,
-                 void *
-             );
+typedef Watchdog_Service_routine
+  ( *Watchdog_Service_routine_entry )( Watchdog_Control * );
 
 /**
  *  @brief The constant for indefinite wait.
@@ -72,22 +76,20 @@ typedef Watchdog_Service_routine ( *Watchdog_Service_routine_entry )(
 #define WATCHDOG_NO_TIMEOUT  0
 
 /**
- *  @brief Set of the states which a watchdog timer may be at any given time.
- *
- *  This enumerated type is the set of the states in which a
- *  watchdog timer may be at any given time.
+ * @brief The watchdog header to manage scheduled watchdogs.
  */
+typedef struct {
+  /**
+   * @brief Red-black tree of scheduled watchdogs sorted by expiration time.
+   */
+  RBTree_Control Watchdogs;
 
-typedef enum {
-  /** This is the state when the watchdog is off all chains */
-  WATCHDOG_INACTIVE,
-  /** This is the state when the watchdog is off all chains, but we are
-   *  currently searching for the insertion point.
+  /**
+   * @brief The scheduled watchdog with the earliest expiration time or NULL in
+   * case no watchdog is scheduled.
    */
-  WATCHDOG_BEING_INSERTED,
-  /** This is the state when the watchdog is on a chain, and allowed to fire. */
-  WATCHDOG_ACTIVE
-} Watchdog_States;
+  RBTree_Node *first;
+} Watchdog_Header;
 
 /**
  *  @brief The control block used to manage each watchdog timer.
@@ -95,30 +97,35 @@ typedef enum {
  *  The following record defines the control block used
  *  to manage each watchdog timer.
  */
-typedef struct {
-  /** This field is a Chain Node structure and allows this to be placed on
-   *  chains for set management.
-   */
-  Chain_Node                      Node;
-  /** This field is the state of the watchdog. */
-  Watchdog_States                 state;
-  /** This field is the initially requested interval. */
-  Watchdog_Interval               initial;
-  /** This field is the remaining portion of the interval. */
-  Watchdog_Interval               delta_interval;
-  /** This field is the number of system clock ticks when this was scheduled. */
-  Watchdog_Interval               start_time;
-  /** This field is the number of system clock ticks when this was suspended. */
-  Watchdog_Interval               stop_time;
-  /** This field is the function to invoke. */
-  Watchdog_Service_routine_entry  routine;
-  /** This field is the Id to pass as an argument to the routine. */
-  Objects_Id                      id;
-  /** This field is an untyped pointer to user data that is passed to the
-   *  watchdog handler routine.
+struct Watchdog_Control {
+  /**
+   * @brief Nodes for the watchdog.
    */
-  void                           *user_data;
-}   Watchdog_Control;
+  union {
+    /**
+     * @brief this field is a red-black tree node structure and allows this to
+     * be placed on a red-black tree used to manage the scheduled watchdogs.
+     */
+    RBTree_Node RBTree;
+
+    /**
+     * @brief this field is a chain node structure and allows this to be placed
+     * on a chain used to manage pending watchdogs by the timer server.
+     */
+    Chain_Node Chain;
+  } Node;
+
+#if defined(RTEMS_SMP)
+  /** @brief This field references the processor of this watchdog control. */
+  struct Per_CPU_Control *cpu;
+#endif
+
+  /** @brief This field is the function to invoke. */
+  Watchdog_Service_routine_entry routine;
+
+  /** @brief This field is the expiration time point. */
+  uint64_t expire;
+};
 
 /**
  * @brief The watchdog ticks counter.
diff --git a/cpukit/score/include/rtems/score/watchdogimpl.h b/cpukit/score/include/rtems/score/watchdogimpl.h
index 49ac2a12b1..2b24cc68e5 100644
--- a/cpukit/score/include/rtems/score/watchdogimpl.h
+++ b/cpukit/score/include/rtems/score/watchdogimpl.h
@@ -21,9 +21,11 @@
 
 #include <rtems/score/watchdog.h>
 #include <rtems/score/assert.h>
-#include <rtems/score/chainimpl.h>
 #include <rtems/score/isrlock.h>
 #include <rtems/score/percpu.h>
+#include <rtems/score/rbtreeimpl.h>
+
+#include <sys/timespec.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -35,241 +37,116 @@ extern "C" {
  */
 
 /**
- * @brief Watchdog initializer for static initialization.
- *
- * @see _Watchdog_Initialize().
- */
-#define WATCHDOG_INITIALIZER( routine, id, user_data ) \
-  { \
-    { NULL, NULL }, \
-    WATCHDOG_INACTIVE, \
-    0, 0, 0, 0, \
-    ( routine ), ( id ), ( user_data ) \
-  }
-
-/**
- * @brief Iterator item to synchronize concurrent insert, remove and tickle
- * operations.
+ * @brief Watchdog states.
  */
-typedef struct {
+typedef enum {
   /**
-   * @brief A node for a Watchdog_Header::Iterators chain.
+   * @brief The watchdog is scheduled and a black node in the red-black tree.
    */
-  Chain_Node Node;
+  WATCHDOG_SCHEDULED_BLACK,
 
   /**
-   * @brief The current delta interval of the new watchdog to insert.
+   * @brief The watchdog is scheduled and a red node in the red-black tree.
    */
-  Watchdog_Interval delta_interval;
+  WATCHDOG_SCHEDULED_RED,
 
   /**
-   * @brief The current watchdog of the chain on the way to insert the new
-   * watchdog.
+   * @brief The watchdog is inactive.
    */
-  Chain_Node *current;
-} Watchdog_Iterator;
-
-/**
- * @brief Watchdog header.
- */
-typedef struct {
-  /**
-   * @brief ISR lock to protect this watchdog chain.
-   */
-  ISR_LOCK_MEMBER( Lock )
+  WATCHDOG_INACTIVE,
 
   /**
-   * @brief The chain of active or transient watchdogs.
-   */
-  Chain_Control Watchdogs;
-
-  /**
-   * @brief Currently active iterators.
+   * @brief The watchdog is on a chain of pending watchdogs.
    *
-   * The iterators are registered in _Watchdog_Insert() and updated in case the
-   * watchdog chain changes.
+   * This state is used by the timer server for example.
    */
-  Chain_Control Iterators;
-} Watchdog_Header;
+  WATCHDOG_PENDING
+} Watchdog_State;
 
 /**
- *  @brief Watchdog chain which is managed at ticks.
+ * @brief Watchdog initializer for static initialization.
  *
- *  This is the watchdog chain which is managed at ticks.
- */
-extern Watchdog_Header _Watchdog_Ticks_header;
-
-/**
- *  @brief Watchdog chain which is managed at second boundaries.
+ * The processor of this watchdog is set to processor with index zero.
  *
- *  This is the watchdog chain which is managed at second boundaries.
+ * @see _Watchdog_Preinitialize().
  */
-extern Watchdog_Header _Watchdog_Seconds_header;
-
-RTEMS_INLINE_ROUTINE void _Watchdog_Acquire(
-  Watchdog_Header  *header,
-  ISR_lock_Context *lock_context
-)
-{
-  _ISR_lock_ISR_disable_and_acquire( &header->Lock, lock_context );
-}
+#if defined(RTEMS_SMP)
+  #define WATCHDOG_INITIALIZER( routine ) \
+    { \
+      { { { NULL, NULL, NULL, WATCHDOG_INACTIVE } } }, \
+      &_Per_CPU_Information[ 0 ].per_cpu, \
+      ( routine ), \
+      0 \
+    }
+#else
+  #define WATCHDOG_INITIALIZER( routine ) \
+    { \
+      { { { NULL, NULL, NULL, WATCHDOG_INACTIVE } } }, \
+      ( routine ), \
+      0 \
+    }
+#endif
 
-RTEMS_INLINE_ROUTINE void _Watchdog_Release(
-  Watchdog_Header  *header,
-  ISR_lock_Context *lock_context
+RTEMS_INLINE_ROUTINE void _Watchdog_Header_initialize(
+  Watchdog_Header *header
 )
 {
-  _ISR_lock_Release_and_ISR_enable( &header->Lock, lock_context );
+  _RBTree_Initialize_empty( &header->Watchdogs );
+  header->first = NULL;
 }
 
-RTEMS_INLINE_ROUTINE void _Watchdog_Flash(
-  Watchdog_Header  *header,
-  ISR_lock_Context *lock_context
+RTEMS_INLINE_ROUTINE void _Watchdog_Header_destroy(
+  Watchdog_Header *header
 )
 {
-  _ISR_lock_Flash( &header->Lock, lock_context );
+  /* Do nothing */
+  (void) header;
 }
 
 /**
- *  @brief Initialize the watchdog handler.
- *
- *  This routine initializes the watchdog handler.  The watchdog
- *  synchronization flag is initialized and the watchdog chains are
- *  initialized and emptied.
- */
-void _Watchdog_Handler_initialization( void );
-
-/**
  *  @brief Performs a watchdog tick.
  *
  *  @param cpu The processor for this watchdog tick.
  */
-void _Watchdog_Tick( Per_CPU_Control *cpu );
-
-/**
- *  @brief Removes @a the_watchdog from the watchdog chain.
- *
- *  This routine removes @a the_watchdog from the watchdog chain on which
- *  it resides and returns the state @a the_watchdog timer was in.
- *
- *  @param[in] header The watchdog chain.
- *  @param[in] the_watchdog will be removed
- *  @retval the state in which @a the_watchdog was in when removed
- */
-Watchdog_States _Watchdog_Remove (
-  Watchdog_Header  *header,
-  Watchdog_Control *the_watchdog
-);
-
-/**
- *  @brief Adjusts the header watchdog chain in the backward direction for
- *  units ticks.
- *
- *  @param[in] header The watchdog chain.
- *  @param[in] units The units of ticks to adjust.
- */
-void _Watchdog_Adjust_backward(
-  Watchdog_Header   *header,
-  Watchdog_Interval  units
-);
-
-/**
- * @brief Adjusts the watchdogs in backward direction in a locked context.
- *
- * The caller must be the owner of the watchdog lock and will be the owner
- * after the call.
- *
- * @param[in] header The watchdog header.
- * @param[in] units The units of ticks to adjust.
- *
- * @see _Watchdog_Adjust_forward().
- */
-void _Watchdog_Adjust_backward_locked(
-  Watchdog_Header   *header,
-  Watchdog_Interval  units
-);
+void _Watchdog_Tick( struct Per_CPU_Control *cpu );
 
-/**
- *  @brief Adjusts the header watchdog chain in the forward direction for units
- *  ticks.
- *
- *  This may lead to several _Watchdog_Tickle() invocations.
- *
- *  @param[in] header The watchdog chain.
- *  @param[in] units The units of ticks to adjust.
- */
-void _Watchdog_Adjust_forward(
-  Watchdog_Header   *header,
-  Watchdog_Interval  units
-);
+RTEMS_INLINE_ROUTINE Watchdog_State _Watchdog_Get_state(
+  const Watchdog_Control *the_watchdog
+)
+{
+  return RB_COLOR( &the_watchdog->Node.RBTree, Node );
+}
 
-/**
- * @brief Adjusts the watchdogs in forward direction in a locked context.
- *
- * The caller must be the owner of the watchdog lock and will be the owner
- * after the call.  This function may release and acquire the watchdog lock
- * internally.
- *
- * @param[in] header The watchdog header.
- * @param[in] units The units of ticks to adjust.
- * @param[in] lock_context The lock context.
- *
- * @see _Watchdog_Adjust_forward().
- */
-void _Watchdog_Adjust_forward_locked(
-  Watchdog_Header   *header,
-  Watchdog_Interval  units,
-  ISR_lock_Context  *lock_context
-);
+RTEMS_INLINE_ROUTINE void _Watchdog_Set_state(
+  Watchdog_Control *the_watchdog,
+  Watchdog_State    state
+)
+{
+  RB_COLOR( &the_watchdog->Node.RBTree, Node ) = state;
+}
 
-/**
- *  @brief Inserts @a the_watchdog into the @a header watchdog chain
- *  for a time of @a units.
- *
- *  This routine inserts @a the_watchdog into the @a header watchdog chain
- *  for a time of @a units.
- *  Update the delta interval counters.
- *
- *  @param[in] header is @a the_watchdog list to insert @a the_watchdog on
- *  @param[in] the_watchdog is the watchdog to insert
- */
-void _Watchdog_Insert (
-  Watchdog_Header  *header,
-  Watchdog_Control *the_watchdog
-);
+RTEMS_INLINE_ROUTINE Per_CPU_Control *_Watchdog_Get_CPU(
+  const Watchdog_Control *the_watchdog
+)
+{
+#if defined(RTEMS_SMP)
+  return the_watchdog->cpu;
+#else
+  return _Per_CPU_Get_by_index( 0 );
+#endif
+}
 
-/**
- * @brief Inserts the watchdog in a locked context.
- *
- * The caller must be the owner of the watchdog lock and will be the owner
- * after the call.  This function may release and acquire the watchdog lock
- * internally.
- *
- * @param[in] header The watchdog header.
- * @param[in] the_watchdog The watchdog.
- * @param[in] lock_context The lock context.
- *
- * @see _Watchdog_Insert().
- */
-void _Watchdog_Insert_locked(
-  Watchdog_Header  *header,
+RTEMS_INLINE_ROUTINE void _Watchdog_Set_CPU(
   Watchdog_Control *the_watchdog,
-  ISR_lock_Context *lock_context
-);
-
-/**
- *  @brief This routine is invoked at appropriate intervals to update
- *  the @a header watchdog chain.
- *
- *  This routine is invoked at appropriate intervals to update
- *  the @a header watchdog chain.
- *  This routine decrements the delta counter in response to a tick.
- *
- *  @param[in] header is the watchdog chain to tickle
- */
-void _Watchdog_Tickle (
-  Watchdog_Header *header
-);
+  Per_CPU_Control  *cpu
+)
+{
+#if defined(RTEMS_SMP)
+  the_watchdog->cpu = cpu;
+#else
+  (void) cpu;
+#endif
+}
 
 /**
  * @brief Pre-initializes a watchdog.
@@ -280,228 +157,284 @@ void _Watchdog_Tickle (
  * @param[in] the_watchdog The uninitialized watchdog.
  */
 RTEMS_INLINE_ROUTINE void _Watchdog_Preinitialize(
-  Watchdog_Control *the_watchdog
+  Watchdog_Control *the_watchdog,
+  Per_CPU_Control  *cpu
 )
 {
-  the_watchdog->state = WATCHDOG_INACTIVE;
+  _Watchdog_Set_CPU( the_watchdog, cpu );
+  _Watchdog_Set_state( the_watchdog, WATCHDOG_INACTIVE );
+
 #if defined(RTEMS_DEBUG)
   the_watchdog->routine = NULL;
-  the_watchdog->id = 0;
-  the_watchdog->user_data = NULL;
+  the_watchdog->expire = 0;
 #endif
 }
 
 /**
- * This routine initializes the specified watchdog.  The watchdog is
- * made inactive, the watchdog id and handler routine are set to the
- * specified values.
+ * @brief Initializes a watchdog with a new service routine.
+ *
+ * The watchdog must be inactive.
  */
-
 RTEMS_INLINE_ROUTINE void _Watchdog_Initialize(
   Watchdog_Control               *the_watchdog,
-  Watchdog_Service_routine_entry  routine,
-  Objects_Id                      id,
-  void                           *user_data
+  Watchdog_Service_routine_entry  routine
 )
 {
-  _Assert( the_watchdog->state == WATCHDOG_INACTIVE );
-  the_watchdog->routine   = routine;
-  the_watchdog->id        = id;
-  the_watchdog->user_data = user_data;
+  _Assert( _Watchdog_Get_state( the_watchdog ) == WATCHDOG_INACTIVE );
+  the_watchdog->routine = routine;
 }
 
+void _Watchdog_Do_tickle(
+  Watchdog_Header  *header,
+  uint64_t          now,
+#if defined(RTEMS_SMP)
+  ISR_lock_Control *lock,
+#endif
+  ISR_lock_Context *lock_context
+);
+
+#if defined(RTEMS_SMP)
+  #define _Watchdog_Tickle( header, now, lock, lock_context ) \
+    _Watchdog_Do_tickle( header, now, lock, lock_context )
+#else
+  #define _Watchdog_Tickle( header, now, lock, lock_context ) \
+    _Watchdog_Do_tickle( header, now, lock_context )
+#endif
+
 /**
- * This routine returns true if the watchdog timer is in the ACTIVE
- * state, and false otherwise.
+ * @brief Inserts a watchdog into the set of scheduled watchdogs according to
+ * the specified expiration time.
+ *
+ * The watchdog must be inactive.
  */
+void _Watchdog_Insert(
+  Watchdog_Header  *header,
+  Watchdog_Control *the_watchdog,
+  uint64_t          expire
+);
 
-RTEMS_INLINE_ROUTINE bool _Watchdog_Is_active(
+/**
+ * @brief In case the watchdog is scheduled, then it is removed from the set of
+ * scheduled watchdogs.
+ *
+ * The watchdog must be initialized before this call.
+ */
+void _Watchdog_Remove(
+  Watchdog_Header  *header,
   Watchdog_Control *the_watchdog
-)
-{
-
-  return ( the_watchdog->state == WATCHDOG_ACTIVE );
-
-}
+);
 
 /**
- * This routine activates THE_WATCHDOG timer which is already
- * on a watchdog chain.
+ * @brief In case the watchdog is scheduled, then it is removed from the set of
+ * scheduled watchdogs.
+ *
+ * The watchdog must be initialized before this call.
+ *
+ * @retval 0 The now time is greater than or equal to the expiration time of
+ * the watchdog.
+ * @retval other The difference of the now and expiration time.
  */
-
-RTEMS_INLINE_ROUTINE void _Watchdog_Activate(
-  Watchdog_Control *the_watchdog
+RTEMS_INLINE_ROUTINE uint64_t _Watchdog_Cancel(
+  Watchdog_Header  *header,
+  Watchdog_Control *the_watchdog,
+  uint64_t          now
 )
 {
+  uint64_t expire;
+  uint64_t remaining;
 
-  the_watchdog->state = WATCHDOG_ACTIVE;
+  expire = the_watchdog->expire;
 
-}
-
-/**
- * This routine is invoked at each clock tick to update the ticks
- * watchdog chain.
- */
-
-RTEMS_INLINE_ROUTINE void _Watchdog_Tickle_ticks( void )
-{
+  if ( now < expire ) {
+    remaining = expire - now;
+  } else {
+    remaining = 0;
+  }
 
-  _Watchdog_Tickle( &_Watchdog_Ticks_header );
+  _Watchdog_Remove( header, the_watchdog );
 
+  return remaining;
 }
 
-/**
- * This routine is invoked at each clock tick to update the seconds
- * watchdog chain.
- */
-
-RTEMS_INLINE_ROUTINE void _Watchdog_Tickle_seconds( void )
+RTEMS_INLINE_ROUTINE bool _Watchdog_Is_scheduled(
+  const Watchdog_Control *the_watchdog
+)
 {
-
-  _Watchdog_Tickle( &_Watchdog_Seconds_header );
-
+  return _Watchdog_Get_state( the_watchdog ) < WATCHDOG_INACTIVE;
 }
 
-/**
- * This routine inserts THE_WATCHDOG into the ticks watchdog chain
- * for a time of UNITS ticks.  The INSERT_MODE indicates whether
- * THE_WATCHDOG is to be activated automatically or later, explicitly
- * by the caller.
- */
-
-RTEMS_INLINE_ROUTINE void _Watchdog_Insert_ticks(
-  Watchdog_Control      *the_watchdog,
-  Watchdog_Interval      units
+RTEMS_INLINE_ROUTINE void _Watchdog_Next_first(
+  Watchdog_Header  *header,
+  Watchdog_Control *the_watchdog
 )
 {
+  RBTree_Node *node = _RBTree_Right( &the_watchdog->Node.RBTree );
 
-  the_watchdog->initial = units;
+  if ( node != NULL ) {
+    RBTree_Node *left;
 
-  _Watchdog_Insert( &_Watchdog_Ticks_header, the_watchdog );
+    while ( ( left = _RBTree_Left( node ) ) != NULL ) {
+      node = left;
+    }
 
+    header->first = node;
+  } else {
+    header->first = _RBTree_Parent( &the_watchdog->Node.RBTree );
+  }
 }
 
 /**
- * This routine inserts THE_WATCHDOG into the seconds watchdog chain
- * for a time of UNITS seconds.  The INSERT_MODE indicates whether
- * THE_WATCHDOG is to be activated automatically or later, explicitly
- * by the caller.
+ * @brief The bits necessary to store 1000000000 nanoseconds.
+ *
+ * The expiration time is an unsigned 64-bit integer.  To store absolute
+ * timeouts we use 30 bits (2**30 == 1073741824) for the nanoseconds and 34
+ * bits for the seconds since UNIX Epoch.  This leads to a year 2514 problem.
  */
+#define WATCHDOG_BITS_FOR_1E9_NANOSECONDS 30
 
-RTEMS_INLINE_ROUTINE void _Watchdog_Insert_seconds(
-  Watchdog_Control      *the_watchdog,
-  Watchdog_Interval      units
+RTEMS_INLINE_ROUTINE uint64_t _Watchdog_Ticks_from_seconds(
+  uint32_t seconds
 )
 {
+  uint64_t ticks = seconds;
 
-  the_watchdog->initial = units;
-
-  _Watchdog_Insert( &_Watchdog_Seconds_header, the_watchdog );
+  ticks <<= WATCHDOG_BITS_FOR_1E9_NANOSECONDS;
 
+  return ticks;
 }
 
-RTEMS_INLINE_ROUTINE Watchdog_States _Watchdog_Remove_ticks(
-  Watchdog_Control *the_watchdog
+RTEMS_INLINE_ROUTINE uint64_t _Watchdog_Ticks_from_timespec(
+  const struct timespec *ts
 )
 {
-  return _Watchdog_Remove( &_Watchdog_Ticks_header, the_watchdog );
+  /*
+   * The seconds are in time_t which is a signed integer.  Thus this cast is
+   * subject to the year 2038 problem in case time_t is a 32-bit integer.
+   */
+  uint64_t ticks = (uint64_t) ts->tv_sec;
+
+  _Assert( ticks < 0x400000000 );
+  _Assert( ts->tv_nsec >= 0 );
+  _Assert( ts->tv_nsec < 1000000000 );
+
+  ticks <<= WATCHDOG_BITS_FOR_1E9_NANOSECONDS;
+  ticks |= ts->tv_nsec;
+
+  return ticks;
 }
 
-RTEMS_INLINE_ROUTINE Watchdog_States _Watchdog_Remove_seconds(
-  Watchdog_Control *the_watchdog
+RTEMS_INLINE_ROUTINE void _Watchdog_Per_CPU_acquire_critical(
+  Per_CPU_Control  *cpu,
+  ISR_lock_Context *lock_context
 )
 {
-  return _Watchdog_Remove( &_Watchdog_Seconds_header, the_watchdog );
+  _ISR_lock_Acquire( &cpu->Watchdog.Lock, lock_context );
 }
 
-/**
- * This routine resets THE_WATCHDOG timer to its state at INSERT
- * time.  This routine is valid only on interval watchdog timers
- * and is used to make an interval watchdog timer fire "every" so
- * many ticks.
- */
-
-RTEMS_INLINE_ROUTINE void _Watchdog_Reset_ticks(
-  Watchdog_Control *the_watchdog
+RTEMS_INLINE_ROUTINE void _Watchdog_Per_CPU_release_critical(
+  Per_CPU_Control  *cpu,
+  ISR_lock_Context *lock_context
 )
 {
-
-  _Watchdog_Remove_ticks( the_watchdog );
-
-  _Watchdog_Insert( &_Watchdog_Ticks_header, the_watchdog );
-
+  _ISR_lock_Release( &cpu->Watchdog.Lock, lock_context );
 }
 
-/**
- * This routine returns a pointer to the watchdog timer following
- * THE_WATCHDOG on the watchdog chain.
- */
-
-RTEMS_INLINE_ROUTINE Watchdog_Control *_Watchdog_Next(
-  Watchdog_Control *the_watchdog
+RTEMS_INLINE_ROUTINE void _Watchdog_Per_CPU_insert_relative(
+  Watchdog_Control *the_watchdog,
+  Per_CPU_Control  *cpu,
+  uint32_t          ticks
 )
 {
+  ISR_lock_Context lock_context;
 
-  return ( (Watchdog_Control *) the_watchdog->Node.next );
+  _Watchdog_Set_CPU( the_watchdog, cpu );
 
+  _Watchdog_Per_CPU_acquire_critical( cpu, &lock_context );
+  _Watchdog_Insert(
+    &cpu->Watchdog.Header[ PER_CPU_WATCHDOG_RELATIVE ],
+    the_watchdog,
+    cpu->Watchdog.ticks + ticks
+  );
+  _Watchdog_Per_CPU_release_critical( cpu, &lock_context );
 }
 
-/**
- * This routine returns a pointer to the watchdog timer preceding
- * THE_WATCHDOG on the watchdog chain.
- */
-
-RTEMS_INLINE_ROUTINE Watchdog_Control *_Watchdog_Previous(
-  Watchdog_Control *the_watchdog
+RTEMS_INLINE_ROUTINE void _Watchdog_Per_CPU_insert_absolute(
+  Watchdog_Control *the_watchdog,
+  Per_CPU_Control  *cpu,
+  uint64_t          expire
 )
 {
+  ISR_lock_Context lock_context;
 
-  return ( (Watchdog_Control *) the_watchdog->Node.previous );
+  _Watchdog_Set_CPU( the_watchdog, cpu );
 
+  _Watchdog_Per_CPU_acquire_critical( cpu, &lock_context );
+  _Watchdog_Insert(
+    &cpu->Watchdog.Header[ PER_CPU_WATCHDOG_ABSOLUTE ],
+    the_watchdog,
+    expire
+  );
+  _Watchdog_Per_CPU_release_critical( cpu, &lock_context );
 }
 
-/**
- * This routine returns a pointer to the first watchdog timer
- * on the watchdog chain HEADER.
- */
-
-RTEMS_INLINE_ROUTINE Watchdog_Control *_Watchdog_First(
-  Watchdog_Header *header
+RTEMS_INLINE_ROUTINE void _Watchdog_Per_CPU_remove(
+  Watchdog_Control *the_watchdog,
+  Per_CPU_Control  *cpu,
+  Watchdog_Header  *header
 )
 {
-
-  return ( (Watchdog_Control *) _Chain_First( &header->Watchdogs ) );
-
+  ISR_lock_Context lock_context;
+
+  _Watchdog_Per_CPU_acquire_critical( cpu, &lock_context );
+  _Watchdog_Remove(
+    header,
+    the_watchdog
+  );
+  _Watchdog_Per_CPU_release_critical( cpu, &lock_context );
 }
 
-/**
- * This routine returns a pointer to the last watchdog timer
- * on the watchdog chain HEADER.
- */
-
-RTEMS_INLINE_ROUTINE Watchdog_Control *_Watchdog_Last(
-  Watchdog_Header *header
+RTEMS_INLINE_ROUTINE void _Watchdog_Per_CPU_remove_relative(
+  Watchdog_Control *the_watchdog
 )
 {
-
-  return ( (Watchdog_Control *) _Chain_Last( &header->Watchdogs ) );
-
+  Per_CPU_Control *cpu;
+
+  cpu = _Watchdog_Get_CPU( the_watchdog );
+  _Watchdog_Per_CPU_remove(
+    the_watchdog,
+    cpu,
+    &cpu->Watchdog.Header[ PER_CPU_WATCHDOG_RELATIVE ]
+  );
 }
 
-RTEMS_INLINE_ROUTINE bool _Watchdog_Is_empty(
-  const Watchdog_Header *header
+RTEMS_INLINE_ROUTINE void _Watchdog_Per_CPU_remove_absolute(
+  Watchdog_Control *the_watchdog
 )
 {
-  return _Chain_Is_empty( &header->Watchdogs );
+  Per_CPU_Control *cpu;
+
+  cpu = _Watchdog_Get_CPU( the_watchdog );
+  _Watchdog_Per_CPU_remove(
+    the_watchdog,
+    cpu,
+    &cpu->Watchdog.Header[ PER_CPU_WATCHDOG_ABSOLUTE ]
+  );
 }
 
-RTEMS_INLINE_ROUTINE void _Watchdog_Header_initialize(
-  Watchdog_Header *header
+RTEMS_INLINE_ROUTINE void _Watchdog_Per_CPU_tickle_absolute(
+  Per_CPU_Control *cpu,
+  uint64_t         now
 )
 {
-  _ISR_lock_Initialize( &header->Lock, "Watchdog" );
-  _Chain_Initialize_empty( &header->Watchdogs );
-  _Chain_Initialize_empty( &header->Iterators );
+  ISR_lock_Context lock_context;
+
+  _ISR_lock_ISR_disable_and_acquire( &cpu->Watchdog.Lock, &lock_context );
+  _Watchdog_Tickle(
+    &cpu->Watchdog.Header[ PER_CPU_WATCHDOG_ABSOLUTE ],
+    now,
+    &cpu->Watchdog.Lock,
+    &lock_context
+  );
 }
 
 /** @} */
diff --git a/cpukit/score/src/condition.c b/cpukit/score/src/condition.c
index 22c2a9b97c..420681955a 100644
--- a/cpukit/score/src/condition.c
+++ b/cpukit/score/src/condition.c
@@ -282,7 +282,7 @@ static int _Condition_Wake( struct _Condition_Control *_condition, int count )
 
       next = _Chain_Next( node );
       thread = THREAD_CHAIN_NODE_TO_THREAD( node );
-      _Watchdog_Remove_ticks( &thread->Timer );
+      _Thread_Timer_remove( thread );
       _Thread_Unblock( thread );
 
       node = next;
diff --git a/cpukit/score/src/coretodset.c b/cpukit/score/src/coretodset.c
index 3230179414..b2efc07115 100644
--- a/cpukit/score/src/coretodset.c
+++ b/cpukit/score/src/coretodset.c
@@ -26,30 +26,26 @@ void _TOD_Set_with_timestamp(
   const Timestamp_Control *tod_as_timestamp
 )
 {
-  struct timespec ts;
-  uint32_t nanoseconds;
-  Watchdog_Interval seconds_next;
-  Watchdog_Interval seconds_now;
-  Watchdog_Header *header;
+  struct timespec tod_as_timespec;
+  uint64_t        tod_as_ticks;
+  uint32_t        cpu_count;
+  uint32_t        cpu_index;
 
-  _Timestamp_To_timespec( tod_as_timestamp, &ts );
-  nanoseconds = ts.tv_nsec;
-  seconds_next = ts.tv_sec;
+  _Timestamp_To_timespec( tod_as_timestamp, &tod_as_timespec );
 
   _Thread_Disable_dispatch();
 
-  seconds_now = _TOD_Seconds_since_epoch();
+  _Timecounter_Set_clock( &tod_as_timespec );
 
-  _Timecounter_Set_clock( &ts );
+  tod_as_ticks = _Watchdog_Ticks_from_timespec( &tod_as_timespec );
+  cpu_count = _SMP_Get_processor_count();
 
-  header = &_Watchdog_Seconds_header;
+  for ( cpu_index = 0 ; cpu_index < cpu_count ; ++cpu_index ) {
+    Per_CPU_Control *cpu = _Per_CPU_Get_by_index( cpu_index );
 
-  if ( seconds_next < seconds_now )
-    _Watchdog_Adjust_backward( header, seconds_now - seconds_next );
-  else
-    _Watchdog_Adjust_forward( header, seconds_next - seconds_now );
+    _Watchdog_Per_CPU_tickle_absolute( cpu, tod_as_ticks );
+  }
 
-  _TOD.seconds_trigger = nanoseconds;
   _TOD.is_set = true;
 
   _Thread_Enable_dispatch();
diff --git a/cpukit/score/src/coretodtickle.c b/cpukit/score/src/coretodtickle.c
deleted file mode 100644
index 3d7c71e1c1..0000000000
--- a/cpukit/score/src/coretodtickle.c
+++ /dev/null
@@ -1,36 +0,0 @@
-/**
- * @file
- *
- * @brief Increments time of day at each clock tick
- *
- * @ingroup ScoreTOD
- */
-
-/*  COPYRIGHT (c) 1989-2014.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in the file LICENSE in this distribution or at
- *  http://www.rtems.org/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/score/todimpl.h>
-#include <rtems/score/watchdogimpl.h>
-#include <rtems/config.h>
-
-void _TOD_Tickle_ticks( void )
-{
-  /* Update the counter of ticks since boot */
-  _Watchdog_Ticks_since_boot += 1;
-
-  _TOD.seconds_trigger += rtems_configuration_get_nanoseconds_per_tick();
-  if ( _TOD.seconds_trigger >= 1000000000UL ) {
-    _TOD.seconds_trigger -= 1000000000UL;
-    _Watchdog_Tickle_seconds();
-  }
-}
-
diff --git a/cpukit/score/src/kern_tc.c b/cpukit/score/src/kern_tc.c
index e56c292ea3..316a16f05b 100644
--- a/cpukit/score/src/kern_tc.c
+++ b/cpukit/score/src/kern_tc.c
@@ -1971,6 +1971,7 @@ tc_ticktock(int cnt)
 		return;
 	count = 0;
 #else /* __rtems__ */
+#include <rtems/score/smp.h>
 void
 _Timecounter_Tick(void)
 {
@@ -2021,7 +2022,7 @@ _Timecounter_Tick_simple(uint32_t delta, uint32_t offset,
 
 	_Timecounter_Release(lock_context);
 
-	_Watchdog_Tick(_Per_CPU_Get());
+	_Watchdog_Tick(_Per_CPU_Get_snapshot());
 }
 #endif /* __rtems__ */
 
diff --git a/cpukit/score/src/smp.c b/cpukit/score/src/smp.c
index 9d9507d1b9..85256b044c 100644
--- a/cpukit/score/src/smp.c
+++ b/cpukit/score/src/smp.c
@@ -87,6 +87,7 @@ void _SMP_Handler_initialize( void )
   for ( cpu_index = 0 ; cpu_index < cpu_max; ++cpu_index ) {
     Per_CPU_Control *cpu = _Per_CPU_Get_by_index( cpu_index );
 
+    _ISR_lock_Initialize( &cpu->Watchdog.Lock, "Watchdog" );
     _SMP_ticket_lock_Initialize( &cpu->Lock );
     _SMP_lock_Stats_initialize( &cpu->Lock_stats, "Per-CPU" );
   }
diff --git a/cpukit/score/src/threadinitialize.c b/cpukit/score/src/threadinitialize.c
index a49406f160..0b5fd3a125 100644
--- a/cpukit/score/src/threadinitialize.c
+++ b/cpukit/score/src/threadinitialize.c
@@ -160,6 +160,10 @@ bool _Thread_Initialize(
   the_thread->Start.budget_algorithm = budget_algorithm;
   the_thread->Start.budget_callout   = budget_callout;
 
+  _ISR_lock_Initialize( &the_thread->Timer.Lock, "Thread Timer" );
+  the_thread->Timer.header = &cpu->Watchdog.Header[ PER_CPU_WATCHDOG_RELATIVE ];
+  _Watchdog_Preinitialize( &the_thread->Timer.Watchdog, cpu );
+
   switch ( budget_algorithm ) {
     case THREAD_CPU_BUDGET_ALGORITHM_NONE:
     case THREAD_CPU_BUDGET_ALGORITHM_RESET_TIMESLICE:
diff --git a/cpukit/score/src/threadqenqueue.c b/cpukit/score/src/threadqenqueue.c
index 960783c74c..07618fc800 100644
--- a/cpukit/score/src/threadqenqueue.c
+++ b/cpukit/score/src/threadqenqueue.c
@@ -35,7 +35,7 @@
 
 static void _Thread_queue_Unblock( Thread_Control *the_thread )
 {
-  _Watchdog_Remove_ticks( &the_thread->Timer );
+  _Thread_Timer_remove( the_thread );
   _Thread_Unblock( the_thread );
 
 #if defined(RTEMS_MULTIPROCESSING)
@@ -84,8 +84,12 @@ void _Thread_queue_Enqueue_critical(
    */
   if ( timeout != WATCHDOG_NO_TIMEOUT ) {
     _Thread_Wait_set_timeout_code( the_thread, timeout_code );
-    _Watchdog_Initialize( &the_thread->Timer, _Thread_Timeout, 0, the_thread );
-    _Watchdog_Insert_ticks( &the_thread->Timer, timeout );
+    _Thread_Timer_insert_relative(
+      the_thread,
+      cpu_self,
+      _Thread_Timeout,
+      timeout
+    );
   }
 
   success = _Thread_Wait_flags_try_change(
diff --git a/cpukit/score/src/threadrestart.c b/cpukit/score/src/threadrestart.c
index 03b1fba8ff..d366f97a0f 100644
--- a/cpukit/score/src/threadrestart.c
+++ b/cpukit/score/src/threadrestart.c
@@ -84,7 +84,7 @@ static void _Thread_Make_zombie( Thread_Control *the_thread )
 
   _Thread_Set_state( the_thread, STATES_ZOMBIE );
   _Thread_queue_Extract_with_proxy( the_thread );
-  _Watchdog_Remove_ticks( &the_thread->Timer );
+  _Thread_Timer_remove( the_thread );
 
   _ISR_lock_ISR_disable_and_acquire( &zombies->Lock, &lock_context );
   _Chain_Append_unprotected( &zombies->Chain, &the_thread->Object.Node );
@@ -191,7 +191,9 @@ static void _Thread_Start_life_change_for_executing(
   Thread_Control *executing
 )
 {
-  _Assert( executing->Timer.state == WATCHDOG_INACTIVE );
+  _Assert(
+    _Watchdog_Get_state( &executing->Timer.Watchdog ) == WATCHDOG_INACTIVE
+  );
   _Assert(
     executing->current_state == STATES_READY
       || executing->current_state == STATES_SUSPENDED
@@ -246,7 +248,9 @@ void _Thread_Life_action_handler(
       /* Someone deleted us in the mean-time */
       _Thread_Start_life_change_for_executing( executing );
     } else {
-      _Assert( executing->Timer.state == WATCHDOG_INACTIVE );
+      _Assert(
+        _Watchdog_Get_state( &executing->Timer.Watchdog ) == WATCHDOG_INACTIVE
+      );
       _Assert(
         executing->current_state == STATES_READY
           || executing->current_state == STATES_SUSPENDED
@@ -274,7 +278,7 @@ static void _Thread_Start_life_change(
 
   _Thread_Set_state( the_thread, STATES_RESTARTING );
   _Thread_queue_Extract_with_proxy( the_thread );
-  _Watchdog_Remove_ticks( &the_thread->Timer );
+  _Thread_Timer_remove( the_thread );
   _Thread_Change_priority(
     the_thread,
     priority,
diff --git a/cpukit/score/src/threadtimeout.c b/cpukit/score/src/threadtimeout.c
index 8ecaebde06..59f6bd97f0 100644
--- a/cpukit/score/src/threadtimeout.c
+++ b/cpukit/score/src/threadtimeout.c
@@ -33,7 +33,7 @@ static void _Thread_Do_timeout( Thread_Control *the_thread )
   _Thread_Lock_restore_default( the_thread );
 }
 
-void _Thread_Timeout( Objects_Id id, void *arg )
+void _Thread_Timeout( Watchdog_Control *watchdog )
 {
   Thread_Control    *the_thread;
   void              *thread_lock;
@@ -41,7 +41,7 @@ void _Thread_Timeout( Objects_Id id, void *arg )
   Thread_Wait_flags  wait_flags;
   bool               unblock;
 
-  the_thread = arg;
+  the_thread = RTEMS_CONTAINER_OF( watchdog, Thread_Control, Timer.Watchdog );
   thread_lock = _Thread_Lock_acquire( the_thread, &lock_context );
 
   wait_flags = _Thread_Wait_flags_get( the_thread );
diff --git a/cpukit/score/src/watchdog.c b/cpukit/score/src/watchdog.c
deleted file mode 100644
index 8d172fb8f9..0000000000
--- a/cpukit/score/src/watchdog.c
+++ /dev/null
@@ -1,36 +0,0 @@
-/**
- *  @file
- *
- *  @brief Watchdog Handler Initialization
- *  @ingroup ScoreWatchdog
- */
-
-/*
- *  Watchdog Handler
- *
- *
- *  COPYRIGHT (c) 1989-1999.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in the file LICENSE in this distribution or at
- *  http://www.rtems.org/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/score/watchdogimpl.h>
-
-Watchdog_Header _Watchdog_Ticks_header;
-
-Watchdog_Header _Watchdog_Seconds_header;
-
-void _Watchdog_Handler_initialization( void )
-{
-  _Watchdog_Ticks_since_boot = 0;
-
-  _Watchdog_Header_initialize( &_Watchdog_Ticks_header );
-  _Watchdog_Header_initialize( &_Watchdog_Seconds_header );
-}
diff --git a/cpukit/score/src/watchdogadjust.c b/cpukit/score/src/watchdogadjust.c
deleted file mode 100644
index 32b5f7990e..0000000000
--- a/cpukit/score/src/watchdogadjust.c
+++ /dev/null
@@ -1,80 +0,0 @@
-/**
- *  @file
- *
- *  @brief Watchdog Adjust
- *  @ingroup ScoreWatchdog
- */
-
-/*
- *  COPYRIGHT (c) 1989-1999.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in the file LICENSE in this distribution or at
- *  http://www.rtems.org/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/score/watchdogimpl.h>
-
-void _Watchdog_Adjust_backward_locked(
-  Watchdog_Header   *header,
-  Watchdog_Interval  units
-)
-{
-  if ( !_Watchdog_Is_empty( header ) ) {
-     _Watchdog_First( header )->delta_interval += units;
-  }
-}
-
-void _Watchdog_Adjust_backward(
-  Watchdog_Header   *header,
-  Watchdog_Interval  units
-)
-{
-  ISR_lock_Context lock_context;
-
-  _Watchdog_Acquire( header, &lock_context );
-  _Watchdog_Adjust_backward_locked( header, units );
-  _Watchdog_Release( header, &lock_context );
-}
-
-void _Watchdog_Adjust_forward_locked(
-  Watchdog_Header   *header,
-  Watchdog_Interval  units,
-  ISR_lock_Context  *lock_context
-)
-{
-  while ( !_Watchdog_Is_empty( header ) && units > 0 ) {
-    Watchdog_Control *first = _Watchdog_First( header );
-
-    if ( units < first->delta_interval ) {
-      first->delta_interval -= units;
-      break;
-    } else {
-      units -= first->delta_interval;
-      first->delta_interval = 1;
-
-      _Watchdog_Release( header, lock_context );
-
-      _Watchdog_Tickle( header );
-
-      _Watchdog_Acquire( header, lock_context );
-    }
-  }
-}
-
-void _Watchdog_Adjust_forward(
-  Watchdog_Header   *header,
-  Watchdog_Interval  units
-)
-{
-  ISR_lock_Context lock_context;
-
-  _Watchdog_Acquire( header, &lock_context );
-  _Watchdog_Adjust_forward_locked( header, units, &lock_context );
-  _Watchdog_Release( header, &lock_context );
-}
diff --git a/cpukit/score/src/watchdoginsert.c b/cpukit/score/src/watchdoginsert.c
index db15f55b0d..22fc7a5f76 100644
--- a/cpukit/score/src/watchdoginsert.c
+++ b/cpukit/score/src/watchdoginsert.c
@@ -1,17 +1,22 @@
 /**
- * @file 
+ * @file
  *
  * @brief Watchdog Insert
  * @ingroup ScoreWatchdog
  */
- 
+
 /*
- *  COPYRIGHT (c) 1989-1999.
- *  On-Line Applications Research Corporation (OAR).
+ * Copyright (c) 2016 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@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.org/license/LICENSE.
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.org/license/LICENSE.
  */
 
 #if HAVE_CONFIG_H
@@ -20,106 +25,41 @@
 
 #include <rtems/score/watchdogimpl.h>
 
-static void _Watchdog_Insert_fixup(
-  Watchdog_Header   *header,
-  Watchdog_Control  *the_watchdog,
-  Watchdog_Interval  delta,
-  Watchdog_Control  *next_watchdog,
-  Watchdog_Interval  delta_next
-)
-{
-  const Chain_Node *iterator_tail;
-  Chain_Node       *iterator_node;
-
-  next_watchdog->delta_interval = delta_next - delta;
-
-  iterator_node = _Chain_First( &header->Iterators );
-  iterator_tail = _Chain_Immutable_tail( &header->Iterators );
-
-  while ( iterator_node != iterator_tail ) {
-    Watchdog_Iterator *iterator;
-
-    iterator = (Watchdog_Iterator *) iterator_node;
-
-    if ( iterator->current == &next_watchdog->Node ) {
-      iterator->current = &the_watchdog->Node;
-    }
-
-    iterator_node = _Chain_Next( iterator_node );
-  }
-}
-
-void _Watchdog_Insert_locked(
+void _Watchdog_Insert(
   Watchdog_Header  *header,
   Watchdog_Control *the_watchdog,
-  ISR_lock_Context *lock_context
+  uint64_t          expire
 )
 {
-  if ( the_watchdog->state == WATCHDOG_INACTIVE ) {
-    Watchdog_Iterator  iterator;
-    Chain_Node        *current;
-    Chain_Node        *next;
-    Watchdog_Interval  delta;
-
-    the_watchdog->state = WATCHDOG_BEING_INSERTED;
-
-    _Chain_Append_unprotected( &header->Iterators, &iterator.Node );
-
-    delta = the_watchdog->initial;
-    current = _Chain_Head( &header->Watchdogs );
+  RBTree_Node **link;
+  RBTree_Node  *parent;
+  RBTree_Node  *old_first;
+  RBTree_Node  *new_first;
 
-    while (
-      ( next = _Chain_Next( current ) ) != _Chain_Tail( &header->Watchdogs )
-    ) {
-      Watchdog_Control  *next_watchdog;
-      Watchdog_Interval  delta_next;
+  _Assert( _Watchdog_Get_state( the_watchdog ) == WATCHDOG_INACTIVE );
 
-      next_watchdog = (Watchdog_Control *) next;
-      delta_next = next_watchdog->delta_interval;
+  link = _RBTree_Root_reference( &header->Watchdogs );
+  parent = NULL;
+  old_first = header->first;
+  new_first = &the_watchdog->Node.RBTree;
 
-      if ( delta < delta_next ) {
-        _Watchdog_Insert_fixup(
-          header,
-          the_watchdog,
-          delta,
-          next_watchdog,
-          delta_next
-        );
-        break;
-      }
+  the_watchdog->expire = expire;
 
-      iterator.delta_interval = delta - delta_next;
-      iterator.current = next;
+  while ( *link != NULL ) {
+    Watchdog_Control *parent_watchdog;
 
-      _Watchdog_Flash( header, lock_context );
+    parent = *link;
+    parent_watchdog = (Watchdog_Control *) parent;
 
-      if ( the_watchdog->state != WATCHDOG_BEING_INSERTED ) {
-        goto abort_insert;
-      }
-
-      delta = iterator.delta_interval;
-      current = iterator.current;
+    if ( expire < parent_watchdog->expire ) {
+      link = _RBTree_Left_reference( parent );
+    } else {
+      link = _RBTree_Right_reference( parent );
+      new_first = old_first;
     }
-
-    the_watchdog->delta_interval = delta;
-    the_watchdog->start_time = _Watchdog_Ticks_since_boot;
-    _Watchdog_Activate( the_watchdog );
-    _Chain_Insert_unprotected( current, &the_watchdog->Node );
-
-abort_insert:
-
-    _Chain_Extract_unprotected( &iterator.Node );
   }
-}
-
-void _Watchdog_Insert(
-  Watchdog_Header  *header,
-  Watchdog_Control *the_watchdog
-)
-{
-  ISR_lock_Context lock_context;
 
-  _Watchdog_Acquire( header, &lock_context );
-  _Watchdog_Insert_locked( header, the_watchdog, &lock_context );
-  _Watchdog_Release( header, &lock_context );
+  header->first = new_first;
+  _RBTree_Add_child( &the_watchdog->Node.RBTree, parent, link );
+  _RBTree_Insert_color( &header->Watchdogs, &the_watchdog->Node.RBTree );
 }
diff --git a/cpukit/score/src/watchdogremove.c b/cpukit/score/src/watchdogremove.c
index 2aa72a4e75..2605a67fa4 100644
--- a/cpukit/score/src/watchdogremove.c
+++ b/cpukit/score/src/watchdogremove.c
@@ -1,17 +1,22 @@
 /**
  * @file
  *
- * @brief Remove Watchdog from List
+ * @brief Remove Watchdog
  * @ingroup ScoreWatchdog
  */
 
 /*
- *  COPYRIGHT (c) 1989-1999.
- *  On-Line Applications Research Corporation (OAR).
+ * Copyright (c) 2016 embedded brains GmbH.  All rights reserved.
  *
- *  The license and distribution terms for this file may be
- *  found in the file LICENSE in this distribution or at
- *  http://www.rtems.org/license/LICENSE.
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@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.org/license/LICENSE.
  */
 
 #if HAVE_CONFIG_H
@@ -19,161 +24,18 @@
 #endif
 
 #include <rtems/score/watchdogimpl.h>
-#include <rtems/score/assert.h>
-
-static void _Watchdog_Remove_it(
-  Watchdog_Header   *header,
-  Watchdog_Control  *the_watchdog
-)
-{
-  Chain_Node        *next;
-  Watchdog_Interval  delta;
-  const Chain_Node  *iterator_tail;
-  Chain_Node        *iterator_node;
-
-  _Assert( the_watchdog->state == WATCHDOG_ACTIVE );
-
-  the_watchdog->state = WATCHDOG_INACTIVE;
-  the_watchdog->stop_time = _Watchdog_Ticks_since_boot;
-
-  next = _Chain_Next( &the_watchdog->Node );
-  delta = the_watchdog->delta_interval;
-
-  if ( next != _Chain_Tail( &header->Watchdogs ) ) {
-    Watchdog_Control *next_watchdog;
-
-    next_watchdog = (Watchdog_Control *) next;
-    next_watchdog->delta_interval += delta;
-  }
-
-  _Chain_Extract_unprotected( &the_watchdog->Node );
-
-  iterator_node = _Chain_First( &header->Iterators );
-  iterator_tail = _Chain_Immutable_tail( &header->Iterators );
-
-  while ( iterator_node != iterator_tail ) {
-    Watchdog_Iterator *iterator;
-
-    iterator = (Watchdog_Iterator *) iterator_node;
-
-    if ( iterator->current == next ) {
-      iterator->delta_interval += delta;
-    }
-
-    if ( iterator->current == &the_watchdog->Node ) {
-      Chain_Node *previous = _Chain_Previous( &the_watchdog->Node );
-
-      iterator->current = previous;
-
-      if ( previous != _Chain_Head( &header->Watchdogs ) ) {
-        Watchdog_Control *previous_watchdog;
-
-        previous_watchdog = (Watchdog_Control *) previous;
-        iterator->delta_interval += previous_watchdog->delta_interval;
-      }
-    }
-
-    iterator_node = _Chain_Next( iterator_node );
-  }
-}
 
-Watchdog_States _Watchdog_Remove(
+void _Watchdog_Remove(
   Watchdog_Header  *header,
   Watchdog_Control *the_watchdog
 )
 {
-  ISR_lock_Context  lock_context;
-  Watchdog_States   previous_state;
-  Watchdog_Interval now;
-
-  _Watchdog_Acquire( header, &lock_context );
-  previous_state = the_watchdog->state;
-  switch ( previous_state ) {
-    case WATCHDOG_INACTIVE:
-      break;
-
-    case WATCHDOG_BEING_INSERTED:
-
-      /*
-       *  It is not actually on the chain so just change the state and
-       *  the Insert operation we interrupted will be aborted.
-       */
-      the_watchdog->state = WATCHDOG_INACTIVE;
-      now = _Watchdog_Ticks_since_boot;
-      the_watchdog->start_time = now;
-      the_watchdog->stop_time = now;
-      break;
-
-    case WATCHDOG_ACTIVE:
-      _Watchdog_Remove_it( header, the_watchdog );
-      break;
-  }
-
-  _Watchdog_Release( header, &lock_context );
-  return( previous_state );
-}
-
-void _Watchdog_Tickle(
-  Watchdog_Header *header
-)
-{
-  ISR_lock_Context lock_context;
-
-  _Watchdog_Acquire( header, &lock_context );
-
-  if ( !_Watchdog_Is_empty( header ) ) {
-    Watchdog_Control  *first;
-    Watchdog_Interval  delta;
-
-    first = _Watchdog_First( header );
-    delta = first->delta_interval;
-
-    /*
-     * Although it is forbidden to insert watchdogs with a delta interval of
-     * zero it is possible to observe watchdogs with a delta interval of zero
-     * at this point.  For example lets have a watchdog chain of one watchdog
-     * with a delta interval of one and insert a new one with an initial value
-     * of one.  At the start of the insert procedure it will advance one step
-     * and reduce its delta interval by one yielding zero.  Now a tick happens.
-     * This will remove the watchdog on the chain and update the insert
-     * iterator.  Now the insert operation continues and will insert the new
-     * watchdog with a delta interval of zero.
-     */
-    if ( delta > 0 ) {
-      --delta;
-      first->delta_interval = delta;
+  if ( _Watchdog_Is_scheduled( the_watchdog ) ) {
+    if ( header->first == &the_watchdog->Node.RBTree ) {
+      _Watchdog_Next_first( header, the_watchdog );
     }
 
-    while ( delta == 0 ) {
-      bool                            run;
-      Watchdog_Service_routine_entry  routine;
-      Objects_Id                      id;
-      void                           *user_data;
-
-      run = ( first->state == WATCHDOG_ACTIVE );
-
-      _Watchdog_Remove_it( header, first );
-
-      routine = first->routine;
-      id = first->id;
-      user_data = first->user_data;
-
-      _Watchdog_Release( header, &lock_context );
-
-      if ( run ) {
-        (*routine)( id, user_data );
-      }
-
-      _Watchdog_Acquire( header, &lock_context );
-
-      if ( _Watchdog_Is_empty( header ) ) {
-        break;
-      }
-
-      first = _Watchdog_First( header );
-      delta = first->delta_interval;
-    }
+    _RBTree_Extract( &header->Watchdogs, &the_watchdog->Node.RBTree );
+    _Watchdog_Set_state( the_watchdog, WATCHDOG_INACTIVE );
   }
-
-  _Watchdog_Release( header, &lock_context );
 }
diff --git a/cpukit/score/src/watchdogtick.c b/cpukit/score/src/watchdogtick.c
index e89c088350..ab7eadcf1d 100644
--- a/cpukit/score/src/watchdogtick.c
+++ b/cpukit/score/src/watchdogtick.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2015, 2016 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -12,25 +12,81 @@
  * http://www.rtems.org/license/LICENSE.
  */
 
-#include <rtems/score/assert.h>
+#if HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <rtems/score/watchdogimpl.h>
 #include <rtems/score/schedulerimpl.h>
 #include <rtems/score/threaddispatch.h>
-#include <rtems/score/todimpl.h>
-#include <rtems/score/watchdogimpl.h>
+#include <rtems/score/timecounter.h>
 
-#if HAVE_CONFIG_H
-#include "config.h"
+void _Watchdog_Do_tickle(
+  Watchdog_Header  *header,
+  uint64_t          now,
+#ifdef RTEMS_SMP
+  ISR_lock_Control *lock,
 #endif
+  ISR_lock_Context *lock_context
+)
+{
+  while ( true ) {
+    Watchdog_Control *the_watchdog;
+
+    the_watchdog = (Watchdog_Control *) header->first;
+
+    if ( the_watchdog == NULL ) {
+      break;
+    }
+
+    if ( the_watchdog->expire <= now ) {
+      Watchdog_Service_routine_entry routine;
+
+      _Watchdog_Next_first( header, the_watchdog );
+      _RBTree_Extract( &header->Watchdogs, &the_watchdog->Node.RBTree );
+      _Watchdog_Set_state( the_watchdog, WATCHDOG_INACTIVE );
+      routine = the_watchdog->routine;
+
+      _ISR_lock_Release_and_ISR_enable( lock, lock_context );
+      ( *routine )( the_watchdog );
+      _ISR_lock_ISR_disable_and_acquire( lock, lock_context );
+    } else {
+      break;
+    }
+  }
+
+  _ISR_lock_Release_and_ISR_enable( lock, lock_context );
+}
 
 void _Watchdog_Tick( Per_CPU_Control *cpu )
 {
-  _Assert( !_Thread_Dispatch_is_enabled() );
+  ISR_lock_Context lock_context;
+  uint64_t         ticks;
+  struct timespec  now;
 
   if ( _Per_CPU_Is_boot_processor( cpu ) ) {
-    _TOD_Tickle_ticks();
+    ++_Watchdog_Ticks_since_boot;
+  }
 
-    _Watchdog_Tickle_ticks();
+  _ISR_lock_ISR_disable_and_acquire( &cpu->Watchdog.Lock, &lock_context );
 
-    _Scheduler_Tick();
-  }
+  ticks = cpu->Watchdog.ticks;
+  _Assert( ticks < UINT64_MAX );
+  ++ticks;
+  cpu->Watchdog.ticks = ticks;
+
+  _Watchdog_Tickle(
+    &cpu->Watchdog.Header[ PER_CPU_WATCHDOG_RELATIVE ],
+    ticks,
+    &cpu->Watchdog.Lock,
+    &lock_context
+  );
+
+  _Timecounter_Getnanotime( &now );
+  _Watchdog_Per_CPU_tickle_absolute(
+    cpu,
+    _Watchdog_Ticks_from_timespec( &now )
+  );
+
+  _Scheduler_Tick( cpu );
 }