1 /*
2 * Copyright (c) 2017 Linaro Limited
3 * Copyright (c) 2019 Intel Corporation
4 * Copyright (c) 2024 Embeint Inc
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include <zephyr/logging/log.h>
10 LOG_MODULE_REGISTER(net_sntp, CONFIG_SNTP_LOG_LEVEL);
11
12 #include <zephyr/net/sntp.h>
13 #include "sntp_pkt.h"
14 #include <limits.h>
15
16 #define SNTP_LI_MAX 3
17 #define SNTP_VERSION_NUMBER 3
18 #define SNTP_MODE_CLIENT 3
19 #define SNTP_MODE_SERVER 4
20 #define SNTP_STRATUM_KOD 0 /* kiss-o'-death */
21 #define OFFSET_1970_JAN_1 2208988800
22
sntp_pkt_dump(struct sntp_pkt * pkt)23 static void sntp_pkt_dump(struct sntp_pkt *pkt)
24 {
25 if (!pkt) {
26 return;
27 }
28
29 NET_DBG("li %x", pkt->li);
30 NET_DBG("vn %x", pkt->vn);
31 NET_DBG("mode %x", pkt->mode);
32 NET_DBG("stratum: %x", pkt->stratum);
33 NET_DBG("poll: %x", pkt->poll);
34 NET_DBG("precision: %x", pkt->precision);
35 NET_DBG("root_delay: %x", ntohl(pkt->root_delay));
36 NET_DBG("root_dispersion: %x", ntohl(pkt->root_dispersion));
37 NET_DBG("ref_id: %x", ntohl(pkt->ref_id));
38 NET_DBG("ref_tm_s: %x", ntohl(pkt->ref_tm_s));
39 NET_DBG("ref_tm_f: %x", ntohl(pkt->ref_tm_f));
40 NET_DBG("orig_tm_s: %x", ntohl(pkt->orig_tm_s));
41 NET_DBG("orig_tm_f: %x", ntohl(pkt->orig_tm_f));
42 NET_DBG("rx_tm_s: %x", ntohl(pkt->rx_tm_s));
43 NET_DBG("rx_tm_f: %x", ntohl(pkt->rx_tm_f));
44 NET_DBG("tx_tm_s: %x", ntohl(pkt->tx_tm_s));
45 NET_DBG("tx_tm_f: %x", ntohl(pkt->tx_tm_f));
46 }
47
48 #if defined(CONFIG_SNTP_UNCERTAINTY)
q16_16_s_to_ll_us(uint32_t t)49 static int64_t q16_16_s_to_ll_us(uint32_t t)
50 {
51 return (int64_t)(t >> 16) * (int64_t)USEC_PER_SEC +
52 (((int64_t)(t & 0xFFFF) * (int64_t)USEC_PER_SEC) >> 16);
53 }
54
q32_32_s_to_ll_us(uint32_t t_s,uint32_t t_f)55 static int64_t q32_32_s_to_ll_us(uint32_t t_s, uint32_t t_f)
56 {
57 return (uint64_t)t_s * USEC_PER_SEC + (((uint64_t)t_f * (uint64_t)USEC_PER_SEC) >> 32);
58 }
59 #endif
60
parse_response(uint8_t * data,uint16_t len,struct sntp_time * expected_orig_ts,struct sntp_time * res)61 static int32_t parse_response(uint8_t *data, uint16_t len, struct sntp_time *expected_orig_ts,
62 struct sntp_time *res)
63 {
64 struct sntp_pkt *pkt = (struct sntp_pkt *)data;
65 uint32_t ts;
66
67 sntp_pkt_dump(pkt);
68
69 if (ntohl(pkt->orig_tm_s) != expected_orig_ts->seconds ||
70 ntohl(pkt->orig_tm_f) != expected_orig_ts->fraction) {
71 NET_DBG("Mismatch originate timestamp: %d.%09d, expect: %llu.%09u",
72 ntohl(pkt->orig_tm_s), ntohl(pkt->orig_tm_f), expected_orig_ts->seconds,
73 expected_orig_ts->fraction);
74 return -ERANGE;
75 }
76
77 if (pkt->mode != SNTP_MODE_SERVER) {
78 /* For unicast and manycast, server should return 4.
79 * For broadcast (which is not supported now), server should
80 * return 5.
81 */
82 NET_DBG("Unexpected mode: %d", pkt->mode);
83 return -EINVAL;
84 }
85
86 if (pkt->stratum == SNTP_STRATUM_KOD) {
87 NET_DBG("kiss-o'-death stratum");
88 return -EBUSY;
89 }
90
91 if (ntohl(pkt->tx_tm_s) == 0 && ntohl(pkt->tx_tm_f) == 0) {
92 NET_DBG("zero transmit timestamp");
93 return -EINVAL;
94 }
95
96 #if defined(CONFIG_SNTP_UNCERTAINTY)
97
98 int64_t dest_ts_us = k_ticks_to_us_near64(k_uptime_ticks());
99 int64_t orig_ts_us =
100 q32_32_s_to_ll_us(expected_orig_ts->seconds, expected_orig_ts->fraction);
101
102 int64_t rx_ts_us = q32_32_s_to_ll_us(ntohl(pkt->rx_tm_s), ntohl(pkt->rx_tm_f));
103 int64_t tx_ts_us = q32_32_s_to_ll_us(ntohl(pkt->tx_tm_s), ntohl(pkt->tx_tm_f));
104
105 if (rx_ts_us > tx_ts_us || orig_ts_us > dest_ts_us) {
106 NET_DBG("Invalid timestamps from SNTP server");
107 return -EINVAL;
108 }
109
110 int64_t d_us = (dest_ts_us - orig_ts_us) - (tx_ts_us - rx_ts_us);
111 int64_t clk_offset_us = ((rx_ts_us - orig_ts_us) + (tx_ts_us - dest_ts_us)) / 2;
112 int64_t root_dispersion_us = q16_16_s_to_ll_us(ntohl(pkt->root_dispersion));
113 int64_t root_delay_us = q16_16_s_to_ll_us(ntohl(pkt->root_delay));
114 uint32_t precision_us;
115
116 if (pkt->precision <= 0) {
117 precision_us = (uint32_t)(USEC_PER_SEC + USEC_PER_SEC / 2) >> -pkt->precision;
118 } else if (pkt->precision <= 10) {
119 precision_us = (uint32_t)(USEC_PER_SEC + USEC_PER_SEC / 2) << pkt->precision;
120 } else {
121 NET_DBG("SNTP packet precision out of range: %d", pkt->precision);
122 return -EINVAL;
123 }
124
125 res->uptime_us = dest_ts_us;
126 res->seconds = (res->uptime_us + clk_offset_us) / USEC_PER_SEC;
127 res->fraction = (res->uptime_us + clk_offset_us) % USEC_PER_SEC;
128 res->uncertainty_us = (d_us + root_delay_us + precision_us) / 2 + root_dispersion_us;
129 #else
130 res->fraction = ntohl(pkt->tx_tm_f);
131 res->seconds = ntohl(pkt->tx_tm_s);
132 #endif
133 ts = ntohl(pkt->tx_tm_s);
134
135 /* Check if most significant bit is set */
136 if (ts & 0x80000000) {
137 /* UTC time is reckoned from 0h 0m 0s UTC
138 * on 1 January 1900.
139 */
140 if (ts >= OFFSET_1970_JAN_1) {
141 res->seconds -= OFFSET_1970_JAN_1;
142 } else {
143 return -EINVAL;
144 }
145 } else {
146 /* UTC time is reckoned from 6h 28m 16s UTC
147 * on 7 February 2036.
148 */
149 res->seconds += 0x100000000ULL - OFFSET_1970_JAN_1;
150 }
151
152 return 0;
153 }
154
sntp_init(struct sntp_ctx * ctx,struct sockaddr * addr,socklen_t addr_len)155 int sntp_init(struct sntp_ctx *ctx, struct sockaddr *addr, socklen_t addr_len)
156 {
157 int ret;
158
159 if (!ctx || !addr) {
160 return -EFAULT;
161 }
162
163 memset(ctx, 0, sizeof(struct sntp_ctx));
164
165 ctx->sock.fd = zsock_socket(addr->sa_family, SOCK_DGRAM, IPPROTO_UDP);
166 if (ctx->sock.fd < 0) {
167 NET_ERR("Failed to create UDP socket %d", errno);
168 return -errno;
169 }
170
171 ret = zsock_connect(ctx->sock.fd, addr, addr_len);
172 if (ret < 0) {
173 (void)zsock_close(ctx->sock.fd);
174 NET_ERR("Cannot connect to UDP remote : %d", errno);
175 return -errno;
176 }
177
178 ctx->sock.fds[ctx->sock.nfds].fd = ctx->sock.fd;
179 ctx->sock.fds[ctx->sock.nfds].events = ZSOCK_POLLIN;
180 ctx->sock.nfds++;
181
182 return 0;
183 }
184
sntp_query_send(struct sntp_ctx * ctx)185 static int sntp_query_send(struct sntp_ctx *ctx)
186 {
187 struct sntp_pkt tx_pkt = { 0 };
188 int64_t ts_us = 0;
189
190 /* prepare request pkt */
191 tx_pkt.li = 0;
192 tx_pkt.vn = SNTP_VERSION_NUMBER;
193 tx_pkt.mode = SNTP_MODE_CLIENT;
194 ts_us = k_ticks_to_us_near64(k_uptime_ticks());
195 ctx->expected_orig_ts.seconds = ts_us / USEC_PER_SEC;
196 ctx->expected_orig_ts.fraction = (ts_us % USEC_PER_SEC) * (UINT32_MAX / USEC_PER_SEC);
197 tx_pkt.tx_tm_s = htonl(ctx->expected_orig_ts.seconds);
198 tx_pkt.tx_tm_f = htonl(ctx->expected_orig_ts.fraction);
199
200 return zsock_send(ctx->sock.fd, (uint8_t *)&tx_pkt, sizeof(tx_pkt), 0);
201 }
202
sntp_query(struct sntp_ctx * ctx,uint32_t timeout,struct sntp_time * ts)203 int sntp_query(struct sntp_ctx *ctx, uint32_t timeout, struct sntp_time *ts)
204 {
205 int ret = 0;
206
207 if (!ctx || !ts) {
208 return -EFAULT;
209 }
210
211 ret = sntp_query_send(ctx);
212 if (ret < 0) {
213 NET_ERR("Failed to send over UDP socket %d", ret);
214 return ret;
215 }
216
217 return sntp_recv_response(ctx, timeout, ts);
218 }
219
sntp_recv_response(struct sntp_ctx * ctx,uint32_t timeout,struct sntp_time * ts)220 int sntp_recv_response(struct sntp_ctx *ctx, uint32_t timeout,
221 struct sntp_time *ts)
222 {
223 struct sntp_pkt buf = { 0 };
224 int status;
225 int rcvd;
226
227 status = zsock_poll(ctx->sock.fds, ctx->sock.nfds, timeout);
228 if (status < 0) {
229 NET_ERR("Error in poll:%d", errno);
230 return -errno;
231 }
232
233 if (status == 0) {
234 return -ETIMEDOUT;
235 }
236
237 rcvd = zsock_recv(ctx->sock.fd, (uint8_t *)&buf, sizeof(buf), 0);
238 if (rcvd < 0) {
239 return -errno;
240 }
241
242 if (rcvd != sizeof(struct sntp_pkt)) {
243 return -EMSGSIZE;
244 }
245
246 status = parse_response((uint8_t *)&buf, sizeof(buf),
247 &ctx->expected_orig_ts,
248 ts);
249 return status;
250 }
251
sntp_close(struct sntp_ctx * ctx)252 void sntp_close(struct sntp_ctx *ctx)
253 {
254 if (ctx) {
255 (void)zsock_close(ctx->sock.fd);
256 }
257 }
258
259 #ifdef CONFIG_NET_SOCKETS_SERVICE
260
sntp_init_async(struct sntp_ctx * ctx,struct sockaddr * addr,socklen_t addr_len,const struct net_socket_service_desc * service)261 int sntp_init_async(struct sntp_ctx *ctx, struct sockaddr *addr, socklen_t addr_len,
262 const struct net_socket_service_desc *service)
263 {
264 int ret;
265
266 /* Validate service pointer */
267 if (service == NULL) {
268 return -EFAULT;
269 }
270 /* Standard init */
271 ret = sntp_init(ctx, addr, addr_len);
272 if (ret < 0) {
273 return ret;
274 }
275 /* Attach socket to socket service */
276 ret = net_socket_service_register(service, ctx->sock.fds, ctx->sock.nfds, ctx);
277 if (ret < 0) {
278 NET_ERR("Failed to register socket %d", ret);
279 /* Cleanup init on register failure*/
280 sntp_close(ctx);
281 }
282 return ret;
283 }
284
sntp_send_async(struct sntp_ctx * ctx)285 int sntp_send_async(struct sntp_ctx *ctx)
286 {
287 int ret = 0;
288
289 if (!ctx) {
290 return -EFAULT;
291 }
292
293 ret = sntp_query_send(ctx);
294 if (ret < 0) {
295 NET_ERR("Failed to send over UDP socket %d", ret);
296 return ret;
297 }
298 return 0;
299 }
300
sntp_read_async(struct net_socket_service_event * event,struct sntp_time * ts)301 int sntp_read_async(struct net_socket_service_event *event, struct sntp_time *ts)
302 {
303 struct sntp_ctx *ctx = event->user_data;
304 struct sntp_pkt buf = {0};
305 int rcvd;
306
307 rcvd = zsock_recv(ctx->sock.fd, (uint8_t *)&buf, sizeof(buf), 0);
308 if (rcvd < 0) {
309 return -errno;
310 }
311
312 if (rcvd != sizeof(struct sntp_pkt)) {
313 return -EMSGSIZE;
314 }
315
316 return parse_response((uint8_t *)&buf, sizeof(buf), &ctx->expected_orig_ts, ts);
317 }
318
sntp_close_async(const struct net_socket_service_desc * service)319 void sntp_close_async(const struct net_socket_service_desc *service)
320 {
321 struct sntp_ctx *ctx = service->pev->user_data;
322 /* Detach socket from socket service */
323 net_socket_service_unregister(service);
324 /* CLose the socket */
325 if (ctx) {
326 (void)zsock_close(ctx->sock.fd);
327 }
328 }
329
330 #endif /* CONFIG_NET_SOCKETS_SERVICE */
331
