Diff: udp/ntp.cpp

Revision:

22:914b970356f0

Parent:

20:23f2b29b48ea

Child:

35:93c39d260a83

--- a/udp/ntp.cpp	Thu Jun 29 19:42:28 2017 +0000
+++ b/udp/ntp.cpp	Mon Jul 03 14:29:07 2017 +0000
@@ -2,21 +2,28 @@
 #include   "log.h"
 #include   "net.h"
 #include   "udp.h"
+#include    "ar.h"
+#include   "arp.h"
+#include    "io.h"
+#include   "eth.h"
+
+#define DEBUG false
 
 #define HEADER_SIZE 48
 
-#define ERA_BASE     0
-#define ERA_PIVOT 2016
-
 #define CLIENT 3
 #define SERVER 4
 
-int64_t (*NtpGetClockRefFunction)    ();
-int64_t (*NtpGetClockNowFunction)    ();
-int     (*NtpGetClockStratumFunction)();
-char*   (*NtpGetClockIdentFunction)  ();
-bool      NtpEnableServer = false;
-bool      NtpEnableClient = false;
+uint64_t (*NtpGetClockNowFunction)    ();
+
+bool       NtpServerEnable  = false;
+uint64_t (*NtpGetClockRefFunction)    ();
+int      (*NtpGetClockStratumFunction)();
+char *   (*NtpGetClockIdentFunction)  ();
+
+bool       NtpClientRequest = false;
+uint32_t   NtpServerIp4 = 0;
+void     (*NtpSetClockFunction)       (uint64_t ori, uint64_t rec);
 
 __packed struct header {
     unsigned Mode : 3;
@@ -34,52 +41,160 @@
     uint64_t RecTimeStamp;
     uint64_t TraTimeStamp;
 };
-int NtpHandleRequest(int* pSize, void * pPacket)
+
+static int handleRequest(struct header* pHeader)
 {
-    if (!NtpEnableServer) return DO_NOTHING;
+    if (!NtpServerEnable) return DO_NOTHING;
     
     if (!NtpGetClockRefFunction || !NtpGetClockNowFunction || !NtpGetClockStratumFunction || !NtpGetClockIdentFunction)
     {
         LogTimeF("NtpHandleRequest - NTP server is enabled but has not been plumbed into a clock\r\n");
         return DO_NOTHING;
     }
-    
-    if (*pSize < HEADER_SIZE) return DO_NOTHING;
-    
-    struct header* pHeader = (struct header*)pPacket;
-    
+        
     uint64_t refNtp  = NtpGetClockRefFunction();
     uint64_t nowNtp  = NtpGetClockNowFunction();
     int      stratum = NtpGetClockStratumFunction();
     char*    ident   = NtpGetClockIdentFunction();
+
+    pHeader->Mode       = SERVER;
+    pHeader->LI         = 0;
+    pHeader->Stratum    = stratum;
+    pHeader->Poll       = 0;
+    pHeader->Precision  = 0;
+    pHeader->RootDelay  = 0;
+    pHeader->Dispersion = 0;
+    pHeader->RefIdentifier[0] = ident[0]; //For stratum 1 (reference clock), this is a four-octet, left-justified, zero-padded ASCII string.
+    pHeader->RefIdentifier[1] = ident[1];
+    pHeader->RefIdentifier[2] = ident[2];
+    pHeader->RefIdentifier[3] = ident[3];
+    
+    pHeader->RefTimeStamp = NetToHost64(refNtp);
+    pHeader->OriTimeStamp = pHeader->TraTimeStamp;
+    pHeader->RecTimeStamp = NetToHost64(nowNtp);
+    pHeader->TraTimeStamp = NetToHost64(nowNtp);
+    return UNICAST;
+}
+static int handleReply(struct header* pHeader)
+{
+    if (!NtpGetClockNowFunction || !NtpSetClockFunction)
+    {
+        LogTimeF("Ntp reply has been received but NTP has not been plumbed into a clock\r\n");
+        return DO_NOTHING;
+    }
+    if (pHeader->Mode != SERVER)
+    {
+        LogTimeF("Ntp reply has been received but mode is not SERVER it is %d\r\n", pHeader->Mode);
+        return DO_NOTHING;
+    }
+    if (DEBUG)
+    {
+        LogTimeF("NTP received reply\r\n");
+        LogTimeF("REF %llu\r\n", NetToHost64(pHeader->RefTimeStamp));
+        LogTimeF("ORI %llu\r\n", NetToHost64(pHeader->OriTimeStamp));
+        LogTimeF("REC %llu\r\n", NetToHost64(pHeader->RecTimeStamp));
+        LogTimeF("TRA %llu\r\n", NetToHost64(pHeader->TraTimeStamp));
+    }
+    uint64_t ori = NetToHost64(pHeader->OriTimeStamp);
+    uint64_t rec = NetToHost64(pHeader->RecTimeStamp);
+    
+    NtpSetClockFunction(ori, rec);
+    return DO_NOTHING;
+}
+int NtpHandlePacketReceived(int* pSize, void * pPacket)
+{
+    if (*pSize != HEADER_SIZE)
+    {
+        LogTimeF("\r\nNTP packet wrong size %d\r\n", *pSize);
+        return DO_NOTHING;
+    }
+    struct header* pHeader = (struct header*)pPacket;
     
     switch (pHeader->Mode)
     {
-        case CLIENT:            
-            pHeader->Mode       = SERVER;
-            pHeader->LI         = 0;
-            pHeader->Stratum    = stratum;
-            pHeader->Poll       = 0;
-            pHeader->Precision  = 0;
-            pHeader->RootDelay  = 0;
-            pHeader->Dispersion = 0;
-            pHeader->RefIdentifier[0] = ident[0]; //For stratum 1 (reference clock), this is a four-octet, left-justified, zero-padded ASCII string.
-            pHeader->RefIdentifier[1] = ident[1];
-            pHeader->RefIdentifier[2] = ident[2];
-            pHeader->RefIdentifier[3] = ident[3];
-            
-            pHeader->RefTimeStamp = NetToHost64(refNtp);
-            pHeader->OriTimeStamp = pHeader->TraTimeStamp;
-            pHeader->RecTimeStamp = NetToHost64(nowNtp);
-            pHeader->TraTimeStamp = NetToHost64(nowNtp);
-            *pSize = HEADER_SIZE;
-            return UNICAST;
+        case CLIENT: return handleRequest(pHeader);            
+        case SERVER: return handleReply(pHeader);
         default:
-            LogTimeF("\r\nNTP packet unknown\r\n");
-            LogTimeF("Mode           %d\r\n",  pHeader->Mode);
-            LogTimeF("Version        %d\r\n",  pHeader->VN);
-            LogTimeF("Stratum        %d\r\n",  pHeader->Stratum);
-            LogTimeF("Reference      %4s\r\n", pHeader->RefIdentifier);
+            LogTimeF("\r\nNTP packet unknown mode %d\r\n", pHeader->Mode);
+            return DO_NOTHING;
     }
-    return DO_NOTHING;
+}
+static int sendRequest(void* pPacket, int* pSize)
+{
+    struct header* pHeader = (struct header*)pPacket;
+    
+    pHeader->Mode       = CLIENT;
+    pHeader->VN         = 3;
+    pHeader->LI         = 0;
+    pHeader->Stratum    = 0;
+    pHeader->Poll       = 0;
+    pHeader->Precision  = 0;
+    pHeader->RootDelay  = 0;
+    pHeader->Dispersion = 0;
+    pHeader->RefIdentifier[0] = 0;
+    pHeader->RefIdentifier[1] = 0;
+    pHeader->RefIdentifier[2] = 0;
+    pHeader->RefIdentifier[3] = 0;
+    
+    pHeader->RefTimeStamp = 0;
+    pHeader->OriTimeStamp = 0;
+    pHeader->RecTimeStamp = 0;
+    pHeader->TraTimeStamp = NetToHost64(NtpGetClockNowFunction());
+    *pSize = HEADER_SIZE;
+
+    return UNICAST_NTP;
 }
+static bool arpRequested = true;
+static uint32_t elapsed = 0;
+static uint32_t started = 0;
+int NtpPollForPacketToSend(int type, void* pPacket, int* pSize)
+{
+    if (!NtpClientRequest) return DO_NOTHING; //Wait until a request for time is made
+    if (type != IPV4) return DO_NOTHING;      //Only have an IP4 address at this moment
+
+    if (!NtpGetClockNowFunction || !NtpSetClockFunction || !NtpServerIp4)
+    {
+        LogTimeF("NtpPollForRequestToSend - A request to send a client message has been made but NTP has not been plumbed into a clock\r\n");
+        NtpClientRequest = false;
+        return DO_NOTHING;
+    }
+    
+    //Check if have MAC
+    char mac[6];
+    ArRev4(NtpServerIp4, mac);
+    if (mac[0] || mac[1] || mac[2] || mac[3] || mac[4] || mac[5])
+    {
+        //Yes - send request
+        if (DEBUG) LogTimeF("Sending NTP request\r\n");
+        NtpClientRequest = false;
+        arpRequested = false;
+        return sendRequest(pPacket, pSize);
+    }
+    else
+    {
+        //No - send RARP
+        if (!arpRequested)
+        {
+            if (DEBUG) LogTimeF("Sending NTP RARP request\r\n");
+            ArpAddressToResolve = NtpServerIp4;
+            ArpResolveRequestFlag = true;
+            arpRequested = true;
+            started = elapsed;
+        }
+        else
+        {
+            if (elapsed - started > 2)
+            {
+                if (DEBUG) LogTimeF("Reaped NTP RARP request\r\n");
+                NtpClientRequest = false;
+                arpRequested = false;
+            }
+        }
+        return DO_NOTHING;
+    }
+}
+
+void NtpTick()
+{
+    elapsed++;
+}