Diff: udp/ntp/ntp.c

Revision:

112:f8694d0b8858

Parent:

106:e0f0e8ccb575

Child:

113:904b40231907

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/udp/ntp/ntp.c	Mon Jan 21 16:58:28 2019 +0000
@@ -0,0 +1,200 @@
+#include <stdint.h>
+#include <stdbool.h>
+
+#include    "log.h"
+#include    "net.h"
+#include "action.h"
+#include    "udp.h"
+#include "ntpclient.h"
+#include "clkntp.h"
+#include "clktime.h"
+
+
+bool NtpTrace = false;
+
+#define CLIENT 3
+#define SERVER 4
+
+bool       NtpServerEnable  = false;
+uint64_t (*NtpFunctionGetClockRef)      ();
+int      (*NtpFunctionGetClockStratum)  ();
+char *   (*NtpFunctionGetClockIdent)    ();
+int      (*NtpFunctionGetClockLi)       ();
+int      (*NtpFunctionGetClockPrecision)();
+
+
+__packed struct header
+{
+    unsigned Mode : 3;
+    unsigned VN   : 3;
+    unsigned LI   : 2;
+    uint8_t  Stratum; 
+     int8_t  Poll;
+     int8_t  Precision;
+    uint32_t RootDelay;
+    uint32_t Dispersion;
+    char     RefIdentifier[4];
+    
+    uint64_t RefTimeStamp;
+    uint64_t OriTimeStamp;
+    uint64_t RecTimeStamp;
+    uint64_t TraTimeStamp;
+};
+static void logHeader(struct header* pHeader)
+{
+    if (NetTraceVerbose) Log ("NTP header\r\n  ");
+    else                 Log ("NTP   header: ");
+    
+    LogF("Mode %d",         pHeader->Mode);
+    LogF(", Version %d",    pHeader->VN);
+    LogF(", LI %d",         pHeader->LI);
+    LogF(", Stratum %d",    pHeader->Stratum);
+    LogF(", Poll %d",       pHeader->Poll);
+    LogF(", Precision %d",  pHeader->Precision);
+    LogF(", Root delay %d", NetToHost32(pHeader->RootDelay));
+    LogF(", Dispersion %d", NetToHost32(pHeader->Dispersion));
+    Log (", Ident ");
+    for (int i = 0; i < 4; i++) if (pHeader->RefIdentifier[i]) LogPush(pHeader->RefIdentifier[i]);
+    Log ("\r\n");
+    
+    if (NetTraceVerbose)
+    {
+        LogF("  REF %llu\r\n",  NetToHost64(pHeader->RefTimeStamp));
+        LogF("  ORI %llu\r\n",  NetToHost64(pHeader->OriTimeStamp));
+        LogF("  REC %llu\r\n",  NetToHost64(pHeader->RecTimeStamp));
+        LogF("  TRA %llu\r\n",  NetToHost64(pHeader->TraTimeStamp));
+    }
+}
+static void (*pTraceBack)(void);
+static int handleRequest(struct header* pHeaderRx, struct header* pHeaderTx)
+{
+    if (!NtpServerEnable) return DO_NOTHING;
+    
+    if (!NtpFunctionGetClockRef     ||
+        !NtpFunctionGetClockStratum ||
+        !NtpFunctionGetClockIdent   ||
+        !NtpFunctionGetClockLi      ||
+        !NtpFunctionGetClockPrecision)
+    {
+        LogTimeF("NtpHandleRequest - NTP server is enabled but has not been plumbed into a clock\r\n");
+        return DO_NOTHING;
+    }
+
+    if (NtpTrace)
+    {
+        if (NetTraceNewLine) Log("\r\n");
+        LogTimeF("NTP received request\r\n");
+        if (NetTraceStack) pTraceBack();
+        logHeader(pHeaderRx);
+    }
+    
+    uint64_t refNtp    = NtpFunctionGetClockRef();
+    uint64_t nowNtp    = ClkTimeToNtpTimeStamp(ClkTimeGet());
+    int      stratum   = NtpFunctionGetClockStratum();
+    char*    ident     = NtpFunctionGetClockIdent();
+    int      li        = NtpFunctionGetClockLi();
+    int      precision = NtpFunctionGetClockPrecision();
+
+    pHeaderTx->Mode             = SERVER;
+    pHeaderTx->VN               = pHeaderRx->VN;
+    pHeaderTx->LI               = li;
+    pHeaderTx->Stratum          = stratum;
+    pHeaderTx->Poll             = pHeaderRx->Poll;
+    pHeaderTx->Precision        = precision;
+    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);
+    
+    if (NtpTrace) logHeader(pHeaderTx);
+    return UNICAST;
+}
+static void handleReply(struct header* pHeader)
+{
+    if (NtpTrace)
+    {
+        if (NetTraceNewLine) Log("\r\n");
+        LogTimeF("NTP received reply\r\n");
+        if (NetTraceStack) pTraceBack();
+        logHeader(pHeader);
+    }
+    
+    uint64_t ori = NetToHost64(pHeader->OriTimeStamp);
+    uint64_t rec = NetToHost64(pHeader->RecTimeStamp);
+    int      li  =             pHeader->LI;
+    
+    NtpClientSetClockTime(ori, rec, li);
+}
+void NtpHdrWriteRequest(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(ClkTimeToNtpTimeStamp(ClkTimeGet()));
+
+    *pSize = sizeof(struct header);
+}
+
+int NtpHandlePacketReceived(void (*traceback)(void), int sizeRx, void * pPacketRx, int* pSizeTx, void* pPacketTx)
+{
+    pTraceBack = traceback;
+    
+    if (sizeRx != sizeof(struct header))
+    {
+        LogTimeF("\r\nNTP packet wrong size %d\r\n", sizeRx);
+        return DO_NOTHING;
+    }
+    struct header* pHeaderRx = (struct header*)pPacketRx;
+    struct header* pHeaderTx = (struct header*)pPacketTx;
+    
+    int dest = DO_NOTHING;
+    switch (pHeaderRx->Mode)
+    {
+        case CLIENT: dest = handleRequest(pHeaderRx, pHeaderTx); break;
+        case SERVER:        handleReply  (pHeaderRx); return DO_NOTHING;
+        default:
+            LogTimeF("\r\nNTP packet unknown mode %d\r\n", pHeaderRx->Mode);
+            return DO_NOTHING;
+    }
+    
+    if (dest == DO_NOTHING) return DO_NOTHING;
+    *pSizeTx = sizeof(struct header);
+    
+    return ActionMakeFromDestAndTrace(dest, NtpTrace && NetTraceStack);
+}
+int NtpPollForPacketToSend(int type, void* pPacket, int* pSize)
+{    
+    int dest = NtpClientPollForPacketToSend(type, pPacket, pSize);
+    if (!dest) return DO_NOTHING;
+    
+    if (NtpTrace)
+    {
+        if (NetTraceNewLine) Log("\r\n");
+        LogTimeF("Sending NTP request\r\n");
+        logHeader((struct header*)pPacket);
+    }
+    return ActionMakeFromDestAndTrace(dest, NtpTrace && NetTraceStack);
+
+
+}
+