Diff: udp/ntp.c

Revision:

61:aad055f1b0d1

Parent:

59:e0e556c8bd46

Child:

68:e8c002e5ee4b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/udp/ntp.c	Thu Jan 11 17:38:21 2018 +0000
@@ -0,0 +1,268 @@
+#include <stdint.h>
+#include <stdbool.h>
+
+#include    "log.h"
+#include    "net.h"
+#include "action.h"
+#include "ip6addr.h"
+#include    "udp.h"
+#include    "ar4.h"
+#include    "ar6.h"
+#include    "arp.h"
+#include    "eth.h"
+#include    "nr4.h"
+#include    "nr6.h"
+#include    "dns.h"
+#include    "mac.h"
+
+bool NtpTrace = false;
+
+#define CLIENT 3
+#define SERVER 4
+
+uint64_t (*NtpGetClockNowFunction)    ();
+
+bool       NtpServerEnable  = false;
+uint64_t (*NtpGetClockRefFunction)    ();
+int      (*NtpGetClockStratumFunction)();
+char *   (*NtpGetClockIdentFunction)  ();
+
+uint32_t   NtpServerIp4;
+char       NtpServerIp6[16];
+bool       NtpClientRequest = false;
+char       NtpServerName[DNS_MAX_LABEL_LENGTH+1];
+void     (*NtpSetClockFunction)       (uint64_t ori, uint64_t rec);
+
+__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 int handleRequest(struct header* pHeaderRx, struct header* pHeaderTx)
+{
+    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;
+    }
+        
+    uint64_t refNtp  = NtpGetClockRefFunction();
+    uint64_t nowNtp  = NtpGetClockNowFunction();
+    int      stratum = NtpGetClockStratumFunction();
+    char*    ident   = NtpGetClockIdentFunction();
+
+    pHeaderTx->Mode             = SERVER;
+    pHeaderTx->VN               = 3;
+    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 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;
+    }
+    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);
+    
+    NtpSetClockFunction(ori, rec);
+}
+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);
+}
+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 = sizeof(struct header);
+
+    return UNICAST_NTP;
+}
+static bool resolve4(char* server, uint32_t* pIp)
+{
+    //Check if have IP, if not, then request it and stop
+    Nr4NameToIp(server, pIp);
+    if (!*pIp)
+    {
+        Nr4MakeRequestForIpFromName(server); //The request is only repeated if made after a freeze time - call as often as you want.
+        return false;
+    }
+
+    //Check if have MAC and, if not, request it and stop
+    char mac[6];
+    Ar4IpToMac(*pIp, mac);
+    if (MacIsEmpty(mac))
+    {
+        Ar4MakeRequestForMacFromIp(*pIp); //The request is only repeated if made after a freeze time - call as often as you want.
+        return false;
+    }
+    
+    return true;
+}
+static bool resolve6(char* server, char* ip)
+{
+    //Check if have IP, if not, then request it and stop
+    Nr6NameToIp(server, ip);
+    if (Ip6AddressIsEmpty(ip))
+    {
+        Nr6MakeRequestForIpFromName(server); //The request is only repeated if made after a freeze time - call as often as you want.
+        return false;
+    }
+
+    //Check if have MAC and, if not, request it and stop
+    char mac[6];
+    Ar6IpToMac(ip, mac);
+    if (MacIsEmpty(mac))
+    {
+        Ar6MakeRequestForMacFromIp(ip); //The request is only repeated if made after a freeze time - call as often as you want.
+        return false;
+    }
+    
+    return true;
+}
+int NtpPollForPacketToSend(int type, void* pPacket, int* pSize)
+{
+    if (!NtpClientRequest) return DO_NOTHING; //Wait until a request for time is made
+
+    if (!NtpServerName[0])
+    {
+        LogTimeF("NtpPollForRequestToSend - A request to send a client message has been made but no server name has been specified\r\n");
+        NtpClientRequest = false;
+        return DO_NOTHING;
+    }
+    
+    if (!NtpGetClockNowFunction || !NtpSetClockFunction)
+    {
+        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;
+    }
+    
+    if (type == IPV4)
+    {
+        if (!resolve4(NtpServerName, &NtpServerIp4)) return DO_NOTHING;
+    }
+    else if (type == IPV6)
+    {
+        if (!resolve6(NtpServerName, NtpServerIp6)) return DO_NOTHING;
+    }
+    else
+    {
+        return DO_NOTHING;
+    }
+
+    //Have IP and MAC so send request
+    NtpClientRequest = false;
+    int dest = sendRequest(pPacket, pSize);
+    if (NtpTrace)
+    {
+        if (NetTraceNewLine) Log("\r\n");
+        LogTimeF("Sending NTP request\r\n");
+        logHeader((struct header*)pPacket);
+    }
+    return ActionMakeFromDestAndTrace(dest, NtpTrace && NetTraceStack);
+}
+