sched: add support for adjtime() interface

This commit adds Linux like adjtime() interface that is used to correct
the system time clock if it varies from real value. The adjustment is
done by slight adjustment of clock period and therefore the adjustment
is without time jumps (both forward and backwards)

The implementation is enabled by CONFIG_CLOCK_ADJTIME and separated from
CONFIG_CLOCK_TIMEKEEPING functions. Options CONFIG_CLOCK_ADJTIME_SLEWLIMIT
and CONFIG_CLOCK_ADJTIME_PERIOD can be used to control the adjustment
speed.

Interfaces up_get_timer_period() and up_adj_timer_period() has to be
defined by architecture level support.

This is not a POSIX interface but derives from 4.3BSD, System V.
It is also supported for Linux compatibility.

Signed-off-by: Michal Lenc <michallenc@seznam.cz>

include/nuttx/arch.h

@@ -1457,6 +1457,36 @@ void up_secure_irq_all(bool secure);
 
 #endif
 
+/****************************************************************************
+ * Function:  up_adj_timer_period
+ *
+ * Description:
+ *   Adjusts timer period. This call is used when adjusting timer period as
+ *   defined in adjtime() function.
+ *
+ * Input Parameters:
+ *   period_inc_usec  - period adjustment in usec (reset to default value
+ *                      if 0)
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_CLOCK_ADJTIME
+void up_adj_timer_period(long long period_inc_usec);
+
+/****************************************************************************
+ * Function:  up_get_timer_period
+ *
+ * Description:
+ *   This function returns the timer period in usec.
+ *
+ * Input Parameters:
+ *   period_usec  - returned timer period in usec
+ *
+ ****************************************************************************/
+
+void up_get_timer_period(long long *period_usec);
+#endif
+
 /****************************************************************************
  * Function:  up_timer_initialize
  *

include/sys/time.h

@@ -251,7 +251,7 @@ int settimeofday(FAR const struct timeval *tv,
  *
  ****************************************************************************/
 
-#ifdef CONFIG_CLOCK_TIMEKEEPING
+#if defined(CONFIG_CLOCK_TIMEKEEPING) || defined(CONFIG_CLOCK_ADJTIME)
 int adjtime(FAR const struct timeval *delta, FAR struct timeval *olddelta);
 #endif
 

sched/Kconfig

@@ -184,6 +184,57 @@ config SYSTEM_TIME64
 		and/or if a very long "uptime" is required, then this option can be
 		selected to support a 64-bit wide timer.
 
+config ARCH_HAVE_ADJTIME
+	bool
+	default n
+
+config CLOCK_ADJTIME
+	bool "Support adjtime function"
+	default n
+	depends on ARCH_HAVE_ADJTIME
+	depends on !SCHED_TICKLESS
+	depends on !CLOCK_TIMEKEEPING
+	---help---
+		Enables usage of adjtime() interface used to correct the system time
+		clock. This requires specific architecture support capable of adjusting
+		system timer period.
+
+		Interfaces up_get_timer_period() and up_adj_timer_period() has to be
+		defined by architecture level support.
+
+		This is not a POSIX interface but derives from 4.3BSD, System V.
+		It is also supported for Linux compatibility.
+
+		Currently it is not possible to use adjtime() if SCHED_TICKLESS or
+		CLOCK_TIMEKEEPING is defined.
+
+if CLOCK_ADJTIME
+
+config CLOCK_ADJTIME_SLEWLIMIT
+	int "Adjtime slew limit"
+	default 2
+	range 0 100
+	---help---
+		In real time systems we do not want the time to adjust too quickly.
+		CLOCK_ADJTIME_SLEWLIMIT defines how many seconds can time change
+		during each clock.
+
+		Note that CLOCK_ADJTIME_SLEWLIMIT is divided by 100 in source code,
+		therefore CLOCK_ADJTIME_SLEWLIMIT=2 will result in possible 0.02 second
+		adjustment.
+
+config CLOCK_ADJTIME_PERIOD
+	int "Adjtime period"
+	default 97
+	range 0 100
+	---help---
+		Define system clock adjustment period. Should be between 0.95 and 0.99.
+
+		Note that CLOCK_ADJTIME_PERIOD is divided by 100 in source code,
+		therefore CLOCK_ADJTIME_PERIOD=97 will result in 0.97.
+
+endif
+
 config ARCH_HAVE_TIMEKEEPING
 	bool
 	default n

sched/clock/Make.defs

@@ -27,6 +27,10 @@ ifeq ($(CONFIG_CLOCK_TIMEKEEPING),y)
 CSRCS += clock_timekeeping.c
 endif
 
+ifeq ($(CONFIG_CLOCK_ADJTIME),y)
+CSRCS += clock_adjtime.c
+endif
+
 # Include clock build support
 
 DEPPATH += --dep-path clock

sched/clock/clock.h

@@ -66,6 +66,11 @@ extern volatile clock_t g_system_ticks;
 extern struct timespec  g_basetime;
 #endif
 
+#ifdef CONFIG_CLOCK_ADJTIME
+extern long long g_clk_adj_usec;
+extern long long g_clk_adj_count;
+#endif
+
 /****************************************************************************
  * Public Function Prototypes
  ****************************************************************************/
@@ -79,6 +84,11 @@ void clock_timer(void);
 #  define clock_timer()
 #endif
 
+#ifdef CONFIG_CLOCK_ADJTIME
+void clock_set_adjust(long long adj_usec, long long adj_count,
+                      long long *adj_usec_old, long long *adj_count_old);
+#endif
+
 int  clock_abstime2ticks(clockid_t clockid,
                          FAR const struct timespec *abstime,
                          FAR sclock_t *ticks);

sched/clock/clock_adjtime.c

@@ -0,0 +1,208 @@
+/****************************************************************************
+ * sched/clock/clock_adjtime.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#ifdef CONFIG_CLOCK_ADJTIME
+
+#include <sys/time.h>
+#include <stdint.h>
+#include <time.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/irq.h>
+#include <nuttx/arch.h>
+
+#include "clock/clock.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Current adjtime implementation uses periodic clock tick to adjust clock
+ * period. Therefore this implementation will not work when tickless mode
+ * is enabled by CONFIG_SCHED_TICKLESS=y
+ */
+
+#ifdef CONFIG_SCHED_TICKLESS
+# error CONFIG_CLOCK_ADJTIME is not supported when CONFIG_SCHED_TICKLESS \
+        is enabled!
+#endif
+
+/* Set slew limit. In real time systems we don't want the time to adjust
+ * too quickly. ADJTIME_SLEWLIMIT defines how many seconds can time change
+ * during each clock.
+ */
+
+#define ADJTIME_SLEWLIMIT      (CONFIG_CLOCK_ADJTIME_SLEWLIMIT * 0.01)
+
+/* Define system clock adjustment period. */
+
+#define ADJTIME_PERIOD         (CONFIG_CLOCK_ADJTIME_PERIOD    * 0.01)
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: adjtime
+ *
+ * Description:
+ *   The adjtime() function gradually adjusts the system clock (as returned
+ *   by gettimeofday(2)).  The amount of time by which the clock is to be
+ *   adjusted is specified in the structure pointed to by delta.
+ *
+ *   This structure has the following form:
+ *
+ *     struct timeval
+ *       {
+ *         time_t      tv_sec;     (seconds)
+ *         long        tv_usec;    (microseconds)
+ *       };
+ *
+ *   If the adjustment in delta is positive, then the system clock is
+ *   speeded up by some small percentage until the adjustment has been
+ *   completed.  If the adjustment in delta is negative, then the clock is
+ *   slowed down in a similar fashion.
+ *
+ *   If a clock adjustment from an earlier adjtime() call is already in
+ *   progress at the time of a later adjtime() call, and delta is not NULL
+ *   for the later call, then the earlier adjustment is stopped, but any
+ *   already completed part of that adjustment is not undone.
+ *
+ *   If olddelta is not NULL, then the buffer that it points to is used to
+ *   return the amount of time remaining from any previous adjustment that
+ *   has not yet been completed.
+ *
+ *   NOTE: This is not a POSIX interface but derives from 4.3BSD, System V.
+ *   It is also supported for Linux compatibility.
+ *
+ ****************************************************************************/
+
+int adjtime(FAR const struct timeval *delta, FAR struct timeval *olddelta)
+{
+  irqstate_t flags;
+  long long adjust_usec;
+  long long period_usec;
+  long long adjust_usec_old;
+  long long count;                /* Number of cycles over which
+                                   * we adjust the period
+                                   */
+  long long incr;                 /* Period increment applied on
+                                   * every cycle.
+                                   */
+  long long count_old;            /* Previous number of cycles over which
+                                   * we adjust the period
+                                   */
+  long long incr_old;             /* Previous period increment applied on
+                                   * every cycle.
+                                   */
+  long long incr_limit;
+  int is_negative;
+
+  is_negative = 0;
+
+  if (!delta)
+    {
+      set_errno(EINVAL);
+      return -1;
+    }
+
+  flags = enter_critical_section();
+
+  adjust_usec = (long long)delta->tv_sec * USEC_PER_SEC + delta->tv_usec;
+
+  if (adjust_usec < 0)
+    {
+      adjust_usec = -adjust_usec;
+      is_negative = 1;
+    }
+
+  /* Get period in usec. Target hardware has to provide support for
+   * this function call.
+   */
+
+  up_get_timer_period(&period_usec);
+
+  /* Determine how much we want to adjust timer period and the number
+   * of cycles over which we want to do the adjustment.
+   */
+
+  count = (USEC_PER_SEC * ADJTIME_PERIOD) / period_usec;
+  incr = adjust_usec / count;
+
+  /* Compute maximum possible period increase and check
+   * whether previously computed increase exceeds the maximum
+   * one.
+   */
+
+  incr_limit = ADJTIME_SLEWLIMIT * period_usec;
+  if (incr > incr_limit)
+    {
+      /* It does... limit computed increase and increment count. */
+
+      incr = incr_limit;
+      count = adjust_usec / incr;
+    }
+
+  if (is_negative == 1)
+    {
+      /* Positive or negative? */
+
+      incr = -incr;
+    }
+
+  /* Ignore small differences. */
+
+  if (incr == 0)
+    {
+      count = 0;
+    }
+
+  leave_critical_section(flags);
+
+  /* Initialize clock adjustment and get old adjust values. */
+
+  clock_set_adjust(incr, count, &incr_old, &count_old);
+
+  adjust_usec_old = count_old * incr_old;
+  if (olddelta)
+    {
+      olddelta->tv_sec  = adjust_usec_old / USEC_PER_SEC;
+      olddelta->tv_usec = adjust_usec_old;
+    }
+
+  return OK;
+}
+
+#endif /* CONFIG_CLOCK_ADJTIME */

sched/clock/clock_initialize.c

@@ -59,6 +59,11 @@ volatile uint32_t g_system_ticks = INITIAL_SYSTEM_TIMER_TICKS;
 struct timespec   g_basetime;
 #endif
 
+#ifdef CONFIG_CLOCK_ADJTIME
+long long g_clk_adj_usec;
+long long g_clk_adj_count;
+#endif
+
 /****************************************************************************
  * Private Functions
  ****************************************************************************/
@@ -222,6 +227,12 @@ void clock_initialize(void)
 
   clock_inittime(NULL);
 #endif
+
+#if defined(CONFIG_CLOCK_ADJTIME)
+  g_clk_adj_count = 0;
+  g_clk_adj_usec = 0;
+#endif
+
 #endif
 }
 
@@ -292,7 +303,7 @@ void clock_synchronize(FAR const struct timespec *tp)
  *
  ****************************************************************************/
 
-#if defined(CONFIG_RTC) && !defined(CONFIG_SCHED_TICKLESS)
+#if defined(CONFIG_RTC) && !defined(CONFIG_SCHED_TICKLESS) && !defined(CONFIG_CLOCK_TIMEKEEPING)
 void clock_resynchronize(FAR struct timespec *rtc_diff)
 {
   struct timespec rtc_time;
@@ -403,6 +414,40 @@ skip:
 }
 #endif
 
+/****************************************************************************
+ * Name: clock_set_adjust
+ *
+ * Description:
+ *   This function is called from adjtime() defined in clock_adjtime.c if
+ *   CONFIG_CLOCK_ADJTIME is enabled. It sets global variables g_clk_adj_usec
+ *   and g_clk_adj_count which are used for clock adjustment.
+ *
+ * Input Parameters:
+ *   adj_usec  - period adjustment in usec
+ *   adj_count - number of clock ticks over which we apply adj_usec
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_CLOCK_ADJTIME
+void clock_set_adjust(long long adj_usec, long long adj_count,
+                      long long *adj_usec_old, long long *adj_count_old)
+{
+  /* Get old adjust values. */
+
+  *adj_usec_old  = g_clk_adj_usec;
+  *adj_count_old = g_clk_adj_count;
+
+  /* Set new adjust values. */
+
+  g_clk_adj_usec  = adj_usec;
+  g_clk_adj_count = adj_count;
+
+  /* And change timer period. */
+
+  up_adj_timer_period(g_clk_adj_usec);
+}
+#endif
+
 /****************************************************************************
  * Name: clock_timer
  *
@@ -419,5 +464,25 @@ void clock_timer(void)
   /* Increment the per-tick system counter */
 
   g_system_ticks++;
+
+#ifdef CONFIG_CLOCK_ADJTIME
+  /* Do we apply timer adjustment? */
+
+  if (g_clk_adj_count > 0)
+    {
+      /* Yes, decrement the count each tick. */
+
+      g_clk_adj_count--;
+
+      /* Check if clock adjusment is finished. */
+
+      if (g_clk_adj_count == 0)
+        {
+          /* Yes... reset timer period. */
+
+          up_adj_timer_period(0);
+        }
+    }
+#endif
 }
 #endif

sched/clock/clock_settime.c

@@ -111,6 +111,11 @@ int clock_settime(clockid_t clock_id, FAR const struct timespec *tp)
         }
 #endif
 
+#ifdef CONFIG_CLOCK_ADJTIME
+      g_clk_adj_count = 0;
+      g_clk_adj_usec = 0;
+#endif
+
       leave_critical_section(flags);
 
       sinfo("basetime=(%ld,%lu) bias=(%ld,%lu)\n",