Diff: ntp.cpp

Revision:

0:faa09bd4e6bf

Child:

1:5f70c361db20

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ntp.cpp	Mon Jan 09 13:29:41 2017 +0000
@@ -0,0 +1,86 @@
+#include "mbed.h"
+#include "net.h"
+#include "gps.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(time_t seconds, int ms)
+{
+    uint64_t timestampMs = ms;
+    timestampMs <<= 32;
+    timestampMs /= 1000;
+    
+    uint64_t timestampS = seconds;
+    timestampS += 2208988800ULL; //Add 1970 - 1900 to align the epochs
+    timestampS <<= 32;           //put the seconds into the upper 32 bits
+    
+    return NetToHost64(timestampS + timestampMs);
+}
+int NtpHandleRequest(int size, void * pPacket)
+{
+    if (size < HEADER_SIZE) return 0;
+    
+    struct header* pHeader = (struct header*)pPacket;
+    
+    switch (pHeader->Mode)
+    {
+        case CLIENT:            
+            pHeader->Mode       = SERVER;
+            pHeader->LI         = 0;
+            if (GpsStartTime) pHeader->Stratum = 1;
+            else              pHeader->Stratum = 0;
+            pHeader->Poll       = 0;
+            pHeader->Precision  = 0;
+            pHeader->RootDelay  = 0;
+            pHeader->Dispersion = 0;
+            if (GpsStartTime)
+            {
+                pHeader->RefIdentifier[0] = 'G'; //For stratum 1 (reference clock), this is a four-octet, left-justified, zero-padded ASCII string.
+                pHeader->RefIdentifier[1] = 'P';
+                pHeader->RefIdentifier[2] = 'S';
+                pHeader->RefIdentifier[3] =  0 ;
+            }
+            else
+            {
+                pHeader->RefIdentifier[0] = 'I'; //For stratum 1 (reference clock), this is a four-octet, left-justified, zero-padded ASCII string.
+                pHeader->RefIdentifier[1] = 'N';
+                pHeader->RefIdentifier[2] = 'I';
+                pHeader->RefIdentifier[3] = 'T' ;
+            }
+            
+            pHeader->RefTimeStamp = makeNtpTimeStamp(GpsStartTime, GpsStartMs);
+            pHeader->OriTimeStamp = pHeader->TraTimeStamp;
+            pHeader->RecTimeStamp = makeNtpTimeStamp(GpsTime, GpsMs);
+            pHeader->TraTimeStamp = makeNtpTimeStamp(GpsTime, GpsMs);
+            return HEADER_SIZE;
+        default:
+            printf("\r\nNTP packet unknown\r\n");
+            printf("Mode           %d\r\n", pHeader->Mode);
+            printf("Version        %d\r\n", pHeader->VN);
+            printf("Stratum        %d\r\n", pHeader->Stratum);
+            printf("Reference      %4s\r\n", pHeader->RefIdentifier);
+    }
+    return 0;
+}