8 changed files with 385 additions and 2 deletions
@ -0,0 +1,298 @@ |
|||||||
|
#include "ntp_time.h" |
||||||
|
#include "utun_instance.h" |
||||||
|
#include "../lib/debug_config.h" |
||||||
|
#include "../lib/socket_compat.h" |
||||||
|
#include "../lib/u_async.h" |
||||||
|
#include "../lib/mem.h" |
||||||
|
#include <stdlib.h> |
||||||
|
#include <string.h> |
||||||
|
|
||||||
|
#ifdef _WIN32 |
||||||
|
#include <winsock2.h> |
||||||
|
#include <ws2tcpip.h> |
||||||
|
#else |
||||||
|
#include <sys/select.h> |
||||||
|
#include <sys/time.h> |
||||||
|
#include <netdb.h> |
||||||
|
#endif |
||||||
|
|
||||||
|
#define NTP_DELTA 2208988800ULL // seconds from 1900 to 1970
|
||||||
|
#define NTP_PORT 123 |
||||||
|
#define NTP_TIMEOUT_SEC 2 |
||||||
|
#define NTP_MAX_TRIES 3 |
||||||
|
|
||||||
|
#pragma pack(push, 1) |
||||||
|
struct ntp_packet { |
||||||
|
uint8_t li_vn_mode; // LI(2) | VN(3) | Mode(3)
|
||||||
|
uint8_t stratum; |
||||||
|
uint8_t poll; |
||||||
|
uint8_t precision; |
||||||
|
uint32_t root_delay; |
||||||
|
uint32_t root_dispersion; |
||||||
|
uint32_t ref_id; |
||||||
|
uint64_t ref_ts; |
||||||
|
uint64_t orig_ts; |
||||||
|
uint64_t recv_ts; |
||||||
|
uint64_t xmit_ts; |
||||||
|
}; |
||||||
|
#pragma pack(pop) |
||||||
|
|
||||||
|
_Static_assert(sizeof(struct ntp_packet) == 48, "NTP packet must be 48 bytes"); |
||||||
|
|
||||||
|
static void ntp_time_sync_cb(void* arg); |
||||||
|
|
||||||
|
static uint64_t timeval_to_ntp(struct timeval *tv) { |
||||||
|
uint64_t sec = (uint64_t)(tv->tv_sec + NTP_DELTA); |
||||||
|
uint64_t frac = ((uint64_t)tv->tv_usec << 32) / 1000000ULL; |
||||||
|
return (sec << 32) | frac; |
||||||
|
} |
||||||
|
|
||||||
|
static int64_t ntp64_to_us(uint64_t ntp) { |
||||||
|
int64_t sec = (int64_t)(ntp >> 32) - (int64_t)NTP_DELTA; |
||||||
|
int64_t frac = ((int64_t)(ntp & 0xFFFFFFFFULL) * 1000000ULL) >> 32; |
||||||
|
return sec * 1000000LL + frac; |
||||||
|
} |
||||||
|
|
||||||
|
static int ntp_query_server(const char* server, int64_t* offset_us_out, int* stratum_out) { |
||||||
|
struct addrinfo hints, *result = NULL; |
||||||
|
memset(&hints, 0, sizeof(hints)); |
||||||
|
hints.ai_family = AF_INET; |
||||||
|
hints.ai_socktype = SOCK_DGRAM; |
||||||
|
|
||||||
|
char port_str[8]; |
||||||
|
snprintf(port_str, sizeof(port_str), "%d", NTP_PORT); |
||||||
|
int gai_err = getaddrinfo(server, port_str, &hints, &result); |
||||||
|
if (gai_err != 0 || !result) { |
||||||
|
DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "NTP: failed to resolve %s: %s", server, gai_strerror(gai_err)); |
||||||
|
return -1; |
||||||
|
} |
||||||
|
|
||||||
|
socket_t sock = socket_create_udp(AF_INET); |
||||||
|
if (sock == SOCKET_INVALID) { |
||||||
|
DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "NTP: socket() failed: %s", socket_strerror(socket_get_error())); |
||||||
|
freeaddrinfo(result); |
||||||
|
return -1; |
||||||
|
} |
||||||
|
|
||||||
|
struct ntp_packet request; |
||||||
|
memset(&request, 0, sizeof(request)); |
||||||
|
request.li_vn_mode = (0 << 6) | (4 << 3) | 3; |
||||||
|
struct timeval t1_tv; |
||||||
|
#ifdef _WIN32 |
||||||
|
utun_gettimeofday(&t1_tv, NULL); |
||||||
|
#else |
||||||
|
gettimeofday(&t1_tv, NULL); |
||||||
|
#endif |
||||||
|
uint64_t t1_ntp = timeval_to_ntp(&t1_tv); |
||||||
|
request.xmit_ts = htobe64(t1_ntp); |
||||||
|
|
||||||
|
struct sockaddr_in* sin = (struct sockaddr_in*)result->ai_addr; |
||||||
|
int send_ok = 0; |
||||||
|
int tries; |
||||||
|
for (tries = 0; tries < NTP_MAX_TRIES; tries++) { |
||||||
|
ssize_t sent = sendto(sock, (const char*)&request, sizeof(request), 0, |
||||||
|
(struct sockaddr*)sin, sizeof(*sin)); |
||||||
|
if (sent < 0) { |
||||||
|
DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "NTP: sendto(%s) failed: %s", server, socket_strerror(socket_get_error())); |
||||||
|
break; |
||||||
|
} |
||||||
|
|
||||||
|
fd_set fds; |
||||||
|
FD_ZERO(&fds); |
||||||
|
FD_SET(sock, &fds); |
||||||
|
struct timeval tv = {NTP_TIMEOUT_SEC, 0}; |
||||||
|
int r = select((int)(sock + 1), &fds, NULL, NULL, &tv); |
||||||
|
if (r < 0) { |
||||||
|
DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "NTP: select() failed: %s", socket_strerror(socket_get_error())); |
||||||
|
break; |
||||||
|
} |
||||||
|
if (r > 0) { send_ok = 1; break; } |
||||||
|
} |
||||||
|
if (!send_ok) { |
||||||
|
DEBUG_WARN(DEBUG_CATEGORY_GENERAL, "NTP: no response from %s after %d tries", server, tries); |
||||||
|
socket_close_wrapper(sock); |
||||||
|
freeaddrinfo(result); |
||||||
|
return -1; |
||||||
|
} |
||||||
|
|
||||||
|
struct ntp_packet reply; |
||||||
|
struct sockaddr_in from; |
||||||
|
socklen_t from_len = sizeof(from); |
||||||
|
ssize_t n = recvfrom(sock, (char*)&reply, sizeof(reply), 0, (struct sockaddr*)&from, &from_len); |
||||||
|
if (n < 0) { |
||||||
|
DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "NTP: recvfrom(%s) failed: %s", server, socket_strerror(socket_get_error())); |
||||||
|
socket_close_wrapper(sock); |
||||||
|
freeaddrinfo(result); |
||||||
|
return -1; |
||||||
|
} |
||||||
|
|
||||||
|
struct timeval t4_tv; |
||||||
|
#ifdef _WIN32 |
||||||
|
utun_gettimeofday(&t4_tv, NULL); |
||||||
|
#else |
||||||
|
gettimeofday(&t4_tv, NULL); |
||||||
|
#endif |
||||||
|
socket_close_wrapper(sock); |
||||||
|
freeaddrinfo(result); |
||||||
|
|
||||||
|
if (n < (ssize_t)sizeof(reply)) { |
||||||
|
DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "NTP: short reply from %s (got %zd, expected %zu)", server, n, sizeof(reply)); |
||||||
|
return -1; |
||||||
|
} |
||||||
|
|
||||||
|
uint8_t mode = reply.li_vn_mode & 0x07; |
||||||
|
if (mode != 4) { |
||||||
|
DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "NTP: unexpected mode %d from %s (expected 4)", mode, server); |
||||||
|
return -1; |
||||||
|
} |
||||||
|
|
||||||
|
int stratum = reply.stratum; |
||||||
|
if (stratum == 0) { |
||||||
|
DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "NTP: kiss-o-death from %s", server); |
||||||
|
return -1; |
||||||
|
} |
||||||
|
|
||||||
|
uint64_t reply_orig = be64toh(reply.orig_ts); |
||||||
|
if (reply_orig != t1_ntp) { |
||||||
|
DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "NTP: originate timestamp mismatch from %s", server); |
||||||
|
return -1; |
||||||
|
} |
||||||
|
|
||||||
|
int64_t t1_us = ntp64_to_us(t1_ntp); |
||||||
|
int64_t t2_us = ntp64_to_us(be64toh(reply.recv_ts)); |
||||||
|
int64_t t3_us = ntp64_to_us(be64toh(reply.xmit_ts)); |
||||||
|
int64_t t4_us = ntp64_to_us(timeval_to_ntp(&t4_tv)); |
||||||
|
|
||||||
|
int64_t offset_us = ((t2_us - t1_us) + (t3_us - t4_us)) / 2; |
||||||
|
int64_t rtt_us = (t4_us - t1_us) - (t3_us - t2_us); |
||||||
|
|
||||||
|
DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "NTP: synced from %s offset=%lldus rtt=%lldus stratum=%d", |
||||||
|
server, (long long)offset_us, (long long)rtt_us, stratum); |
||||||
|
|
||||||
|
*offset_us_out = offset_us; |
||||||
|
*stratum_out = stratum; |
||||||
|
return 0; |
||||||
|
} |
||||||
|
|
||||||
|
static void ntp_time_sync_cb(void* arg) { |
||||||
|
struct UTUN_INSTANCE* instance = (struct UTUN_INSTANCE*)arg; |
||||||
|
if (!instance) return; |
||||||
|
struct NTP_TIME* ntp = &instance->ntp; |
||||||
|
ntp->timer = NULL; |
||||||
|
|
||||||
|
if (!ntp->enabled || ntp->server_count == 0) { |
||||||
|
ntp->timer = uasync_set_timeout(instance->ua, ntp->resync_interval_sec * 10000, |
||||||
|
instance, ntp_time_sync_cb, "ntp_sync"); |
||||||
|
return; |
||||||
|
} |
||||||
|
|
||||||
|
const char* server = ntp->servers[ntp->server_current]; |
||||||
|
int64_t offset_us = 0; |
||||||
|
int stratum = 0; |
||||||
|
|
||||||
|
if (ntp_query_server(server, &offset_us, &stratum) == 0) { |
||||||
|
ntp->offset_us = offset_us; |
||||||
|
ntp->synced = 1; |
||||||
|
ntp->last_sync_tb = get_time_tb(); |
||||||
|
ntp->server_current = (ntp->server_current + 1) % ntp->server_count; |
||||||
|
} else { |
||||||
|
for (int i = 0; i < ntp->server_count; i++) { |
||||||
|
ntp->server_current = (ntp->server_current + 1) % ntp->server_count; |
||||||
|
const char* next_server = ntp->servers[ntp->server_current]; |
||||||
|
if (strcmp(next_server, server) == 0) break; |
||||||
|
if (ntp_query_server(next_server, &offset_us, &stratum) == 0) { |
||||||
|
ntp->offset_us = offset_us; |
||||||
|
ntp->synced = 1; |
||||||
|
ntp->last_sync_tb = get_time_tb(); |
||||||
|
break; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
if (ntp->synced) { |
||||||
|
DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "NTP: time corrected, offset=%lldus", |
||||||
|
(long long)ntp->offset_us); |
||||||
|
} else { |
||||||
|
DEBUG_WARN(DEBUG_CATEGORY_GENERAL, "NTP: all servers unreachable, retry in %ds", |
||||||
|
ntp->resync_interval_sec); |
||||||
|
} |
||||||
|
|
||||||
|
ntp->timer = uasync_set_timeout(instance->ua, ntp->resync_interval_sec * 10000, |
||||||
|
instance, ntp_time_sync_cb, "ntp_sync"); |
||||||
|
} |
||||||
|
|
||||||
|
int ntp_time_init(struct UTUN_INSTANCE* instance) { |
||||||
|
if (!instance) return -1; |
||||||
|
struct NTP_TIME* ntp = &instance->ntp; |
||||||
|
|
||||||
|
struct global_config* g = &instance->config->global; |
||||||
|
ntp->enabled = g->ntp_enabled; |
||||||
|
ntp->synced = 0; |
||||||
|
ntp->offset_us = 0; |
||||||
|
ntp->last_sync_tb = 0; |
||||||
|
ntp->timer = NULL; |
||||||
|
ntp->server_count = g->ntp_server_count; |
||||||
|
ntp->server_current = 0; |
||||||
|
ntp->resync_interval_sec = g->ntp_resync_interval; |
||||||
|
for (int i = 0; i < g->ntp_server_count && i < NTP_MAX_SERVERS; i++) { |
||||||
|
strncpy(ntp->servers[i], g->ntp_servers[i], sizeof(ntp->servers[i]) - 1); |
||||||
|
ntp->servers[i][sizeof(ntp->servers[i]) - 1] = '\0'; |
||||||
|
} |
||||||
|
|
||||||
|
if (!ntp->enabled || ntp->server_count == 0) { |
||||||
|
DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "NTP: disabled or no servers configured"); |
||||||
|
return 0; |
||||||
|
} |
||||||
|
|
||||||
|
DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "NTP: initialized, %d servers, interval=%ds, first sync immediately", |
||||||
|
ntp->server_count, ntp->resync_interval_sec); |
||||||
|
|
||||||
|
ntp->timer = uasync_set_timeout(instance->ua, NTP_FIRST_SYNC_DELAY_SEC * 10000, |
||||||
|
instance, ntp_time_sync_cb, "ntp_sync"); |
||||||
|
if (!ntp->timer) { |
||||||
|
DEBUG_ERROR(DEBUG_CATEGORY_GENERAL, "NTP: failed to start sync timer"); |
||||||
|
return -1; |
||||||
|
} |
||||||
|
return 0; |
||||||
|
} |
||||||
|
|
||||||
|
void ntp_time_destroy(struct UTUN_INSTANCE* instance) { |
||||||
|
if (!instance) return; |
||||||
|
struct NTP_TIME* ntp = &instance->ntp; |
||||||
|
|
||||||
|
if (ntp->timer && instance->ua) { |
||||||
|
uasync_cancel_timeout(instance->ua, ntp->timer); |
||||||
|
ntp->timer = NULL; |
||||||
|
} |
||||||
|
ntp->enabled = 0; |
||||||
|
DEBUG_INFO(DEBUG_CATEGORY_GENERAL, "NTP: destroyed (synced=%d, offset=%lldus)", |
||||||
|
ntp->synced, (long long)ntp->offset_us); |
||||||
|
} |
||||||
|
|
||||||
|
int64_t ntp_time_get_us(struct UTUN_INSTANCE* instance) { |
||||||
|
if (!instance || !instance->ntp.synced) { |
||||||
|
struct timeval tv; |
||||||
|
#ifdef _WIN32 |
||||||
|
utun_gettimeofday(&tv, NULL); |
||||||
|
#else |
||||||
|
gettimeofday(&tv, NULL); |
||||||
|
#endif |
||||||
|
return (int64_t)tv.tv_sec * 1000000LL + tv.tv_usec; |
||||||
|
} |
||||||
|
struct timeval tv; |
||||||
|
#ifdef _WIN32 |
||||||
|
utun_gettimeofday(&tv, NULL); |
||||||
|
#else |
||||||
|
gettimeofday(&tv, NULL); |
||||||
|
#endif |
||||||
|
int64_t local_us = (int64_t)tv.tv_sec * 1000000LL + tv.tv_usec; |
||||||
|
return local_us - instance->ntp.offset_us; |
||||||
|
} |
||||||
|
|
||||||
|
time_t ntp_time_get_seconds(struct UTUN_INSTANCE* instance) { |
||||||
|
return (time_t)(ntp_time_get_us(instance) / 1000000LL); |
||||||
|
} |
||||||
|
|
||||||
|
int ntp_time_is_synced(struct UTUN_INSTANCE* instance) { |
||||||
|
return instance && instance->ntp.synced; |
||||||
|
} |
||||||
@ -0,0 +1,40 @@ |
|||||||
|
#ifndef NTP_TIME_H |
||||||
|
#define NTP_TIME_H |
||||||
|
|
||||||
|
#include <stdint.h> |
||||||
|
#include <time.h> |
||||||
|
|
||||||
|
#ifdef __cplusplus |
||||||
|
extern "C" { |
||||||
|
#endif |
||||||
|
|
||||||
|
struct UTUN_INSTANCE; |
||||||
|
|
||||||
|
#define NTP_MAX_SERVERS 5 |
||||||
|
#define NTP_DEFAULT_INTERVAL_SEC 3600 |
||||||
|
#define NTP_FIRST_SYNC_DELAY_SEC 0 |
||||||
|
|
||||||
|
struct NTP_TIME { |
||||||
|
int enabled; |
||||||
|
int synced; |
||||||
|
int64_t offset_us; // local_time - ntp_time (positive = local clock ahead)
|
||||||
|
uint64_t last_sync_tb; // monotonic time of last successful sync (0.1ms units)
|
||||||
|
void* timer; // uasync timer handle
|
||||||
|
char servers[NTP_MAX_SERVERS][256]; |
||||||
|
int server_count; |
||||||
|
int server_current; |
||||||
|
int resync_interval_sec; |
||||||
|
}; |
||||||
|
|
||||||
|
int ntp_time_init(struct UTUN_INSTANCE* instance); |
||||||
|
void ntp_time_destroy(struct UTUN_INSTANCE* instance); |
||||||
|
|
||||||
|
int64_t ntp_time_get_us(struct UTUN_INSTANCE* instance); |
||||||
|
time_t ntp_time_get_seconds(struct UTUN_INSTANCE* instance); |
||||||
|
int ntp_time_is_synced(struct UTUN_INSTANCE* instance); |
||||||
|
|
||||||
|
#ifdef __cplusplus |
||||||
|
} |
||||||
|
#endif |
||||||
|
|
||||||
|
#endif // NTP_TIME_H
|
||||||
Loading…
Reference in new issue