File content as of revision 22:914b970356f0:

#include  "mbed.h"
#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 CLIENT 3
#define SERVER 4

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;
    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 int handleRequest(struct header* pHeader)
{
    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();

    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: return handleRequest(pHeader);            
        case SERVER: return handleReply(pHeader);
        default:
            LogTimeF("\r\nNTP packet unknown mode %d\r\n", pHeader->Mode);
            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++;
}