From 0bf39c0ab93f0ed4a9b235b1187762586d24b688 Mon Sep 17 00:00:00 2001
From: Miroslav Lichvar <mlichvar@redhat.com>
Date: Wed, 10 Nov 2021 14:56:31 +0100
Subject: [PATCH] ntp: add server support for experimental extension field

Maintain a server monotonic timescale needed for the experimental
extension field. It follows the best estimate of frequency without
time corrections. Implement it as an offset relative to the NTP time,
starting at zero, using a slew handler to cancel time corrections of the
NTP clock. The 32-bit epoch ID is set to a random value on start and
every step of the system clock.
---
 ntp_core.c | 61 ++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 61 insertions(+)

diff --git a/ntp_core.c b/ntp_core.c
index 9eb6758..bb92af5 100644
--- a/ntp_core.c
+++ b/ntp_core.c
@@ -294,6 +294,11 @@ static int server_sock_fd6;
 
 static ADF_AuthTable access_auth_table;
 
+/* Current offset between monotonic and cooked time, and its epoch ID
+   which is reset on clock steps */
+static double server_mono_offset;
+static uint32_t server_mono_epoch;
+
 /* Characters for printing synchronisation status and timestamping source */
 static const char leap_chars[4] = {'N', '+', '-', '?'};
 static const char tss_chars[3] = {'D', 'K', 'H'};
@@ -380,6 +385,20 @@ zero_local_timestamp(NTP_Local_Timestamp *ts)
 
 /* ================================================== */
 
+static void
+handle_slew(struct timespec *raw, struct timespec *cooked, double dfreq,
+            double doffset, LCL_ChangeType change_type, void *anything)
+{
+  if (change_type == LCL_ChangeAdjust) {
+    server_mono_offset += doffset;
+  } else {
+    UTI_GetRandomBytes(&server_mono_epoch, sizeof (server_mono_epoch));
+    server_mono_offset = 0.0;
+  }
+}
+
+/* ================================================== */
+
 void
 NCR_Initialise(void)
 {
@@ -396,6 +415,9 @@ NCR_Initialise(void)
   /* Server socket will be opened when access is allowed */
   server_sock_fd4 = INVALID_SOCK_FD;
   server_sock_fd6 = INVALID_SOCK_FD;
+
+  LCL_AddParameterChangeHandler(handle_slew, NULL);
+  handle_slew(NULL, NULL, 0.0, 0.0, LCL_ChangeUnknownStep, NULL);
 }
 
 /* ================================================== */
@@ -405,6 +427,8 @@ NCR_Finalise(void)
 {
   unsigned int i;
 
+  LCL_RemoveParameterChangeHandler(handle_slew, NULL);
+
   if (server_sock_fd4 != INVALID_SOCK_FD)
     NIO_CloseServerSocket(server_sock_fd4);
   if (server_sock_fd6 != INVALID_SOCK_FD)
@@ -925,6 +949,38 @@ receive_timeout(void *arg)
 
 /* ================================================== */
 
+static int
+add_ext_exp1(NTP_Packet *message, NTP_PacketInfo *info, struct timespec *rx,
+             double root_delay, double root_dispersion)
+{
+  struct timespec mono_rx;
+  NTP_ExtFieldExp1 exp1;
+  NTP_int64 ts_fuzz;
+
+  memset(&exp1, 0, sizeof (exp1));
+  exp1.magic = htonl(NTP_EF_EXP1_MAGIC);
+
+  if (info->mode != MODE_CLIENT) {
+    exp1.root_delay = UTI_DoubleToNtp32f28(root_delay);
+    exp1.root_dispersion = UTI_DoubleToNtp32f28(root_dispersion);
+    UTI_AddDoubleToTimespec(rx, server_mono_offset, &mono_rx);
+    UTI_GetNtp64Fuzz(&ts_fuzz, message->precision);
+    UTI_TimespecToNtp64(&mono_rx, &exp1.mono_receive_ts, &ts_fuzz);
+    exp1.mono_epoch = htonl(server_mono_epoch);
+  }
+
+  if (!NEF_AddField(message, info, NTP_EF_EXP1, &exp1, sizeof (exp1))) {
+    DEBUG_LOG("Could not add EF");
+    return 0;
+  }
+
+  info->ext_field_flags |= NTP_EF_FLAG_EXP1;
+
+  return 1;
+}
+
+/* ================================================== */
+
 static int
 transmit_packet(NTP_Mode my_mode, /* The mode this machine wants to be */
                 int interleaved, /* Flag enabling interleaved mode */
@@ -1070,6 +1126,11 @@ transmit_packet(NTP_Mode my_mode, /* The mode this machine wants to be */
     return 0;
 
   if (ext_field_flags) {
+    if (ext_field_flags & NTP_EF_FLAG_EXP1) {
+      if (!add_ext_exp1(&message, &info, &local_receive,
+                        our_root_delay, our_root_dispersion))
+        return 0;
+    }
   }
 
   do {