File content as of revision 61:aad055f1b0d1:

#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);
}