rtc: Add settimeofday()

Author: ssilverman

teensy3/pins_teensy.c

@@ -404,6 +404,36 @@ int _gettimeofday(struct timeval *tv, void *ignore)
 	}
 }
 
+__attribute__((weak))
+int settimeofday(const struct timeval *const tv,
+                 const struct timezone *const tz __attribute__((unused))) {
+  // Disable time counter
+  RTC_SR = 0;
+
+  // Notes:
+  // * The type of tv_usec is suseconds_t, range is [-1, 1000000]
+  // * There are 32768 ticks per 1,000,000 microseconds; that's where 512/15625
+  //   comes from, it's 32768/1000000 in lowest terms
+  // Refs:
+  // * https://pubs.opengroup.org/onlinepubs/007904975/basedefs/sys/time.h.html
+  // * https://pubs.opengroup.org/onlinepubs/007904975/basedefs/sys/types.h.html
+  uint32_t sec = (uint32_t)tv->tv_sec;
+  uint32_t usec = (uint32_t)tv->tv_usec;
+  if (usec == 1000000) {
+    sec++;
+    usec = 0;
+  } else if (usec == UINT32_MAX) {
+    sec--;
+    usec = 999999;
+  }
+  RTC_TPR = ((usec << 9) / 15625) & 0x7fff;
+  RTC_TSR = sec;
+
+  // Enable time counter
+  RTC_SR = RTC_SR_TCE;
+  return 0;
+}
+
 #else
 
 unsigned long rtc_get(void) { return 0; }
@@ -413,6 +443,12 @@ void rtc_compensate(int adjust) { }
 __attribute__((weak))
 int _gettimeofday(struct timeval *tv, void *ignore) { return -1; }
 
+__attribute__((weak))
+int settimeofday(const struct timeval *const tv __attribute__((unused)),
+                 const struct timezone *const tz __attribute__((unused))) {
+  return -1;
+}
+
 #endif
 
 

teensy4/rtc.c

@@ -30,7 +30,7 @@
 
 #include "imxrt.h"
 #include "debug/printf.h"
-#include <sys/time.h> // for struct timeval
+#include <sys/time.h>  // for struct timeval and struct timezone
 
 unsigned long rtc_get(void)
 {
@@ -88,3 +88,50 @@ int _gettimeofday(struct timeval *tv, void *ignore __attribute__((unused)))
 		lo1 = lo2;
 	}
 }
+
+__attribute__((weak))
+int settimeofday(const struct timeval *const tv,
+                 const struct timezone *const tz __attribute__((unused))) {
+  // Stop the RTC
+  SNVS_HPCR &= ~(SNVS_HPCR_RTC_EN | SNVS_HPCR_HP_TS);
+  while (SNVS_HPCR & SNVS_HPCR_RTC_EN) {
+    // Wait
+  }
+
+  // Stop the SRTC
+  SNVS_LPCR &= ~SNVS_LPCR_SRTC_ENV;
+  while (SNVS_LPCR & SNVS_LPCR_SRTC_ENV) {
+    // Wait
+  }
+
+  // Set the SRTC
+  // Notes:
+  // * The type of tv_usec is suseconds_t, range is [-1, 1000000]
+  // * There are 32768 ticks per 1,000,000 microseconds; that's where 512/15625
+  //   comes from, it's 32768/1000000 in lowest terms
+  // Refs:
+  // * https://pubs.opengroup.org/onlinepubs/007904975/basedefs/sys/time.h.html
+  // * https://pubs.opengroup.org/onlinepubs/007904975/basedefs/sys/types.h.html
+  uint32_t sec = (uint32_t)tv->tv_sec;
+  uint32_t usec = (uint32_t)tv->tv_usec;
+  if (usec == 1000000) {
+    sec++;
+    usec = 0;
+  } else if (usec == UINT32_MAX) {
+    sec--;
+    usec = 999999;
+  }
+  SNVS_LPSRTCLR = (sec << 15) | (((usec << 9) / 15625) & 0x7fff);
+  SNVS_LPSRTCMR = (sec >> 17) & 0x7fff;
+
+  // Start the SRTC
+  SNVS_LPCR |= SNVS_LPCR_SRTC_ENV;
+  while (!(SNVS_LPCR & SNVS_LPCR_SRTC_ENV)) {
+    // Wait
+  }
+
+  // Start the RTC and sync it to the SRTC
+  SNVS_HPCR |= SNVS_HPCR_RTC_EN | SNVS_HPCR_HP_TS;
+
+  return 0;
+}