ptp: add clock_adjtime for windows

Signed-off-by: Ric Li <[email protected]>

app/src/app_platform.h

@@ -133,66 +133,18 @@ static inline void st_pause(void) {
 static inline void st_usleep(
     useconds_t usec) {  // windows usleep function precision is only 1~15ms
 #ifdef WINDOWSENV
-  LARGE_INTEGER delay;
-  HANDLE delay_timer_handle = NULL;
-  delay.QuadPart = usec;
-  delay.QuadPart = -(10 * delay.QuadPart);
-  delay_timer_handle = CreateWaitableTimer(NULL, TRUE, NULL);
-  if (delay_timer_handle) {
-    SetWaitableTimer(delay_timer_handle, &delay, 0, NULL, NULL, 0);
-    WaitForSingleObject(delay_timer_handle, INFINITE);
-    CloseHandle(delay_timer_handle);
-  } else {
-    Sleep((usec + 999) / 1000);
-  }
+  nanosleep(usec * 1000, NULL);
 #else
   usleep(usec);
 #endif
 }
 
 static inline int st_get_real_time(struct timespec* ts) {
-#ifdef WINDOWSENV
-  unsigned __int64 t;
-  union {
-    unsigned __int64 u64;
-    FILETIME ft;
-  } ct;
-  GetSystemTimePreciseAsFileTime(&ct.ft);
-  t = ct.u64 - INT64_C(116444736000000000);
-  ts->tv_sec = t / 10000000;
-  ts->tv_nsec = ((int)(t % 10000000)) * 100;
-  return 0;
-#else
   return clock_gettime(CLOCK_REALTIME, ts);
-#endif
 }
 
 static inline int st_set_real_time(struct timespec* ts) {
-#ifdef WINDOWSENV
-  time_t secs = ts->tv_sec;
-  WORD milliseconds = ts->tv_nsec / 1000000;
-
-  struct tm* tm;
-  tm = gmtime(&secs);
-
-  SYSTEMTIME st;
-  st.wYear = tm->tm_year + 1900;
-  st.wMonth = tm->tm_mon + 1;
-  st.wDayOfWeek = tm->tm_wday;
-  st.wDay = tm->tm_mday;
-  st.wHour = tm->tm_hour;
-  st.wMinute = tm->tm_min;
-  st.wSecond = tm->tm_sec;
-  st.wMilliseconds = milliseconds;
-
-  if (!SetSystemTime(&st)) {
-    /* set failed */
-    return -EPERM;
-  }
-  return 0;
-#else
   return clock_settime(CLOCK_REALTIME, ts);
-#endif
 }
 
 #endif

lib/src/mt_ptp.c

@@ -147,7 +147,6 @@ static inline double pi_sample(struct mt_pi_servo* s, double offset, double loca
 }
 
 static void ptp_adj_system_clock_time(struct mt_ptp_impl* ptp, int64_t delta) {
-#ifndef WINDOWSENV
   struct timex adjtime;
   int sign = 1;
 
@@ -168,14 +167,9 @@ static void ptp_adj_system_clock_time(struct mt_ptp_impl* ptp, int64_t delta) {
   dbg("%s(%d), delta %" PRId64 "\n", __func__, ptp->port, delta);
   int ret = clock_adjtime(CLOCK_REALTIME, &adjtime);
   if (ret < 0) err("%s(%d), adj system time offset fail %d\n", __func__, ptp->port, ret);
-#else
-  MTL_MAY_UNUSED(delta);
-  err("%s(%d), not supported for windows\n", __func__, ptp->port);
-#endif
 }
 
 static void ptp_adj_system_clock_freq(struct mt_ptp_impl* ptp, double freq) {
-#ifndef WINDOWSENV
   struct timex adjfreq;
   memset(&adjfreq, 0, sizeof(adjfreq));
 
@@ -191,10 +185,6 @@ static void ptp_adj_system_clock_freq(struct mt_ptp_impl* ptp, double freq) {
   adjfreq.freq = (long)(freq * 65.536);
   int ret = clock_adjtime(CLOCK_REALTIME, &adjfreq);
   if (ret < 0) err("%s(%d), adj system time freq fail %d\n", __func__, ptp->port, ret);
-#else
-  MTL_MAY_UNUSED(freq);
-  err("%s(%d), not supported for windows\n", __func__, ptp->port);
-#endif
 }
 
 static void phc2sys_adjust(struct mt_ptp_impl* ptp) {

lib/windows/win_posix.c

@@ -49,4 +49,42 @@ int shmctl(int shmid, int cmd, struct shmid_ds* buf) {
     return (-1);
 }
 
+int clock_adjtime(int clk_id, struct timex* tp) {
+  if (clk_id == CLOCK_REALTIME) {
+    if (tp->modes & ADJ_SETOFFSET) {
+      SYSTEMTIME st;
+      GetSystemTime(&st);
+      FILETIME ft;
+      SystemTimeToFileTime(&st, &ft);
+      ULARGE_INTEGER ui;
+      ui.LowPart = ft.dwLowDateTime;
+      ui.HighPart = ft.dwHighDateTime;
+      ui.QuadPart += tp->time.tv_sec * 10000000ULL;
+      if (tp->modes & ADJ_NANO) {
+        ui.QuadPart += tp->time.tv_usec / 100;
+      } else {
+        ui.QuadPart += tp->time.tv_usec * 10;
+      }
+      ft.dwLowDateTime = ui.LowPart;
+      ft.dwHighDateTime = ui.HighPart;
+      FileTimeToSystemTime(&ft, &st);
+      if (!SetSystemTime(&st)) {
+        return -1;
+      }
+    } else if (tp->modes == ADJ_FREQUENCY) {
+      if (!SetSystemTimeAdjustmentPrecise((DWORD)(tp->freq * 10), FALSE)) {
+        return -1;
+      }
+    } else {
+      /* Not supported in Windows */
+      return -1;
+    }
+  } else {
+    /* Unknown clock ID */
+    return -1;
+  }
+
+  return 0;
+}
+
 #endif

lib/windows/win_posix.h

@@ -15,6 +15,7 @@
 // clang-format off
 #include <winsock2.h>
 #include <windows.h>
+#include <sysinfoapi.h>
 // clang-format on
 
 #ifndef MTL_MAY_UNUSED
@@ -145,6 +146,35 @@ int pthread_cond_timedwait(pthread_cond_t* cond, pthread_mutex_t* mutex,
 int pthread_cond_destroy(pthread_cond_t* cv);
 int pthread_mutex_trylock(pthread_mutex_t* mutex);
 
+#define ADJ_FREQUENCY 0x0002 /* frequency offset */
+#define ADJ_SETOFFSET 0x0100 /* add 'time' to current time */
+#define ADJ_NANO 0x2000      /* select nanosecond resolution */
+#define ADJ_TICK 0x4000      /* tick value */
+
+struct timex {
+  int modes;           /* mode selector */
+  long offset;         /* time offset (usec) */
+  long freq;           /* frequency offset (scaled ppm) */
+  long maxerror;       /* maximum error (usec) */
+  long esterror;       /* estimated error (usec) */
+  int status;          /* clock command/status */
+  long constant;       /* pll time constant */
+  long precision;      /* clock precision (usec) (read only) */
+  long tolerance;      /* clock frequency tolerance (ppm) (read only) */
+  struct timeval time; /* previous time stamp (read only) */
+  long tick;           /* time tick (usec) (read only) */
+  long ppsfreq;        /* pps frequency (scaled ppm) (read only) */
+  long jitter;         /* pps jitter (usec) (read only) */
+  int shift;           /* interval duration (s) (shift) (read only) */
+  long stabil;         /* pps stability (scaled ppm) (read only) */
+  long jitcnt;         /* jitter limit exceeded count (read only) */
+  long calcnt;         /* calibration intervals (read only) */
+  long errcnt;         /* calibration errors (read only) */
+  long stbcnt;         /* stability limit exceeded count (read only) */
+};
+
+int clock_adjtime(clockid_t clk_id, struct timex* tp);
+
 #ifdef __MTL_LIB_BUILD__
 static inline pid_t getpid() { return GetCurrentProcessId(); }
 #endif