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
14 #define SNTP_LI_MAX 3
15 #define SNTP_VERSION_NUMBER 3
16 #define SNTP_MODE_CLIENT 3
17 #define SNTP_MODE_SERVER 4
18 #define SNTP_STRATUM_KOD 0 /* kiss-o'-death */
19 #define OFFSET_1970_JAN_1 2208988800
20
sntp_pkt_dump(struct sntp_pkt * pkt)21 static void sntp_pkt_dump(struct sntp_pkt *pkt)
22 {
23 if (!pkt) {
24 return;
25 }
26
27 NET_DBG("li %x", SNTP_GET_LI(pkt->lvm));
28 NET_DBG("vn %x", SNTP_GET_VN(pkt->lvm));
29 NET_DBG("mode %x", SNTP_GET_MODE(pkt->lvm));
30 NET_DBG("stratum: %x", pkt->stratum);
31 NET_DBG("poll: %x", pkt->poll);
32 NET_DBG("precision: %x", pkt->precision);
33 NET_DBG("root_delay: %x", pkt->root_delay);
34 NET_DBG("root_dispersion: %x", pkt->root_dispersion);
35 NET_DBG("ref_id: %x", pkt->ref_id);
36 NET_DBG("ref_tm_s: %x", pkt->ref_tm_s);
37 NET_DBG("ref_tm_f: %x", pkt->ref_tm_f);
38 NET_DBG("orig_tm_s: %x", pkt->orig_tm_s);
39 NET_DBG("orig_tm_f: %x", pkt->orig_tm_f);
40 NET_DBG("rx_tm_s: %x", pkt->rx_tm_s);
41 NET_DBG("rx_tm_f: %x", pkt->rx_tm_f);
42 NET_DBG("tx_tm_s: %x", pkt->tx_tm_s);
43 NET_DBG("tx_tm_f: %x", pkt->tx_tm_f);
44 }
45
parse_response(uint8_t * data,uint16_t len,uint32_t orig_ts,struct sntp_time * time)46 static int32_t parse_response(uint8_t *data, uint16_t len, uint32_t orig_ts,
47 struct sntp_time *time)
48 {
49 struct sntp_pkt *pkt = (struct sntp_pkt *)data;
50 uint32_t ts;
51
52 sntp_pkt_dump(pkt);
53
54 if (ntohl(pkt->orig_tm_s) != orig_ts) {
55 NET_DBG("Mismatch originate timestamp: %d, expect: %d",
56 ntohl(pkt->orig_tm_s), orig_ts);
57 return -EINVAL;
58 }
59
60 if (SNTP_GET_MODE(pkt->lvm) != SNTP_MODE_SERVER) {
61 /* For unicast and manycast, server should return 4.
62 * For broadcast (which is not supported now), server should
63 * return 5.
64 */
65 NET_DBG("Unexpected mode: %d", SNTP_GET_MODE(pkt->lvm));
66 return -EINVAL;
67 }
68
69 if (pkt->stratum == SNTP_STRATUM_KOD) {
70 NET_DBG("kiss-o'-death stratum");
71 return -EBUSY;
72 }
73
74 if (ntohl(pkt->tx_tm_s) == 0 && ntohl(pkt->tx_tm_f) == 0) {
75 NET_DBG("zero transmit timestamp");
76 return -EINVAL;
77 }
78
79 time->fraction = ntohl(pkt->tx_tm_f);
80 ts = ntohl(pkt->tx_tm_s);
81
82 /* Check if most significant bit is set */
83 if (ts & 0x80000000) {
84 /* UTC time is reckoned from 0h 0m 0s UTC
85 * on 1 January 1900.
86 */
87 if (ts >= OFFSET_1970_JAN_1) {
88 time->seconds = ts - OFFSET_1970_JAN_1;
89 } else {
90 return -EINVAL;
91 }
92 } else {
93 /* UTC time is reckoned from 6h 28m 16s UTC
94 * on 7 February 2036.
95 */
96 time->seconds = ts + 0x100000000ULL - OFFSET_1970_JAN_1;
97 }
98
99 return 0;
100 }
101
sntp_recv_response(struct sntp_ctx * sntp,uint32_t timeout,struct sntp_time * time)102 static int sntp_recv_response(struct sntp_ctx *sntp, uint32_t timeout,
103 struct sntp_time *time)
104 {
105 struct sntp_pkt buf = { 0 };
106 int status;
107 int rcvd;
108
109 status = zsock_poll(sntp->sock.fds, sntp->sock.nfds, timeout);
110 if (status < 0) {
111 NET_ERR("Error in poll:%d", errno);
112 return -errno;
113 }
114
115 if (status == 0) {
116 return -ETIMEDOUT;
117 }
118
119 rcvd = zsock_recv(sntp->sock.fd, (uint8_t *)&buf, sizeof(buf), 0);
120 if (rcvd < 0) {
121 return -errno;
122 }
123
124 if (rcvd != sizeof(struct sntp_pkt)) {
125 return -EMSGSIZE;
126 }
127
128 status = parse_response((uint8_t *)&buf, sizeof(buf),
129 sntp->expected_orig_ts,
130 time);
131 return status;
132 }
133
get_uptime_in_sec(void)134 static uint32_t get_uptime_in_sec(void)
135 {
136 uint64_t time;
137
138 time = k_uptime_get_32();
139
140 return time / MSEC_PER_SEC;
141 }
142
sntp_init(struct sntp_ctx * ctx,struct sockaddr * addr,socklen_t addr_len)143 int sntp_init(struct sntp_ctx *ctx, struct sockaddr *addr, socklen_t addr_len)
144 {
145 int ret;
146
147 if (!ctx || !addr) {
148 return -EFAULT;
149 }
150
151 memset(ctx, 0, sizeof(struct sntp_ctx));
152
153 ctx->sock.fd = zsock_socket(addr->sa_family, SOCK_DGRAM, IPPROTO_UDP);
154 if (ctx->sock.fd < 0) {
155 NET_ERR("Failed to create UDP socket %d", errno);
156 return -errno;
157 }
158
159 ret = zsock_connect(ctx->sock.fd, addr, addr_len);
160 if (ret < 0) {
161 (void)zsock_close(ctx->sock.fd);
162 NET_ERR("Cannot connect to UDP remote : %d", errno);
163 return -errno;
164 }
165
166 ctx->sock.fds[ctx->sock.nfds].fd = ctx->sock.fd;
167 ctx->sock.fds[ctx->sock.nfds].events = ZSOCK_POLLIN;
168 ctx->sock.nfds++;
169
170 return 0;
171 }
172
sntp_query(struct sntp_ctx * ctx,uint32_t timeout,struct sntp_time * time)173 int sntp_query(struct sntp_ctx *ctx, uint32_t timeout, struct sntp_time *time)
174 {
175 struct sntp_pkt tx_pkt = { 0 };
176 int ret = 0;
177
178 if (!ctx || !time) {
179 return -EFAULT;
180 }
181
182 /* prepare request pkt */
183 SNTP_SET_LI(tx_pkt.lvm, 0);
184 SNTP_SET_VN(tx_pkt.lvm, SNTP_VERSION_NUMBER);
185 SNTP_SET_MODE(tx_pkt.lvm, SNTP_MODE_CLIENT);
186 ctx->expected_orig_ts = get_uptime_in_sec() + OFFSET_1970_JAN_1;
187 tx_pkt.tx_tm_s = htonl(ctx->expected_orig_ts);
188
189 ret = zsock_send(ctx->sock.fd, (uint8_t *)&tx_pkt, sizeof(tx_pkt), 0);
190 if (ret < 0) {
191 NET_ERR("Failed to send over UDP socket %d", ret);
192 return ret;
193 }
194
195 return sntp_recv_response(ctx, timeout, time);
196 }
197
sntp_close(struct sntp_ctx * ctx)198 void sntp_close(struct sntp_ctx *ctx)
199 {
200 if (ctx) {
201 (void)zsock_close(ctx->sock.fd);
202 }
203 }
204
