File content as of revision 20:23f2b29b48ea:

#include  "mbed.h"
#include   "log.h"
#include   "net.h"
#include   "udp.h"

#define HEADER_SIZE 48

#define ERA_BASE     0
#define ERA_PIVOT 2016

#define CLIENT 3
#define SERVER 4

int64_t (*NtpGetClockRefFunction)    ();
int64_t (*NtpGetClockNowFunction)    ();
int     (*NtpGetClockStratumFunction)();
char*   (*NtpGetClockIdentFunction)  ();
bool      NtpEnableServer = false;
bool      NtpEnableClient = false;

__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;
};
int NtpHandleRequest(int* pSize, void * pPacket)
{
    if (!NtpEnableServer) 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;
    }
    
    if (*pSize < HEADER_SIZE) return DO_NOTHING;
    
    struct header* pHeader = (struct header*)pPacket;
    
    uint64_t refNtp  = NtpGetClockRefFunction();
    uint64_t nowNtp  = NtpGetClockNowFunction();
    int      stratum = NtpGetClockStratumFunction();
    char*    ident   = NtpGetClockIdentFunction();
    
    switch (pHeader->Mode)
    {
        case CLIENT:            
            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);
            *pSize = HEADER_SIZE;
            return UNICAST;
        default:
            LogTimeF("\r\nNTP packet unknown\r\n");
            LogTimeF("Mode           %d\r\n",  pHeader->Mode);
            LogTimeF("Version        %d\r\n",  pHeader->VN);
            LogTimeF("Stratum        %d\r\n",  pHeader->Stratum);
            LogTimeF("Reference      %4s\r\n", pHeader->RefIdentifier);
    }
    return DO_NOTHING;
}