1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
|
#include "project.h"
typedef struct {
union {
struct {
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;
};
uint32_t root_delay;
uint32_t root_dispersion;
uint32_t reference_id;
uint64_t reference_ts;
uint64_t origin_ts;
uint64_t receive_ts;
uint64_t transmit_ts;
} ntp_packet_t;
#if 0
packet (uint8_t *_p)
{
EPOCH reftime = pll_decompose (pll_last_update);
ntp_packet_t rx = (uint8_t *)_p;
ntp_packet_t tx;
tx.li = NTP LEAP_MEH;
tx.vn = NTP_VERSION;
tx.mode = NTP_MODE_SERVER;
tx.poll = ?
tx.precision = ?
tx.rootdelay = ?
tx.rootdisp = ?
tx.refid = FCC ('M', 'S', 'F', 0);
tx.reftime =;
}
#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;
if (!pll_valid) break;
if (!time_known) 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);
udp_recv (s, ntp_rx, NULL);
}
|
