Next round of sntp work: server and multicast modes, actually set time...

1 files changed, 145 insertions, 91 deletions

diff --git a/toys/pending/sntp.c b/toys/pending/sntp.c
index 057a30b6..b85b9dce 100644
--- a/toys/pending/sntp.c
+++ b/toys/pending/sntp.c
@@ -6,21 +6,13 @@
 
   modes: oneshot display, oneshot set, persist, serve, multi
 
-  verify source addr
-  serve time
-    - precision -6
-    - source LOCL
-    - copy source packet transmit to originate (5->3), 2=4=5
-  wait for multicast updates
-  uniclient: retry at poll interval
-
-USE_SNTP(NEWTOY(sntp, "m:Sp:asdD[!as]", TOYFLAG_USR|TOYFLAG_BIN))
+USE_SNTP(NEWTOY(sntp, "m:Sp:asdDqr#<4>17=10[!as]", TOYFLAG_USR|TOYFLAG_BIN))
 
 config SNTP
   bool "sntp"
   default n
   help
-    usage: sntp [-saS] [-dD[-m ADDRESS] [-p PORT] [SERVER]
+    usage: sntp [-saSdDqm] [-r SHIFT] [-m ADDRESS] [-p PORT] [SERVER]
 
     Simple Network Time Protocol client. Query SERVER and display time.
 
@@ -31,12 +23,15 @@ config SNTP
     -m	Wait for updates from multicast ADDRESS (RFC 4330 says use 224.0.1.1)
     -d	Daemonize (run in background re-querying )
     -D	Daemonize but stay in foreground: re-query time every 1000 seconds
+    -r	Retry shift (every 1<<SHIFT seconds)
+    -q	Quiet (don't display time)
 */
 
 #define FOR_sntp
 #include "toys.h"
 
 GLOBALS(
+  long r;
   char *p, *m;
 )
 
@@ -67,14 +62,30 @@ int xrecvwait(int fd, char *buf, int len, union socksaddr *sa, int timeout)
   return len;
 }
 
+// Adjust timespec by nanosecond offset
+static void nanomove(struct timespec *ts, long long offset)
+{
+  long long nano = ts->tv_nsec + offset, secs = nano/1000000000;
+
+  ts->tv_sec += secs;
+  nano %= 1000000000;
+  if (nano<0) {
+    ts->tv_sec--;
+    nano += 1000000000;
+  }
+  ts->tv_nsec = nano;
+}
+
 // Get time and return ntptime (saving timespec in pointer if not null)
 // NTP time is high 32 bits = seconds since 1970 (blame RFC 868), low 32 bits
 // fraction of a second.
-static unsigned long long lunchtime(struct timespec *television)
+// diff is how far off we think our clock is from reality (in nanoseconds)
+static unsigned long long lunchtime(struct timespec *television, long long diff)
 {
   struct timespec tv;
 
   clock_gettime(CLOCK_REALTIME, &tv);
+  if (diff) nanomove(&tv, diff);
 
   if (television) *television = tv;
 
@@ -99,37 +110,11 @@ static long long nanodiff(struct timespec *old, struct timespec *new)
   return (new->tv_sec - old->tv_sec)*1000000000LL+(new->tv_nsec - old->tv_nsec);
 }
 
-// Adjust timespec by nanosecond offset
-static void nanomove(struct timespec *ts, long long offset)
-{
-  long long nano = ts->tv_nsec + offset, secs = nano/1000000000;
-
-  ts->tv_sec += secs;
-  nano %= 1000000000;
-  if (nano<0) {
-    ts->tv_sec--;
-    nano += 1000000000;
-  }
-  ts->tv_nsec = nano;
-}
-
-static void server_packet(char *buf, unsigned long long ref)
-{
-  *buf = 0x24;
-  buf[1] = 3;
-  buf[2] = 10;
-  buf[3] = 253;
-  strcpy(buf+12, "LOCL");
-  pktime[3] = ref;
-  pktime[2] = pktime[4] = pktime[5] = newtime;
-}
-
 void sntp_main(void)
 {
   struct timespec tv, tv2;
   unsigned long long *pktime = (void *)toybuf, now, then, before;
-  long long diff;
-  long diffsecs, diffnano;
+  long long diff = 0;
   struct addrinfo *ai;
   union socksaddr sa;
   int fd, tries = 0;
@@ -142,7 +127,7 @@ void sntp_main(void)
   ai = xgetaddrinfo(*toys.optargs, TT.p, AF_UNSPEC, SOCK_DGRAM, IPPROTO_UDP,
     AI_PASSIVE*!*toys.optargs);
 
-  if (FLAG(d)) daemon(0,0);
+  if (FLAG(d) && daemon(0, 0)) perror_exit("daemonize");
 
   // Act as server if necessary
   if (FLAG(S)|FLAG(m)) {
@@ -152,65 +137,134 @@ void sntp_main(void)
 
       // subscribe to multicast group
       memset(&group, 0, sizeof(group));
-      group.imr_multiaddr.s_addr = inet_addr(addr);
-      xsetsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, (void *)&group,
-        sizeof(group));
+      group.imr_multiaddr.s_addr = inet_addr(TT.m);
+      xsetsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &group, sizeof(group));
     }
   } else fd = xsocket(ai->ai_family, SOCK_DGRAM, IPPROTO_UDP);
 
-// Sm - loop waiting for input
-// Dd - loop polling with probe
-// else poll 3 times
-// if SmDd loop
+  // -Sm = loop waiting for input
+  // -Dd = loop polling time and waiting until next poll period
+  // Otherwise poll up to 3 times to get 2 responses, then exit
 
+  // loop sending/receiving packets
   for (;;) {
+    // Figure out if we're in server and multicast modes don't poll
+    if (FLAG(m) || FLAG(S)) then = -1;
+
+    // daemon and oneshot modes send a packet each time through outer loop
+    else {
+      then = (now = millitime()) + 4000;
+
+      if (FLAG(d) || FLAG(D)) then = now + (1<<TT.r)*1000;
+
+      // Prepare outgoing NTP packet
+      memset(toybuf, 0, 48);
+      *toybuf = 0xe3; // li = 3 (unsynchronized), version = 4, mode = 3 (client)
+      toybuf[2] = 8;  // poll frequency 1<<8 = 256 seconds
+      pktime[5] = SWAP_BE64(before = lunchtime(&tv, diff));
+
+      // Send packet
+      xsendto(fd, toybuf, 48, ai->ai_addr);
+    }
+
+    // Loop receiving packets until it's time to send the next one.
+    while (then>0 && now<then) {
+      int strike;
+
+      // Wait to receive a packet
 
-    // Daemon mode isn't limited to 3 tries
-    if (FLAG(d) || FLAG(D)) {
-      then = retry_timeout;
-    } else if (++tries>3) break;
-
-    // Prepare outgoing NTP packet
-    memset(toybuf, 0, 48);
-    *toybuf = 0xe3; // li = 3 (unsynchronized), version = 4, mode = 3 (client)
-    toybuf[2] = 8;  // poll frequency 1<<8 = 256 seconds
-    pktime[5] = SWAP_BE64(before = lunchtime(&tv));
-
-    // Send and ye shall receive
-    xsendto(fd, toybuf, 48, ai->ai_addr);
-    then = (now = millitime())+4000;
-    while (now<then) {
-      // TODO: confirm sa matches
-      if (48 == xrecvwait(fd, toybuf, sizeof(toybuf), &sa, then-now)) {
-        if (TT.m || pktime[3] == SWAP_BE64(before)) break;
-      }
       now = millitime();
+      strike = xrecvwait(fd, toybuf, sizeof(toybuf), &sa, then-now);
+      if (strike<1) {
+        if (!(FLAG(S)||FLAG(m)||FLAG(D)||FLAG(d)) && ++tries>3)
+          error_exit("no reply from %s", *toys.optargs);
+        break;
+      }
+      if (strike<48) continue;
+
+      // Validate packet
+      if (!FLAG(S) || FLAG(m)) {
+        char buf[128];
+        int mode = 7&*toybuf;
+
+        // Is source address what we expect?
+        xstrncpy(buf, ntop(ai->ai_addr), 128);
+        strike = strcmp(buf, ntop((void *)&sa));
+        // Does this reply's orignate timestamp match the packet we sent?
+        if (!FLAG(S) && !FLAG(m) && before != SWAP_BE64(pktime[3])) continue;
+        // Ignore packets from wrong address or with wrong mode
+        if (strike && !FLAG(S)) continue;
+        if (!(FLAG(m) && mode==5 || FLAG(S) && mode==3 ||
+            !FLAG(m) && !FLAG(S) && mode==4)) continue;
+      }
+
+      // If received a -S request packet, send server packet
+      if (strike) {
+        char *buf = toybuf+48;
+
+        *buf = 0x24;  // LI 0 VN 4 mode 4.
+        buf[1] = 3;   // stratum 3
+        buf[2] = 10;  // recommended retry every 1<<10=1024 seconds
+        buf[3] = 250; // precision -6, minimum allowed
+        strcpy(buf+12, "LOCL");
+        pktime[6+3] = pktime[5]; // send back reference time they sent us
+        // everything else is current time
+        pktime[6+2] = pktime[6+4] = pktime[6+5] = lunchtime(0, 0);
+        xsendto(fd, toybuf, 48, (void *)&sa);
+
+      // Got a time packet from a recognized server
+      } else {
+        int unset = !diff;
+
+        // First packet: figure out how far off our clock is from what server
+        // said and try again. Don't set clock, just record offset to use
+        // generating second reuest. (We know this time is in the past
+        // because transmission took time, but it's a start. And if time is
+        // miraculously exact, don't loop.)
+ 
+        lunchtime(&tv2, diff);
+        diff = nanodiff(&tv, &tv2);
+        if (unset && diff) break;
+
+        // Second packet: determine midpoint of packet transit time according
+        // to local clock, assuming each direction took same time so midpoint
+        // is time server reported. The first television was the adjusted time
+        // we sent the packet at, tv2 is what server replied, so now diff
+        // is round trip time.
+
+        // What time did the server say and how far off are we?
+        nanomove(&tv, diff/2);
+        doublyso(SWAP_BE64(pktime[5]), &tv2);
+        diff = nanodiff(&tv, &tv2);
+
+        if (FLAG(s)) {
+          // Do read/adjust/set to lose as little time as possible.
+          clock_gettime(CLOCK_REALTIME, &tv2);
+          nanomove(&tv2, diff);
+          if (clock_settime(CLOCK_REALTIME, &tv2))
+            perror_exit("clock_settime");
+        } else if (FLAG(a)) {
+          struct timeval why;
+
+          // call adjtime() to move the clock gradually, copying nanoseconds
+          // into gratuitous microseconds structure for sad historical reasons
+          memset(&tv2, 0, sizeof(tv2));
+          nanomove(&tv2, diff);
+          why.tv_sec = tv2.tv_sec;
+          why.tv_usec = tv2.tv_nsec/1000;
+          if (adjtime(&why, 0)) perror_exit("adjtime");
+        }
+
+        // Display the time and offset
+        if (!FLAG(q)) {
+          format_iso_time(toybuf, sizeof(toybuf)-1, &tv2);
+          printf("%s offset %c%d.%09d secs\n", toybuf, (diff<0) ? '-' : '+',
+            abs(diff/1000000000), abs(diff%1000000000));
+        }
+
+        // If we're not in daemon mode, we're done. (Can't get here for -S.)
+        if (!FLAG(d) && !FLAG(D)) return;
+      }
     }
-    if (now < then) break;
-  }
-  lunchtime(&tv2);
-
-  // determine midpoint of packet transit time according to local clock
-  // (simple calculation: assume each direction took same time so midpoint
-  // is time reported by other clock)
-  diff = nanodiff(&tv, &tv2)/2;
-  nanomove(&tv, diff);
-
-  doublyso(SWAP_BE64(pktime[5]), &tv2);
-  diff = nanodiff(&tv, &tv2);
-  diffsecs = diff/1000000000;
-  diffnano = diff%1000000000;
-  if (FLAG(s)) {
-    clock_gettime(CLOCK_REALTIME, &tv);
-    nanomove(&tv, diff);
-    if (clock_settime(CLOCK_REALTIME, &tv)) perror_exit("clock_settime");
-  } else if (FLAG(a)) {
-    struct timeval tv = {diffsecs, diffnano/1000};
-
-    if (adjtime(&tv, 0)) perror_exit("adjtime");
-  } else {
-    format_iso_time(toybuf, sizeof(toybuf)-1, &tv2);
-    printf("%s offset %c%d.%09d secs\n", toybuf, (diff<0) ? '-' : '+',
-      abs(diffsecs), abs(diffnano));
   }
 }