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