diff options
Diffstat (limited to 'app/ntp.c')
| -rw-r--r-- | app/ntp.c | 74 |
1 files changed, 56 insertions, 18 deletions
@@ -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); } |