Merge branch 'feature/adjtime_newlib' into 'master'

newlib: Add adjtime - makes a gradual adjustment the system clock

See merge request idf/esp-idf!2462

components/newlib/test/test_time.c

@@ -51,3 +51,241 @@ TEST_CASE("Reading RTC registers on APP CPU doesn't affect clock", "[newlib]")
 }
 
 #endif // portNUM_PROCESSORS == 2
+
+TEST_CASE("test adjtime function", "[newlib]")
+{
+    struct timeval tv_time;
+    struct timeval tv_delta;
+    struct timeval tv_outdelta;
+
+    TEST_ASSERT_EQUAL(adjtime(NULL, NULL), 0);
+
+    tv_time.tv_sec = 5000;
+    tv_time.tv_usec = 5000;
+    TEST_ASSERT_EQUAL(settimeofday(&tv_time, NULL), 0);
+
+    tv_outdelta.tv_sec = 5;
+    tv_outdelta.tv_usec = 5;
+    TEST_ASSERT_EQUAL(adjtime(NULL, &tv_outdelta), 0);
+    TEST_ASSERT_EQUAL(tv_outdelta.tv_sec,  0);
+    TEST_ASSERT_EQUAL(tv_outdelta.tv_usec, 0);
+
+    tv_delta.tv_sec = INT_MAX / 1000000L;
+    TEST_ASSERT_EQUAL(adjtime(&tv_delta, &tv_outdelta), -1);
+
+    tv_delta.tv_sec = INT_MIN / 1000000L;
+    TEST_ASSERT_EQUAL(adjtime(&tv_delta, &tv_outdelta), -1);
+
+    tv_delta.tv_sec = 0;
+    tv_delta.tv_usec = -900000;
+    TEST_ASSERT_EQUAL(adjtime(&tv_delta, &tv_outdelta), 0);
+    TEST_ASSERT_TRUE(tv_outdelta.tv_usec <= 0);
+
+    tv_delta.tv_sec = 0;
+    tv_delta.tv_usec = 900000;
+    TEST_ASSERT_EQUAL(adjtime(&tv_delta, &tv_outdelta), 0);
+    TEST_ASSERT_TRUE(tv_outdelta.tv_usec >= 0);
+
+    tv_delta.tv_sec = -4;
+    tv_delta.tv_usec = -900000;
+    TEST_ASSERT_EQUAL(adjtime(&tv_delta, &tv_outdelta), 0);
+    TEST_ASSERT_EQUAL(tv_outdelta.tv_sec,  -4);
+    TEST_ASSERT_TRUE(tv_outdelta.tv_usec <= 0);
+
+    // after settimeofday() adjtime() is stopped
+    tv_delta.tv_sec = 15;
+    tv_delta.tv_usec = 900000;
+    TEST_ASSERT_EQUAL(adjtime(&tv_delta, &tv_outdelta), 0);
+    TEST_ASSERT_EQUAL(tv_outdelta.tv_sec,  15);
+    TEST_ASSERT_TRUE(tv_outdelta.tv_usec >= 0);
+
+    TEST_ASSERT_EQUAL(gettimeofday(&tv_time, NULL), 0);
+    TEST_ASSERT_EQUAL(settimeofday(&tv_time, NULL), 0);
+
+    TEST_ASSERT_EQUAL(adjtime(NULL, &tv_outdelta), 0);
+    TEST_ASSERT_EQUAL(tv_outdelta.tv_sec,  0);
+    TEST_ASSERT_EQUAL(tv_outdelta.tv_usec, 0);
+
+    // after gettimeofday() adjtime() is not stopped
+    tv_delta.tv_sec = 15;
+    tv_delta.tv_usec = 900000;
+    TEST_ASSERT_EQUAL(adjtime(&tv_delta, &tv_outdelta), 0);
+    TEST_ASSERT_EQUAL(tv_outdelta.tv_sec,  15);
+    TEST_ASSERT_TRUE(tv_outdelta.tv_usec >= 0);
+
+    TEST_ASSERT_EQUAL(gettimeofday(&tv_time, NULL), 0);
+
+    TEST_ASSERT_EQUAL(adjtime(NULL, &tv_outdelta), 0);
+    TEST_ASSERT_EQUAL(tv_outdelta.tv_sec,  15);
+    TEST_ASSERT_TRUE(tv_outdelta.tv_usec >= 0);
+
+    tv_delta.tv_sec = 1;
+    tv_delta.tv_usec = 0;
+    TEST_ASSERT_EQUAL(adjtime(&tv_delta, NULL), 0);
+    vTaskDelay(1000 / portTICK_PERIOD_MS);
+    TEST_ASSERT_EQUAL(adjtime(NULL, &tv_outdelta), 0);
+    TEST_ASSERT_TRUE(tv_outdelta.tv_sec == 0);
+    // the correction will be equal to (1_000_000us >> 6) = 15_625 us.
+    TEST_ASSERT_TRUE(1000000L - tv_outdelta.tv_usec >= 15600);
+    TEST_ASSERT_TRUE(1000000L - tv_outdelta.tv_usec <= 15650);
+}
+
+static volatile bool exit_flag;
+static bool adjtime_test_result;
+static bool gettimeofday_test_result;
+static uint64_t count_adjtime;
+static uint64_t count_settimeofday;
+static uint64_t count_gettimeofday;
+
+static void adjtimeTask2(void *pvParameters)
+{
+    struct timeval delta = {.tv_sec = 0, .tv_usec = 0};
+    struct timeval outdelta;
+
+    // although exit flag is set in another task, checking (exit_flag == false) is safe
+    while (exit_flag == false) {
+        delta.tv_sec += 1;
+        delta.tv_usec = 900000;
+        if (delta.tv_sec >= 2146) delta.tv_sec = 1;
+        adjtime(&delta, &outdelta);
+        count_adjtime++;
+    }
+    vTaskDelete(NULL);
+}
+
+static void settimeofdayTask2(void *pvParameters)
+{
+    struct timeval tv_time = { .tv_sec = 1520000000, .tv_usec = 900000 };
+
+    // although exit flag is set in another task, checking (exit_flag == false) is safe
+    while (exit_flag == false) {
+        tv_time.tv_sec += 1;
+        settimeofday(&tv_time, NULL);
+        count_settimeofday++;
+        vTaskDelay(1);
+    }
+    vTaskDelete(NULL);
+}
+
+static void gettimeofdayTask2(void *pvParameters)
+{
+    struct timeval tv_time;
+    // although exit flag is set in another task, checking (exit_flag == false) is safe
+    while (exit_flag == false) {
+        gettimeofday(&tv_time, NULL);
+        count_gettimeofday++;
+        vTaskDelay(1);
+    }
+    vTaskDelete(NULL);
+}
+
+TEST_CASE("test for no interlocking adjtime, gettimeofday and settimeofday functions", "[newlib]")
+{
+    TaskHandle_t th[4];
+    exit_flag = false;
+    count_adjtime = 0;
+    count_settimeofday = 0;
+    count_gettimeofday = 0;
+    struct timeval tv_time = { .tv_sec = 1520000000, .tv_usec = 900000 };
+    TEST_ASSERT_EQUAL(settimeofday(&tv_time, NULL), 0);
+
+#ifndef CONFIG_FREERTOS_UNICORE
+    printf("CPU0 and CPU1. Tasks run: 1 - adjtimeTask, 2 - gettimeofdayTask, 3 - settimeofdayTask \n");
+    xTaskCreatePinnedToCore(adjtimeTask2, "adjtimeTask1", 2048, NULL, UNITY_FREERTOS_PRIORITY - 1, &th[0], 0);
+    xTaskCreatePinnedToCore(gettimeofdayTask2, "gettimeofdayTask1", 2048, NULL, UNITY_FREERTOS_PRIORITY - 1, &th[1], 1);
+    xTaskCreatePinnedToCore(settimeofdayTask2, "settimeofdayTask1", 2048, NULL, UNITY_FREERTOS_PRIORITY - 1, &th[2], 0);
+#else
+    printf("Only one CPU. Tasks run: 1 - adjtimeTask, 2 - gettimeofdayTask, 3 - settimeofdayTask\n");
+    xTaskCreate(adjtimeTask2, "adjtimeTask1", 2048, NULL, UNITY_FREERTOS_PRIORITY - 1, &th[0]);
+    xTaskCreate(gettimeofdayTask2, "gettimeofdayTask1", 2048, NULL, UNITY_FREERTOS_PRIORITY - 1, &th[1]);
+    xTaskCreate(settimeofdayTask2, "settimeofdayTask1", 2048, NULL, UNITY_FREERTOS_PRIORITY - 1, &th[2]);
+#endif
+
+    printf("start wait for 10 seconds\n");
+    vTaskDelay(10000 / portTICK_PERIOD_MS);
+
+    // set exit flag to let thread exit
+    exit_flag = true;
+    vTaskDelay(20 / portTICK_PERIOD_MS);
+    printf("count_adjtime %lld, count_settimeofday %lld, count_gettimeofday %lld\n", count_adjtime, count_settimeofday, count_gettimeofday);
+    TEST_ASSERT(count_adjtime > 1000LL && count_settimeofday > 1000LL && count_gettimeofday > 1000LL);
+}
+
+static void adjtimeTask(void *pvParameters)
+{
+    struct timeval delta = {.tv_sec = 0, .tv_usec = 0};
+    struct timeval outdelta = {.tv_sec = 0, .tv_usec = 0};
+
+    // although exit flag is set in another task, checking (exit_flag == false) is safe
+    while (exit_flag == false) {
+        delta.tv_sec = 1000;
+        delta.tv_usec = 0;
+        if(adjtime(&delta, &outdelta) != 0) {
+            adjtime_test_result = true;
+            exit_flag = true;
+        }
+        delta.tv_sec = 0;
+        delta.tv_usec = 1000;
+        if(adjtime(&delta, &outdelta) != 0) {
+            adjtime_test_result = true;
+            exit_flag = true;
+        }
+    }
+    vTaskDelete(NULL);
+}
+
+static void gettimeofdayTask(void *pvParameters)
+{
+    struct timeval tv_time;
+
+    gettimeofday(&tv_time, NULL);
+    uint64_t time_old = (uint64_t)tv_time.tv_sec * 1000000L + tv_time.tv_usec;
+    // although exit flag is set in another task, checking (exit_flag == false) is safe
+    while (exit_flag == false) {
+        gettimeofday(&tv_time, NULL);
+        uint64_t time   = (uint64_t)tv_time.tv_sec * 1000000L + tv_time.tv_usec;
+        if(((time - time_old) > 1000000LL) || (time_old > time)) {
+            printf("ERROR: time jumped for %lld/1000 seconds. No locks. Need to use locks.\n", (time - time_old)/1000000LL);
+            gettimeofday_test_result = true;
+            exit_flag = true;
+        }
+        time_old = time;
+    }
+    vTaskDelete(NULL);
+}
+
+TEST_CASE("test for thread safety adjtime and gettimeofday functions", "[newlib]")
+{
+    TaskHandle_t th[4];
+    exit_flag = false;
+    adjtime_test_result = false;
+    gettimeofday_test_result = false;
+
+    struct timeval tv_time = { .tv_sec = 1520000000, .tv_usec = 900000 };
+    TEST_ASSERT_EQUAL(settimeofday(&tv_time, NULL), 0);
+
+#ifndef CONFIG_FREERTOS_UNICORE
+    printf("CPU0 and CPU1. Tasks run: 1 - adjtimeTask, 2 - gettimeofdayTask\n");
+    xTaskCreatePinnedToCore(adjtimeTask, "adjtimeTask1", 2048, NULL, UNITY_FREERTOS_PRIORITY - 1, &th[0], 0);
+    xTaskCreatePinnedToCore(gettimeofdayTask, "gettimeofdayTask1", 2048, NULL, UNITY_FREERTOS_PRIORITY - 1, &th[1], 1);
+
+    xTaskCreatePinnedToCore(adjtimeTask, "adjtimeTask2", 2048, NULL, UNITY_FREERTOS_PRIORITY - 1, &th[2], 0);
+    xTaskCreatePinnedToCore(gettimeofdayTask, "gettimeofdayTask2", 2048, NULL, UNITY_FREERTOS_PRIORITY - 1, &th[3], 1);
+#else
+    printf("Only one CPU. Tasks run: 1 - adjtimeTask, 2 - gettimeofdayTask\n");
+    xTaskCreate(adjtimeTask, "adjtimeTask1", 2048, NULL, UNITY_FREERTOS_PRIORITY - 1, &th[0]);
+    xTaskCreate(gettimeofdayTask, "gettimeofdayTask1", 2048, NULL, UNITY_FREERTOS_PRIORITY - 1, &th[1]);
+
+    xTaskCreate(adjtimeTask, "adjtimeTask2", 2048, NULL, UNITY_FREERTOS_PRIORITY - 1, &th[2]);
+    xTaskCreate(gettimeofdayTask, "gettimeofdayTask2", 2048, NULL, UNITY_FREERTOS_PRIORITY - 1, &th[3]);
+#endif
+
+    printf("start wait for 10 seconds\n");
+    vTaskDelay(10000 / portTICK_PERIOD_MS);
+
+    // set exit flag to let thread exit
+    exit_flag = true;
+    vTaskDelay(20 / portTICK_PERIOD_MS);
+
+    TEST_ASSERT(adjtime_test_result == false && gettimeofday_test_result == false);
+}

components/newlib/time.c

@@ -78,8 +78,14 @@ static uint64_t s_boot_time;
 
 #if defined(WITH_RTC) || defined(WITH_FRC)
 static _lock_t s_boot_time_lock;
+static _lock_t s_adjust_time_lock;
+// stores the start time of the slew
+RTC_DATA_ATTR static uint64_t adjtime_start = 0;
+// is how many microseconds total to slew
+RTC_DATA_ATTR static int64_t adjtime_total_correction = 0;
+#define ADJTIME_CORRECTION_FACTOR 6
+static uint64_t get_time_since_boot();
 #endif
-
 // Offset between FRC timer and the RTC.
 // Initialized after reset or light sleep.
 #if defined(WITH_RTC) && defined(WITH_FRC)
@@ -111,8 +117,106 @@ static uint64_t get_boot_time()
     _lock_release(&s_boot_time_lock);
     return result;
 }
+
+// This function gradually changes boot_time to the correction value and immediately updates it.
+static uint64_t adjust_boot_time()
+{
+    uint64_t boot_time = get_boot_time();
+    if ((boot_time == 0) || (get_time_since_boot() < adjtime_start)) {
+        adjtime_start = 0;
+    }
+    if (adjtime_start > 0) {
+        uint64_t since_boot = get_time_since_boot();
+        // If to call this function once per second, then (since_boot - adjtime_start) will be 1_000_000 (1 second),
+        // and the correction will be equal to (1_000_000us >> 6) = 15_625 us.
+        // The minimum possible correction step can be (64us >> 6) = 1us.
+        // Example: if the time error is 1 second, then it will be compensate for 1 sec / 0,015625 = 64 seconds.
+        int64_t correction = (since_boot - adjtime_start) >> ADJTIME_CORRECTION_FACTOR;
+        if (correction > 0) {
+            adjtime_start = since_boot;
+            if (adjtime_total_correction < 0) {
+                if ((adjtime_total_correction + correction) >= 0) {
+                    boot_time = boot_time + adjtime_total_correction;
+                    adjtime_start = 0;
+                } else {
+                    adjtime_total_correction += correction;
+                    boot_time -= correction;
+                }
+            } else {
+                if ((adjtime_total_correction - correction) <= 0) {
+                    boot_time = boot_time + adjtime_total_correction;
+                    adjtime_start = 0;
+                } else {
+                    adjtime_total_correction -= correction;
+                    boot_time += correction;
+                }
+            }
+            set_boot_time(boot_time);
+        }
+    }
+    return boot_time;
+}
+
+// Get the adjusted boot time.
+static uint64_t get_adjusted_boot_time (void)
+{
+    _lock_acquire(&s_adjust_time_lock);
+    uint64_t adjust_time = adjust_boot_time();
+    _lock_release(&s_adjust_time_lock);
+    return adjust_time;
+}
+
+// Applying the accumulated correction to boot_time and stopping the smooth time adjustment.
+static void adjtime_corr_stop (void)
+{
+    _lock_acquire(&s_adjust_time_lock);
+    if (adjtime_start != 0){
+        adjust_boot_time();
+        adjtime_start = 0;
+    }
+    _lock_release(&s_adjust_time_lock);
+}
 #endif //defined(WITH_RTC) || defined(WITH_FRC)
 
+int adjtime(const struct timeval *delta, struct timeval *outdelta)
+{
+#if defined( WITH_FRC ) || defined( WITH_RTC )
+    if(delta != NULL){
+        int64_t sec  = delta->tv_sec;
+        int64_t usec = delta->tv_usec;
+        if(llabs(sec) > ((INT_MAX / 1000000L) - 1L)) {
+            return -1;
+        }
+        /*
+        * If adjusting the system clock by adjtime () is already done during the second call adjtime (),
+        * and the delta of the second call is not NULL, the earlier tuning is stopped,
+        * but the already completed part of the adjustment is not canceled.
+        */
+        _lock_acquire(&s_adjust_time_lock);
+        // If correction is already in progress (adjtime_start != 0), then apply accumulated corrections.
+        adjust_boot_time();
+        adjtime_start = get_time_since_boot();
+        adjtime_total_correction = sec * 1000000L + usec;
+        _lock_release(&s_adjust_time_lock);
+    }
+    if(outdelta != NULL){
+        _lock_acquire(&s_adjust_time_lock);
+        adjust_boot_time();
+        if (adjtime_start != 0) {
+            outdelta->tv_sec    = adjtime_total_correction / 1000000L;
+            outdelta->tv_usec   = adjtime_total_correction % 1000000L;
+        } else {
+            outdelta->tv_sec    = 0;
+            outdelta->tv_usec   = 0;
+        }
+        _lock_release(&s_adjust_time_lock);
+    }
+  return 0;
+#else
+  return -1;
+#endif
+
+}
 
 void esp_clk_slowclk_cal_set(uint32_t new_cal)
 {
@@ -190,7 +294,7 @@ int IRAM_ATTR _gettimeofday_r(struct _reent *r, struct timeval *tv, void *tz)
     (void) tz;
 #if defined( WITH_FRC ) || defined( WITH_RTC )
     if (tv) {
-        uint64_t microseconds = get_boot_time() + get_time_since_boot();
+        uint64_t microseconds = get_adjusted_boot_time() + get_time_since_boot();
         tv->tv_sec = microseconds / 1000000;
         tv->tv_usec = microseconds % 1000000;
     }
@@ -206,6 +310,7 @@ int settimeofday(const struct timeval *tv, const struct timezone *tz)
     (void) tz;
 #if defined( WITH_FRC ) || defined( WITH_RTC )
     if (tv) {
+        adjtime_corr_stop();
         uint64_t now = ((uint64_t) tv->tv_sec) * 1000000LL + tv->tv_usec;
         uint64_t since_boot = get_time_since_boot();
         set_boot_time(now - since_boot);