Diff: udp/ntp.cpp

Revision:

10:f0854784e960

Parent:

9:91dae5300a4d

Child:

17:e475ab861365

diff -r 91dae5300a4d -r f0854784e960 udp/ntp.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/udp/ntp.cpp	Sun Apr 16 14:21:55 2017 +0000
@@ -0,0 +1,89 @@
+#include  "mbed.h"
+#include   "log.h"
+#include   "net.h"
+#include "clock.h"
+#include  "sync.h"
+#include   "udp.h"
+
+#define HEADER_SIZE 48
+
+#define ERA_BASE     0
+#define ERA_PIVOT 2016
+
+#define CLIENT 3
+#define SERVER 4
+
+__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;
+};
+uint64_t makeNtpTimeStamp(int64_t ns)
+{    
+    uint64_t timestamp = ns << 2;
+    timestamp /= 1000; timestamp <<= 10;
+    timestamp /= 1000; timestamp <<= 10;
+    timestamp /= 1000; timestamp <<= 10;
+    
+    timestamp += 2208988800ULL << 32;
+    
+    return NetToHost64(timestamp);
+}
+int NtpHandleRequest(int* pSize, void * pPacket)
+{
+    if (*pSize < HEADER_SIZE) return DO_NOTHING;
+    
+    struct header* pHeader = (struct header*)pPacket;
+    
+    switch (pHeader->Mode)
+    {
+        case CLIENT:            
+            pHeader->Mode       = SERVER;
+            pHeader->LI         = 0;
+            if (ClockStartNs) pHeader->Stratum = 1;
+            else              pHeader->Stratum = 0;
+            pHeader->Poll       = 0;
+            pHeader->Precision  = 0;
+            pHeader->RootDelay  = 0;
+            pHeader->Dispersion = 0;
+            char* pRef;
+            if (ClockStartNs)
+            {
+                if (SyncedRate && SyncedTime) pRef = "GPS";
+                else                          pRef = "LOCL";
+            }
+            else
+            {
+                pRef = "INIT";
+            }
+            pHeader->RefIdentifier[0] = pRef[0]; //For stratum 1 (reference clock), this is a four-octet, left-justified, zero-padded ASCII string.
+            pHeader->RefIdentifier[1] = pRef[1];
+            pHeader->RefIdentifier[2] = pRef[2];
+            pHeader->RefIdentifier[3] = pRef[3];
+            
+            pHeader->RefTimeStamp = makeNtpTimeStamp(ClockStartNs);
+            pHeader->OriTimeStamp = pHeader->TraTimeStamp;
+            pHeader->RecTimeStamp = makeNtpTimeStamp(ClockGetNs());
+            pHeader->TraTimeStamp = makeNtpTimeStamp(ClockGetNs());
+            *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;
+}