Working

1 files changed, 56 insertions, 18 deletions

diff --git a/app/ntp.c b/app/ntp.c
index 44002cc..2aa33e4 100644
--- a/app/ntp.c
+++ b/app/ntp.c
@@ -4,12 +4,12 @@
 typedef struct {
   union {
     struct {
-      uint32_t li: 2;
-      uint32_t vn: 3;
-      uint32_t mode: 3;
-      uint32_t stratum: 8;
-      uint32_t poll: 8;
       uint32_t precision: 8;
+      uint32_t poll: 8;
+      uint32_t stratum: 8;
+      uint32_t mode: 3;
+      uint32_t vn: 3;
+      uint32_t li: 2;
     };
     uint32_t word_0;
   };
@@ -49,36 +49,74 @@ packet (uint8_t *_p)
 
 
 
+}
+#endif
 
+static uint64_t ntp_ts (uint64_t v)
+{
+  uint64_t ret;
+  EPOCH e = pll_decompose (v);
 
+  ret = e.ns;
+  ret <<= 32;
+  ret = ret / 1000000000;
+  ret = htonl (ret);
+  ret <<= 32;
+  ret |= htonl (2208988800UL + e.s);
 
+  return ret;
+}
 
+static void ntp_rx (void *arg, struct udp_pcb *s, struct pbuf *p, struct ip_addr *src_addr, u16_t port)
+{
+  uint32_t now = SCS_DWT_CYCCNT;
+  struct ip_addr dst_addr = *src_addr;
+  ntp_packet_t pkt;
 
+  do {
+    if (p->type != PBUF_POOL) break;
 
+    if (p->len < sizeof (ntp_packet_t)) break;
 
+    memcpy (&pkt, p->payload, sizeof (ntp_packet_t));
+    pbuf_realloc (p, sizeof (ntp_packet_t));
 
+    pkt.li = 0;
+    pkt.vn = 4;
+    pkt.mode = 4;
+    pkt.stratum = 1;
+    pkt.poll = 3;
+    pkt.precision = 0xec;
+    pkt.word_0 = htonl (pkt.word_0);
 
+    pkt.root_delay = htonl (0x0);
+    pkt.root_dispersion = htonl (0x825);
+    pkt.reference_id = htonl (0x47505300);
 
+    pkt.origin_ts = pkt.transmit_ts;
+    pkt.receive_ts = ntp_ts (abs_extend (now));
+    pkt.reference_ts = ntp_ts (pll_last_update);
 
+    now = SCS_DWT_CYCCNT;
+    pkt.transmit_ts = ntp_ts (abs_extend (now));
 
+    memcpy (p->payload, &pkt, sizeof (ntp_packet_t));
 
+    udp_sendto (s, p, &dst_addr, port);
+  } while (0);
 
+  /* Free the p buffer */
+  pbuf_free (p);
+}
 
 
+void ntp_init (void)
+{
+  struct udp_pcb *s;
 
+  s = udp_new();
 
+  udp_bind (s, IP_ADDR_ANY, 123);
 
-
-
-
-
-
-
-
-}
-#endif
-
-
-void ntp_init(void)
-{
+  udp_recv (s, ntp_rx, NULL);
 }