diff --git a/app/src/app_platform.h b/app/src/app_platform.h index 1a7bdb77e..807ee9d4b 100644 --- a/app/src/app_platform.h +++ b/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 diff --git a/lib/src/mt_ptp.c b/lib/src/mt_ptp.c index 149a77c99..c16078c99 100644 --- a/lib/src/mt_ptp.c +++ b/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) { diff --git a/lib/windows/win_posix.c b/lib/windows/win_posix.c index eeb0d2f2f..3071b2351 100644 --- a/lib/windows/win_posix.c +++ b/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 diff --git a/lib/windows/win_posix.h b/lib/windows/win_posix.h index a448f2eaf..bd407f470 100644 --- a/lib/windows/win_posix.h +++ b/lib/windows/win_posix.h @@ -145,6 +145,34 @@ 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 */ + +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