Diff: udp/ntp.cpp

Revision:

59:e0e556c8bd46

Parent:

50:492f2d2954e4

--- a/udp/ntp.cpp	Thu Dec 07 20:44:32 2017 +0000
+++ b/udp/ntp.cpp	Thu Dec 14 20:55:40 2017 +0000
@@ -16,10 +16,6 @@
 
 bool NtpTrace = false;
 
-#define HEADER_SIZE 48
-
-#define USE_IP4 false
-
 #define CLIENT 3
 #define SERVER 4
 
@@ -89,7 +85,7 @@
         Log ("\r\n");
     }
 }
-static int handleRequest(struct header* pHeader)
+static int handleRequest(struct header* pHeaderRx, struct header* pHeaderTx)
 {
     if (!NtpServerEnable) return DO_NOTHING;
     
@@ -104,36 +100,30 @@
     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);
+    pHeaderTx->Mode             = SERVER;
+    pHeaderTx->LI               = 0;
+    pHeaderTx->Stratum          = stratum;
+    pHeaderTx->Poll             = 0;
+    pHeaderTx->Precision        = 0;
+    pHeaderTx->RootDelay        = 0;
+    pHeaderTx->Dispersion       = 0;
+    pHeaderTx->RefIdentifier[0] = ident[0]; //For stratum 1 (reference clock), this is a four-octet, left-justified, zero-padded ASCII string.
+    pHeaderTx->RefIdentifier[1] = ident[1];
+    pHeaderTx->RefIdentifier[2] = ident[2];
+    pHeaderTx->RefIdentifier[3] = ident[3];
+    pHeaderTx->RefTimeStamp     = NetToHost64(refNtp);
+    pHeaderTx->OriTimeStamp     = pHeaderRx->TraTimeStamp;
+    pHeaderTx->RecTimeStamp     = NetToHost64(nowNtp);
+    pHeaderTx->TraTimeStamp     = NetToHost64(nowNtp);
     return UNICAST;
 }
 static void (*pTraceBack)(void);
-static int handleReply(struct header* pHeader)
+static void 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;
+        return;
     }
     if (NtpTrace)
     {
@@ -147,50 +137,56 @@
     uint64_t rec = NetToHost64(pHeader->RecTimeStamp);
     
     NtpSetClockFunction(ori, rec);
-    return DO_NOTHING;
 }
-int NtpHandlePacketReceived(void (*traceback)(void), int* pSize, void * pPacket)
+int NtpHandlePacketReceived(void (*traceback)(void), int sizeRx, void * pPacketRx, int* pSizeTx, void* pPacketTx)
 {
     pTraceBack = traceback;
     
-    if (*pSize != HEADER_SIZE)
+    if (sizeRx != sizeof(header))
     {
-        LogTimeF("\r\nNTP packet wrong size %d\r\n", *pSize);
+        LogTimeF("\r\nNTP packet wrong size %d\r\n", sizeRx);
         return DO_NOTHING;
     }
-    struct header* pHeader = (struct header*)pPacket;
+    struct header* pHeaderRx = (struct header*)pPacketRx;
+    struct header* pHeaderTx = (struct header*)pPacketTx;
     
-    switch (pHeader->Mode)
+    int dest = DO_NOTHING;
+    switch (pHeaderRx->Mode)
     {
-        case CLIENT: return handleRequest(pHeader);            
-        case SERVER: return handleReply(pHeader);
+        case CLIENT: dest = handleRequest(pHeaderRx, pHeaderTx); break;
+        case SERVER:        handleReply  (pHeaderRx); return DO_NOTHING;
         default:
-            LogTimeF("\r\nNTP packet unknown mode %d\r\n", pHeader->Mode);
+            LogTimeF("\r\nNTP packet unknown mode %d\r\n", pHeaderRx->Mode);
             return DO_NOTHING;
     }
+    
+    if (dest == DO_NOTHING) return DO_NOTHING;
+    *pSizeTx = sizeof(header);
+    
+    return ActionMakeFromDestAndTrace(dest, NtpTrace && NetTraceStack);
 }
 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->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;
+    pHeader->RefTimeStamp     = 0;
+    pHeader->OriTimeStamp     = 0;
+    pHeader->RecTimeStamp     = 0;
+    pHeader->TraTimeStamp     = NetToHost64(NtpGetClockNowFunction());
+
+    *pSize = sizeof(header);
 
     return UNICAST_NTP;
 }