@@ -85,10 +85,9 @@ __attribute__((weak)) void display_status([[maybe_unused]] const ::ntp::Status s
8585
8686
8787namespace net ::globals {
88- extern uint32_t ptpTimestamp[2 ];
88+ extern uint32_t ptpTimestamp[2 ];
8989} // namespace net::globals
9090
91-
9291#define _NTPFRAC_ (x ) ( 4294U *static_cast <uint32_t >(x) + ( (1981U *static_cast <uint32_t >(x))>>11 ) + ((2911U *static_cast <uint32_t >(x))>>28 ) )
9392#define NTPFRAC (x ) _NTPFRAC_(x / 1000 )
9493
@@ -254,9 +253,9 @@ static void send() {
254253
255254#ifndef NDEBUG
256255 printf (" Request: org=%.8x%.8x rx=%.8x%.8x tx=%.8x%.8x\n " ,
257- __builtin_bswap32 (s_ntpClient.Request .OriginTimestamp_s ), __builtin_bswap32 (s_ntpClient.Request .OriginTimestamp_f ),
258- __builtin_bswap32 (s_ntpClient.Request .ReceiveTimestamp_s ), __builtin_bswap32 (s_ntpClient.Request .ReceiveTimestamp_f ),
259- __builtin_bswap32 (s_ntpClient.Request .TransmitTimestamp_s ), __builtin_bswap32 (s_ntpClient.Request .TransmitTimestamp_f ));
256+ __builtin_bswap32 (s_ntpClient.Request .OriginTimestamp_s ), __builtin_bswap32 (s_ntpClient.Request .OriginTimestamp_f ),
257+ __builtin_bswap32 (s_ntpClient.Request .ReceiveTimestamp_s ), __builtin_bswap32 (s_ntpClient.Request .ReceiveTimestamp_f ),
258+ __builtin_bswap32 (s_ntpClient.Request .TransmitTimestamp_s ), __builtin_bswap32 (s_ntpClient.Request .TransmitTimestamp_f ));
260259#endif
261260
262261 if (s_ntpClient.state .x > 0 ) {
@@ -276,13 +275,17 @@ static void send() {
276275}
277276
278277static void difference (const ntp::TimeStamp& Start, const ntp::TimeStamp& Stop, int32_t & nDiffSeconds, int32_t & nDiffNanoSeconds) {
279- gd32::ptp::time_t r;
278+ gd32::ptp::time_t r;
280279 const gd32::ptp::time_t x = {.tv_sec = static_cast <int32_t >(Stop.nSeconds ), .tv_nsec = static_cast <int32_t >(USEC (Stop.nFraction ) * 1000 )};
281280 const gd32::ptp::time_t y = {.tv_sec = static_cast <int32_t >(Start.nSeconds ), .tv_nsec = static_cast <int32_t >(USEC (Start.nFraction ) * 1000 )};
282- gd32::sub_time (&r, &x, &y);
281+ gd32::sub_time (&r, &x, &y);
282+
283+ nDiffSeconds = r.tv_sec ;
284+ nDiffNanoSeconds = r.tv_nsec ;
285+ }
283286
284- nDiffSeconds = r. tv_sec ;
285- nDiffNanoSeconds = r. tv_nsec ;
287+ static inline int32_t abs_int32 ( const int32_t x) {
288+ return (x < 0 ) ? -x : x ;
286289}
287290
288291static void update_ptp_time () {
@@ -305,6 +308,16 @@ static void update_ptp_time() {
305308 gd32::ptp::ptptime ptp_get;
306309 gd32_ptp_get_time (&ptp_get);
307310
311+ const auto offset_ns = (static_cast <int64_t >(ptpOffset.tv_sec ) * 1000000000LL ) + static_cast <int64_t >(ptpOffset.tv_nsec );
312+ int32_t adjust_ppb = -(offset_ns / s_ntpClient.nPollSeconds );
313+
314+ if (abs_int32 (adjust_ppb) > 1 ) {
315+ gd32_adj_frequency (adjust_ppb);
316+ #ifndef NDEBUG
317+ printf (" Applied frequency adjustment: %d ppb\n " , adjust_ppb);
318+ #endif
319+ }
320+
308321 s_ntpClient.Request .ReferenceTimestamp_s = __builtin_bswap32 (static_cast <uint32_t >(ptp_get.tv_sec ) + ntp::JAN_1970);
309322 s_ntpClient.Request .ReferenceTimestamp_f = __builtin_bswap32 (NTPFRAC (ptp_get.tv_nsec ));
310323
@@ -365,8 +378,8 @@ static void update_ptp_time() {
365378
366379 printf (" %s : offset=%c%d.%09d delay=%d.%09d\n " ,
367380 s_ntpClient.state .mode == ntp::Modes::BASIC ? " Basic" : " Interleaved" ,
368- sign, ptpOffset.tv_sec , ptpOffset.tv_nsec ,
369- ptpDelay.tv_sec , ptpDelay.tv_nsec );
381+ sign, ptpOffset.tv_sec , ptpOffset.tv_nsec ,
382+ ptpDelay.tv_sec , ptpDelay.tv_nsec );
370383#endif
371384}
372385
@@ -386,9 +399,9 @@ static void process() {
386399 const auto *const pReply = s_ntpClient.pReply ;
387400#ifndef NDEBUG
388401 printf (" Response: org=%.8x%.8x rx=%.8x%.8x tx=%.8x%.8x\n " ,
389- __builtin_bswap32 (pReply->OriginTimestamp_s ), __builtin_bswap32 (pReply->OriginTimestamp_f ),
390- __builtin_bswap32 (pReply->ReceiveTimestamp_s ), __builtin_bswap32 (pReply->ReceiveTimestamp_f ),
391- __builtin_bswap32 (pReply->TransmitTimestamp_s ), __builtin_bswap32 (pReply->TransmitTimestamp_f ));
402+ __builtin_bswap32 (pReply->OriginTimestamp_s ), __builtin_bswap32 (pReply->OriginTimestamp_f ),
403+ __builtin_bswap32 (pReply->ReceiveTimestamp_s ), __builtin_bswap32 (pReply->ReceiveTimestamp_f ),
404+ __builtin_bswap32 (pReply->TransmitTimestamp_s ), __builtin_bswap32 (pReply->TransmitTimestamp_f ));
392405#endif
393406 /* *
394407 * If the origin timestamp is equal to the transmit timestamp,
@@ -409,28 +422,28 @@ static void process() {
409422 s_ntpClient.state .mode = ntp::Modes::BASIC;
410423#endif
411424 } else
412- /* *
413- * If the origin timestamp is equal to the receive timestamp,
414- * the response is in the interleaved mode.
415- */
416- if ((pReply->OriginTimestamp_s == s_ntpClient.Request .ReceiveTimestamp_s ) && (pReply->OriginTimestamp_f == s_ntpClient.Request .ReceiveTimestamp_f )) {
417- if (s_ntpClient.state .x > 0 ) {
418- s_ntpClient.T1 .nSeconds = s_ntpClient.state .sentB .nSeconds ;
419- s_ntpClient.T1 .nFraction = s_ntpClient.state .sentB .nFraction ;
420- } else {
421- s_ntpClient.T1 .nSeconds = s_ntpClient.state .sentA .nSeconds ;
422- s_ntpClient.T1 .nFraction = s_ntpClient.state .sentA .nFraction ;
423- }
424-
425- s_ntpClient.T4 .nSeconds = s_ntpClient.state .previousReceive .nSeconds ;
426- s_ntpClient.T4 .nFraction = s_ntpClient.state .previousReceive .nFraction ;
425+ /* *
426+ * If the origin timestamp is equal to the receive timestamp,
427+ * the response is in the interleaved mode.
428+ */
429+ if ((pReply->OriginTimestamp_s == s_ntpClient.Request .ReceiveTimestamp_s ) && (pReply->OriginTimestamp_f == s_ntpClient.Request .ReceiveTimestamp_f )) {
430+ if (s_ntpClient.state .x > 0 ) {
431+ s_ntpClient.T1 .nSeconds = s_ntpClient.state .sentB .nSeconds ;
432+ s_ntpClient.T1 .nFraction = s_ntpClient.state .sentB .nFraction ;
433+ } else {
434+ s_ntpClient.T1 .nSeconds = s_ntpClient.state .sentA .nSeconds ;
435+ s_ntpClient.T1 .nFraction = s_ntpClient.state .sentA .nFraction ;
436+ }
437+
438+ s_ntpClient.T4 .nSeconds = s_ntpClient.state .previousReceive .nSeconds ;
439+ s_ntpClient.T4 .nFraction = s_ntpClient.state .previousReceive .nFraction ;
427440#ifndef NDEBUG
428- s_ntpClient.state .mode = ntp::Modes::INTERLEAVED;
441+ s_ntpClient.state .mode = ntp::Modes::INTERLEAVED;
429442#endif
430- } else {
431- DEBUG_PUTS (" INVALID RESPONSE" );
432- return ;
433- }
443+ } else {
444+ DEBUG_PUTS (" INVALID RESPONSE" );
445+ return ;
446+ }
434447
435448 s_ntpClient.T2 .nSeconds = __builtin_bswap32 (pReply->ReceiveTimestamp_s );
436449 s_ntpClient.T2 .nFraction = __builtin_bswap32 (pReply->ReceiveTimestamp_f );
0 commit comments