[PATCH] score: Optimize watchdog tickle

[Sebastian Huber]

Avoid unnecessary lock acquire/release operations.  Get realtime via
timecounter only if necessary.

Update #3264.
---
 cpukit/score/include/rtems/score/watchdogimpl.h | 32 ++++-------
 cpukit/score/src/coretodset.c                   | 24 +++++++-
 cpukit/score/src/watchdogtick.c                 | 75 ++++++++++++++-----------
 testsuites/sptests/spwatchdog/init.c            | 10 +++-
 4 files changed, 86 insertions(+), 55 deletions(-)

diff --git a/cpukit/score/include/rtems/score/watchdogimpl.h b/cpukit/score/include/rtems/score/watchdogimpl.h
index f219a70768..f1693d9fda 100644
--- a/cpukit/score/include/rtems/score/watchdogimpl.h
+++ b/cpukit/score/include/rtems/score/watchdogimpl.h
@@ -96,6 +96,13 @@ RTEMS_INLINE_ROUTINE void _Watchdog_Header_initialize(
   header->first = NULL;
 }
 
+RTEMS_INLINE_ROUTINE Watchdog_Control *_Watchdog_Header_first(
+  const Watchdog_Header *header
+)
+{
+  return (Watchdog_Control *) header->first;
+}
+
 RTEMS_INLINE_ROUTINE void _Watchdog_Header_destroy(
   Watchdog_Header *header
 )
@@ -187,6 +194,7 @@ RTEMS_INLINE_ROUTINE void _Watchdog_Initialize(
 
 void _Watchdog_Do_tickle(
   Watchdog_Header  *header,
+  Watchdog_Control *first,
   uint64_t          now,
 #if defined(RTEMS_SMP)
   ISR_lock_Control *lock,
@@ -195,11 +203,11 @@ void _Watchdog_Do_tickle(
 );
 
 #if defined(RTEMS_SMP)
-  #define _Watchdog_Tickle( header, now, lock, lock_context ) \
-    _Watchdog_Do_tickle( header, now, lock, lock_context )
+  #define _Watchdog_Tickle( header, first, now, lock, lock_context ) \
+    _Watchdog_Do_tickle( header, first, now, lock, lock_context )
 #else
-  #define _Watchdog_Tickle( header, now, lock, lock_context ) \
-    _Watchdog_Do_tickle( header, now, lock_context )
+  #define _Watchdog_Tickle( header, first, now, lock, lock_context ) \
+    _Watchdog_Do_tickle( header, first, now, lock_context )
 #endif
 
 /**
@@ -548,22 +556,6 @@ RTEMS_INLINE_ROUTINE void _Watchdog_Per_CPU_remove_realtime(
   );
 }
 
-RTEMS_INLINE_ROUTINE void _Watchdog_Per_CPU_tickle_realtime(
-  Per_CPU_Control *cpu,
-  uint64_t         now
-)
-{
-  ISR_lock_Context lock_context;
-
-  _ISR_lock_ISR_disable_and_acquire( &cpu->Watchdog.Lock, &lock_context );
-  _Watchdog_Tickle(
-    &cpu->Watchdog.Header[ PER_CPU_WATCHDOG_REALTIME ],
-    now,
-    &cpu->Watchdog.Lock,
-    &lock_context
-  );
-}
-
 /** @} */
 
 #ifdef __cplusplus
diff --git a/cpukit/score/src/coretodset.c b/cpukit/score/src/coretodset.c
index fa6407cfaf..787c263643 100644
--- a/cpukit/score/src/coretodset.c
+++ b/cpukit/score/src/coretodset.c
@@ -41,9 +41,29 @@ void _TOD_Set(
   cpu_count = _SMP_Get_processor_count();
 
   for ( cpu_index = 0 ; cpu_index < cpu_count ; ++cpu_index ) {
-    Per_CPU_Control *cpu = _Per_CPU_Get_by_index( cpu_index );
+    Per_CPU_Control  *cpu;
+    Watchdog_Header  *header;
+    ISR_lock_Context  lock_context;
+    Watchdog_Control *first;
 
-    _Watchdog_Per_CPU_tickle_realtime( cpu, tod_as_ticks );
+    cpu = _Per_CPU_Get_by_index( cpu_index );
+    header = &cpu->Watchdog.Header[ PER_CPU_WATCHDOG_REALTIME ];
+
+    _ISR_lock_ISR_disable_and_acquire( &cpu->Watchdog.Lock, &lock_context );
+
+    first = _Watchdog_Header_first( header );
+
+    if ( first != NULL ) {
+      _Watchdog_Tickle(
+        header,
+        first,
+        tod_as_ticks,
+        &cpu->Watchdog.Lock,
+        &lock_context
+      );
+    }
+
+    _ISR_lock_Release_and_ISR_enable( &cpu->Watchdog.Lock, &lock_context );
   }
 
   _TOD.is_set = true;
diff --git a/cpukit/score/src/watchdogtick.c b/cpukit/score/src/watchdogtick.c
index 3384ea60f3..7a5863b824 100644
--- a/cpukit/score/src/watchdogtick.c
+++ b/cpukit/score/src/watchdogtick.c
@@ -23,6 +23,7 @@
 
 void _Watchdog_Do_tickle(
   Watchdog_Header  *header,
+  Watchdog_Control *first,
   uint64_t          now,
 #ifdef RTEMS_SMP
   ISR_lock_Control *lock,
@@ -30,39 +31,33 @@ void _Watchdog_Do_tickle(
   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 ) {
+  do {
+    if ( first->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;
+      _Watchdog_Next_first( header, first );
+      _RBTree_Extract( &header->Watchdogs, &first->Node.RBTree );
+      _Watchdog_Set_state( first, WATCHDOG_INACTIVE );
+      routine = first->routine;
 
       _ISR_lock_Release_and_ISR_enable( lock, lock_context );
-      ( *routine )( the_watchdog );
+      ( *routine )( first );
       _ISR_lock_ISR_disable_and_acquire( lock, lock_context );
     } else {
       break;
     }
-  }
 
-  _ISR_lock_Release_and_ISR_enable( lock, lock_context );
+    first = _Watchdog_Header_first( header );
+  } while ( first != NULL );
 }
 
 void _Watchdog_Tick( Per_CPU_Control *cpu )
 {
-  ISR_lock_Context lock_context;
-  uint64_t         ticks;
-  struct timespec  now;
+  ISR_lock_Context  lock_context;
+  Watchdog_Header  *header;
+  Watchdog_Control *first;
+  uint64_t          ticks;
+  struct timespec   now;
 
   if ( _Per_CPU_Is_boot_processor( cpu ) ) {
     ++_Watchdog_Ticks_since_boot;
@@ -75,18 +70,34 @@ void _Watchdog_Tick( Per_CPU_Control *cpu )
   ++ticks;
   cpu->Watchdog.ticks = ticks;
 
-  _Watchdog_Tickle(
-    &cpu->Watchdog.Header[ PER_CPU_WATCHDOG_MONOTONIC ],
-    ticks,
-    &cpu->Watchdog.Lock,
-    &lock_context
-  );
-
-  _Timecounter_Getnanotime( &now );
-  _Watchdog_Per_CPU_tickle_realtime(
-    cpu,
-    _Watchdog_Realtime_from_timespec( &now )
-  );
+  header = &cpu->Watchdog.Header[ PER_CPU_WATCHDOG_MONOTONIC ];
+  first = _Watchdog_Header_first( header );
+
+  if ( first != NULL ) {
+    _Watchdog_Tickle(
+      header,
+      first,
+      ticks,
+      &cpu->Watchdog.Lock,
+      &lock_context
+    );
+  }
+
+  header = &cpu->Watchdog.Header[ PER_CPU_WATCHDOG_REALTIME ];
+  first = _Watchdog_Header_first( header );
+
+  if ( first != NULL ) {
+    _Timecounter_Getnanotime( &now );
+    _Watchdog_Tickle(
+      header,
+      first,
+      _Watchdog_Realtime_from_timespec( &now ),
+      &cpu->Watchdog.Lock,
+      &lock_context
+    );
+  }
+
+  _ISR_lock_Release_and_ISR_enable( &cpu->Watchdog.Lock, &lock_context );
 
   _Scheduler_Tick( cpu );
 }
diff --git a/testsuites/sptests/spwatchdog/init.c b/testsuites/sptests/spwatchdog/init.c
index 0c7d41fd33..5b6266d033 100644
--- a/testsuites/sptests/spwatchdog/init.c
+++ b/testsuites/sptests/spwatchdog/init.c
@@ -82,10 +82,18 @@ static uint64_t test_watchdog_tick( Watchdog_Header *header, uint64_t now )
 {
   ISR_LOCK_DEFINE( , lock, "Test" )
   ISR_lock_Context lock_context;
+  Watchdog_Control *first;
 
   _ISR_lock_ISR_disable_and_acquire( &lock, &lock_context );
+
   ++now;
-  _Watchdog_Tickle( header, now, &lock, &lock_context );
+  first = _Watchdog_Header_first( header );
+
+  if ( first != NULL ) {
+    _Watchdog_Tickle( header, first, now, &lock, &lock_context );
+  }
+
+  _ISR_lock_Release_and_ISR_enable( &lock, &lock_context );
   _ISR_lock_Destroy( &lock );
 
   return now;
-- 
2.12.3