clock: add clock_resynchronize and use subseconds RTC

Add clock_resynchronize for better synchronization of CLOCK_REALTIME and CLOCK_MONOTONIC to match RTC after resume from low-power state.

Add up_rtc_getdatetime_with_subseconds under CONFIG_ARCH_HAVE_RTC_SUBSECONDS to allow initializing (and resynchronizing) system clock with subseconds accuracy RTC.

arch/Kconfig

@@ -190,6 +190,10 @@ config ARCH_HAVE_RESET
 	bool
 	default n
 
+config ARCH_HAVE_RTC_SUBSECONDS
+	bool
+	default n
+
 config ARCH_USE_MMU
 	bool "Enable MMU"
 	default n

arch/arm/src/stm32/Kconfig

@@ -6362,6 +6362,7 @@ config STM32_HAVE_RTC_COUNTER
 
 config STM32_HAVE_RTC_SUBSECONDS
 	bool
+	select ARCH_HAVE_RTC_SUBSECONDS
 	default n
 
 config RTC_MAGIC_REG

arch/arm/src/stm32/stm32_rtcc.c

@@ -928,6 +928,40 @@ int up_rtc_getdatetime(FAR struct tm *tp)
 }
 #endif
 
+/************************************************************************************
+ * Name: up_rtc_getdatetime_with_subseconds
+ *
+ * Description:
+ *   Get the current date and time from the date/time RTC.  This interface
+ *   is only supported by the date/time RTC hardware implementation.
+ *   It is used to replace the system timer.  It is only used by the RTOS during
+ *   initialization to set up the system time when CONFIG_RTC and CONFIG_RTC_DATETIME
+ *   are selected (and CONFIG_RTC_HIRES is not).
+ *
+ *   NOTE: This interface exposes sub-second accuracy capability of RTC hardware.
+ *   This interface allow maintaining timing accuracy when system time needs constant
+ *   resynchronization with RTC, for example on MCU with low-power state that
+ *   stop system timer.
+ *
+ * Input Parameters:
+ *   tp - The location to return the high resolution time value.
+ *   nsec - The location to return the subsecond time value.
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno on failure
+ *
+ ************************************************************************************/
+
+#ifdef CONFIG_ARCH_HAVE_RTC_SUBSECONDS
+#  ifndef CONFIG_STM32_HAVE_RTC_SUBSECONDS
+#    error "Invalid config, enable CONFIG_STM32_HAVE_RTC_SUBSECONDS."
+#  endif
+int up_rtc_getdatetime_with_subseconds(FAR struct tm *tp, FAR long *nsec)
+{
+  return stm32_rtc_getdatetime_with_subseconds(tp, nsec);
+}
+#endif
+
 /************************************************************************************
  * Name: stm32_rtc_setdatetime
  *

arch/arm/src/stm32f7/Kconfig

@@ -1821,6 +1821,7 @@ config STM32F7_HAVE_RTC_COUNTER
 
 config STM32F7_HAVE_RTC_SUBSECONDS
 	bool
+	select ARCH_HAVE_RTC_SUBSECONDS
 	default y
 
 config RTC_MAGIC_REG

arch/arm/src/stm32f7/stm32_rtc.c

@@ -1178,7 +1178,7 @@ int up_rtc_getdatetime(FAR struct tm *tp)
     }
 
   rtc_dumptime((FAR const struct tm *)tp, &usecs, "Returning");
-#else /* CONFIG_STM32_HAVE_RTC_SUBSECONDS */
+#else /* CONFIG_STM32F7_HAVE_RTC_SUBSECONDS */
   rtc_dumptime((FAR const struct tm *)tp, NULL, "Returning");
 #endif
 
@@ -1215,6 +1215,40 @@ int up_rtc_getdatetime(FAR struct tm *tp)
 }
 #endif
 
+/************************************************************************************
+ * Name: up_rtc_getdatetime_with_subseconds
+ *
+ * Description:
+ *   Get the current date and time from the date/time RTC.  This interface
+ *   is only supported by the date/time RTC hardware implementation.
+ *   It is used to replace the system timer.  It is only used by the RTOS during
+ *   initialization to set up the system time when CONFIG_RTC and CONFIG_RTC_DATETIME
+ *   are selected (and CONFIG_RTC_HIRES is not).
+ *
+ *   NOTE: This interface exposes sub-second accuracy capability of RTC hardware.
+ *   This interface allow maintaining timing accuracy when system time needs constant
+ *   resynchronization with RTC, for example with board level power-save mode utilizing
+ *   deep-sleep modes such as STOP on STM32 MCUs.
+ *
+ * Input Parameters:
+ *   tp - The location to return the high resolution time value.
+ *   nsec - The location to return the subsecond time value.
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno on failure
+ *
+ ************************************************************************************/
+
+#ifdef CONFIG_ARCH_HAVE_RTC_SUBSECONDS
+#  ifndef CONFIG_STM32F7_HAVE_RTC_SUBSECONDS
+#    error "Invalid config, enable CONFIG_STM32F7_HAVE_RTC_SUBSECONDS."
+#  endif
+int up_rtc_getdatetime_with_subseconds(FAR struct tm *tp, FAR long *nsec)
+{
+  return stm32_rtc_getdatetime_with_subseconds(tp, nsec);
+}
+#endif
+
 /****************************************************************************
  * Name: stm32_rtc_setdatetime
  *

include/nuttx/arch.h

@@ -2185,6 +2185,35 @@ int up_rtc_gettime(FAR struct timespec *tp);
 int up_rtc_getdatetime(FAR struct tm *tp);
 #endif
 
+/************************************************************************************
+ * Name: up_rtc_getdatetime_with_subseconds
+ *
+ * Description:
+ *   Get the current date and time from the date/time RTC.  This interface
+ *   is only supported by the date/time RTC hardware implementation.
+ *   It is used to replace the system timer.  It is only used by the RTOS during
+ *   initialization to set up the system time when CONFIG_RTC and CONFIG_RTC_DATETIME
+ *   are selected (and CONFIG_RTC_HIRES is not).
+ *
+ *   NOTE: This interface exposes sub-second accuracy capability of RTC hardware.
+ *   This interface allow maintaining timing accuracy when system time needs constant
+ *   resynchronization with RTC, for example on MCU with low-power state that
+ *   stop system timer.
+ *
+ * Input Parameters:
+ *   tp - The location to return the high resolution time value.
+ *   nsec - The location to return the subsecond time value.
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno on failure
+ *
+ ************************************************************************************/
+
+#if defined(CONFIG_RTC) && defined(CONFIG_RTC_DATETIME) && \
+    defined(CONFIG_ARCH_HAVE_RTC_SUBSECONDS)
+int up_rtc_getdatetime_with_subseconds(FAR struct tm *tp, FAR long *nsec);
+#endif
+
 /************************************************************************************
  * Name: up_rtc_settime
  *

include/nuttx/clock.h

@@ -1,7 +1,7 @@
 /****************************************************************************
  * include/nuttx/clock.h
  *
- *   Copyright (C) 2007-2009, 2011-2012, 2014, 2016 Gregory Nutt.
+ *   Copyright (C) 2007-2009, 2011-2012, 2014, 2016-2017 Gregory Nutt.
              All rights reserved.
  *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
@@ -50,6 +50,7 @@
 /****************************************************************************
  * Pre-processor Definitions
  ****************************************************************************/
+
 /* Configuration ************************************************************/
 /* Efficient, direct access to OS global timer variables will be supported
  * if the execution environment has direct access to kernel global data.
@@ -261,6 +262,37 @@ EXTERN volatile systime_t g_system_timer;
 void clock_synchronize(void);
 #endif
 
+/****************************************************************************
+ * Name: clock_resynchronize
+ *
+ * Description:
+ *   Resynchronize the system timer to a hardware RTC.  The user can
+ *   explicitly re-synchronize the system timer to the RTC under certain
+ *   conditions where the system timer is known to be in error.  For example,
+ *   in certain low-power states, the system timer may be stopped but the
+ *   RTC will continue keep correct time.  After recovering from such
+ *   low-power state, this function should be called to restore the correct
+ *   system time. Function also keeps monotonic clock at rate of RTC.
+ *
+ *   Calling this function will not result in system time going "backward" in
+ *   time. If setting system time with RTC would result time going "backward"
+ *   then resynchronization is not performed.
+ *
+ * Parameters:
+ *   diff:  amount of time system-time is adjusted forward with RTC
+ *
+ * Return Value:
+ *   None
+ *
+ * Assumptions:
+ *
+ ****************************************************************************/
+
+#if defined(CONFIG_RTC) && !defined(CONFIG_SCHED_TICKLESS) && \
+    !defined(CONFIG_CLOCK_TIMEKEEPING)
+void clock_resynchronize(FAR struct timespec *rtc_diff);
+#endif
+
 /****************************************************************************
  * Function:  clock_systimer
  *

sched/clock/clock.h

@@ -1,7 +1,7 @@
 /****************************************************************************
  * sched/clock/clock.h
  *
- *   Copyright (C) 2007-2009, 2014 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2007-2009, 2014, 2017 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -81,6 +81,10 @@ extern volatile uint32_t g_system_timer;
 
 #ifndef CONFIG_CLOCK_TIMEKEEPING
 extern struct timespec   g_basetime;
+
+#ifdef CONFIG_CLOCK_MONOTONIC
+extern struct timespec   g_monotonic_basetime;
+#endif
 #endif
 
 /****************************************************************************

sched/clock/clock_gettime.c

@@ -100,6 +100,33 @@ int clock_gettime(clockid_t clock_id, struct timespec *tp)
 #else
       ret = clock_systimespec(tp);
 #endif
+
+#ifndef CONFIG_CLOCK_TIMEKEEPING
+      if (ret == OK)
+        {
+          irqstate_t flags;
+
+          /* Add the offset time to this. The offset time allows
+           * CLOCK_MONOTONIC be introduced additional increases to systime.
+           */
+
+          flags = enter_critical_section();
+
+          tp->tv_sec  += (uint32_t)g_monotonic_basetime.tv_sec;
+          tp->tv_nsec += (uint32_t)g_monotonic_basetime.tv_nsec;
+
+          leave_critical_section(flags);
+
+          /* Handle carry to seconds. */
+
+          if (tp->tv_nsec >= NSEC_PER_SEC)
+            {
+              carry        = tp->tv_nsec / NSEC_PER_SEC;
+              tp->tv_sec  += carry;
+              tp->tv_nsec -= (carry * NSEC_PER_SEC);
+            }
+        }
+#endif /* CONFIG_CLOCK_TIMEKEEPING */
     }
   else
 #endif
@@ -129,14 +156,20 @@ int clock_gettime(clockid_t clock_id, struct timespec *tp)
 #ifndef CONFIG_CLOCK_TIMEKEEPING
       if (ret == OK)
         {
+          irqstate_t flags;
+
           /* Add the base time to this.  The base time is the time-of-day
            * setting.  When added to the elapsed time since the time-of-day
            * was last set, this gives us the current time.
            */
 
+          flags = enter_critical_section();
+
           ts.tv_sec  += (uint32_t)g_basetime.tv_sec;
           ts.tv_nsec += (uint32_t)g_basetime.tv_nsec;
 
+          leave_critical_section(flags);
+
           /* Handle carry to seconds. */
 
           if (ts.tv_nsec >= NSEC_PER_SEC)

sched/clock/clock_initialize.c

@@ -1,7 +1,7 @@
 /****************************************************************************
  * sched/clock/clock_initialize.c
  *
- *   Copyright (C) 2007, 2009, 2011-2012 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2007, 2009, 2011-2012, 2017 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <gnutt@nuttx.org>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -80,8 +80,14 @@ volatile uint32_t g_system_timer;
 #endif
 #endif
 
+#ifndef CONFIG_CLOCK_TIMEKEEPING
 struct timespec   g_basetime;
 
+#ifdef CONFIG_CLOCK_MONOTONIC
+struct timespec   g_monotonic_basetime;
+#endif
+#endif
+
 /****************************************************************************
  * Private Functions
  ****************************************************************************/
@@ -101,17 +107,22 @@ struct timespec   g_basetime;
 static inline int clock_basetime(FAR struct timespec *tp)
 {
   struct tm rtctime;
+  long nsecs = 0;
   int ret;
 
   /* Get the broken-out time from the date/time RTC. */
 
+#ifdef CONFIG_ARCH_HAVE_RTC_SUBSECONDS
+  ret = up_rtc_getdatetime_with_subseconds(&rtctime, &nsecs);
+#else
   ret = up_rtc_getdatetime(&rtctime);
+#endif
   if (ret >= 0)
     {
       /* And use the broken-out time to initialize the system time */
 
       tp->tv_sec  = mktime(&rtctime);
-      tp->tv_nsec = 0;
+      tp->tv_nsec = nsecs;
     }
 
   return ret;
@@ -256,6 +267,179 @@ void clock_synchronize(void)
 }
 #endif
 
+/****************************************************************************
+ * Name: clock_resynchronize
+ *
+ * Description:
+ *   Resynchronize the system timer to a hardware RTC.  The user can
+ *   explicitly re-synchronize the system timer to the RTC under certain
+ *   conditions where the system timer is known to be in error.  For example,
+ *   in certain low-power states, the system timer may be stopped but the
+ *   RTC will continue keep correct time.  After recovering from such
+ *   low-power state, this function should be called to restore the correct
+ *   system time. Function also keeps monotonic clock at rate of RTC.
+ *
+ *   Calling this function will not result in system time going "backward" in
+ *   time. If setting system time with RTC would result time going "backward"
+ *   then resynchronization is not performed.
+ *
+ * Parameters:
+ *   rtc_diff:  amount of time system-time is adjusted forward with RTC
+ *
+ * Return Value:
+ *   None
+ *
+ * Assumptions:
+ *
+ ****************************************************************************/
+
+#if defined(CONFIG_RTC) && !defined(CONFIG_SCHED_TICKLESS) && \
+    !defined(CONFIG_CLOCK_TIMEKEEPING)
+void clock_resynchronize(FAR struct timespec *rtc_diff)
+{
+  struct timespec rtc_time, bias, curr_ts;
+  struct timespec rtc_diff_tmp;
+  irqstate_t flags;
+  int32_t carry;
+  int ret;
+
+  if (rtc_diff == NULL)
+    {
+      rtc_diff = &rtc_diff_tmp;
+    }
+
+  /* Set the time value to match the RTC */
+
+  flags = enter_critical_section();
+
+  /* Get RTC time */
+
+  ret = clock_basetime(&rtc_time);
+  if (ret < 0)
+    {
+      /* Error reading RTC, skip resynchronization. */
+
+      sinfo("rtc error %d, skip resync\n", ret);
+
+      rtc_diff->tv_sec = 0;
+      rtc_diff->tv_nsec = 0;
+      goto skip;
+    }
+
+  /* Get the elapsed time since power up (in milliseconds).  This is a
+   * bias value that we need to use to correct the base time.
+   */
+
+  (void)clock_systimespec(&bias);
+
+  /* Add the base time to this.  The base time is the time-of-day
+   * setting.  When added to the elapsed time since the time-of-day
+   * was last set, this gives us the current time.
+   */
+
+  curr_ts.tv_sec  = bias.tv_sec + g_basetime.tv_sec;
+  curr_ts.tv_nsec = bias.tv_nsec + g_basetime.tv_nsec;
+
+  /* Handle carry to seconds. */
+
+  if (curr_ts.tv_nsec >= NSEC_PER_SEC)
+    {
+      carry            = curr_ts.tv_nsec / NSEC_PER_SEC;
+      curr_ts.tv_sec  += carry;
+      curr_ts.tv_nsec -= (carry * NSEC_PER_SEC);
+    }
+
+  /* Check if RTC has advanced past system time. */
+
+  if (curr_ts.tv_sec > rtc_time.tv_sec ||
+      (curr_ts.tv_sec == rtc_time.tv_sec && curr_ts.tv_nsec >= rtc_time.tv_nsec))
+    {
+      /* Setting system time with RTC now would result time going
+       * backwards. Skip resynchronization.
+       */
+
+      sinfo("skip resync\n");
+
+      rtc_diff->tv_sec = 0;
+      rtc_diff->tv_nsec = 0;
+    }
+  else
+    {
+      /* Save RTC time as the new base time. */
+
+      g_basetime.tv_sec  = rtc_time.tv_sec;
+      g_basetime.tv_nsec = rtc_time.tv_nsec;
+
+      /* Subtract that bias from the basetime so that when the system
+       * timer is again added to the base time, the result is the current
+       * time relative to basetime.
+       */
+
+      if (g_basetime.tv_nsec < bias.tv_nsec)
+        {
+          g_basetime.tv_nsec += NSEC_PER_SEC;
+          g_basetime.tv_sec--;
+        }
+
+      /* Result could be negative seconds */
+
+      g_basetime.tv_nsec -= bias.tv_nsec;
+      g_basetime.tv_sec  -= bias.tv_sec;
+
+      sinfo("basetime=(%ld,%lu) bias=(%ld,%lu)\n",
+            (long)g_basetime.tv_sec, (unsigned long)g_basetime.tv_nsec,
+            (long)bias.tv_sec, (unsigned long)bias.tv_nsec);
+
+      /* Output difference between time at entry and new current time. */
+
+      rtc_diff->tv_sec = (bias.tv_sec + g_basetime.tv_sec) - curr_ts.tv_sec;
+      rtc_diff->tv_nsec = (bias.tv_nsec + g_basetime.tv_nsec) - curr_ts.tv_nsec;
+
+      /* Handle carry to seconds. */
+
+      if (rtc_diff->tv_nsec < 0)
+        {
+          carry = -((-(rtc_diff->tv_nsec + 1)) / NSEC_PER_SEC + 1);
+        }
+      else if (rtc_diff->tv_nsec >= NSEC_PER_SEC)
+        {
+          carry = rtc_diff->tv_nsec / NSEC_PER_SEC;
+        }
+      else
+        {
+          carry = 0;
+        }
+
+      if (carry)
+        {
+          rtc_diff->tv_sec  += carry;
+          rtc_diff->tv_nsec -= (carry * NSEC_PER_SEC);
+        }
+
+#ifdef CONFIG_CLOCK_MONOTONIC
+      /* Monotonic clock follows wall time since system start-up. Adjust
+       * CLOCK_MONOTONIC same amount as CLOCK_REALTIME.
+       */
+
+      g_monotonic_basetime.tv_sec  += (uint32_t)rtc_diff->tv_sec;
+      g_monotonic_basetime.tv_nsec += (uint32_t)rtc_diff->tv_nsec;
+
+      /* Handle carry to seconds. */
+
+      if (g_monotonic_basetime.tv_nsec >= NSEC_PER_SEC)
+        {
+          carry = g_monotonic_basetime.tv_nsec / NSEC_PER_SEC;
+          g_monotonic_basetime.tv_sec += carry;
+          g_monotonic_basetime.tv_nsec -= (carry * NSEC_PER_SEC);
+        }
+#endif
+    }
+
+skip:
+  leave_critical_section(flags);
+}
+#endif
+
 /****************************************************************************
  * Name: clock_timer
  *