MirrorSave
/
nodemcu-firmware
mirror of https://github.com/nodemcu/nodemcu-firmware.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
nodemcu-firmware/app/modules/sntppkt.c

404 lines
12 KiB
C
Raw Blame History

/*
* Copyright 2015 Dius Computing Pty Ltd. All rights reserved.
* Copyright 2020 Nathaniel Wesley Filardo. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* - Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* - Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the
* distribution.
* - Neither the name of the copyright holders nor the names of
* its contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
* THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
* OF THE POSSIBILITY OF SUCH DAMAGE.
*
* @author Johny Mattsson <jmattsson@dius.com.au>
* @author Nathaniel Wesley Filardo <nwfilardo@gmail.com>
*/
// Module for Simple Network Time Protocol (SNTP) packet processing;
// see lua_modules/sntp/sntp.lua for the user-friendly bits of this.
#include "module.h"
#include "lauxlib.h"
#include "os_type.h"
#include "osapi.h"
#include "lwip/udp.h"
#include <stdlib.h>
#include "user_modules.h"
#include "lwip/dns.h"
#define max(a,b) ((a < b) ? b : a)
#define NTP_PORT 123
#define NTP_ANYCAST_ADDR(dst) IP4_ADDR(dst, 224, 0, 1, 1)
#if 0
# define sntppkt_dbg(...) printf(__VA_ARGS__)
#else
# define sntppkt_dbg(...)
#endif
typedef struct
{
uint32_t sec;
uint32_t frac;
} ntp_timestamp_t;
static const uint32_t NTP_TO_UNIX_EPOCH = 2208988800ul;
typedef struct
{
uint8_t mode : 3;
uint8_t ver : 3;
uint8_t LI : 2;
uint8_t stratum;
uint8_t poll;
uint8_t precision;
uint32_t delta_r;
uint32_t epsilon_r;
uint32_t refid;
ntp_timestamp_t ref;
ntp_timestamp_t origin;
ntp_timestamp_t recv;
ntp_timestamp_t xmit;
} __attribute__((packed)) ntp_frame_t;
#define NTP_RESPONSE_METATABLE "sntppkt.resp"
typedef struct {
/* Copied from incoming packet */
uint32_t delta_r;
uint32_t epsilon_r;
uint8_t LI;
uint8_t stratum;
/* Computed as per RFC 5905; units are 2^(-32) seconds */
int64_t theta;
int64_t delta;
/* Local computation */
uint64_t t_rx;
uint32_t score;
} ntp_response_t;
static const uint32_t MICROSECONDS = 1000000;
static uint64_t
sntppkt_div1m(uint64_t n) {
uint64_t q1 = (n >> 5) + (n >> 10);
uint64_t q2 = (n >> 12) + (q1 >> 1);
uint64_t q3 = (q2 >> 11) - (q2 >> 23);
uint64_t q = n + q1 + q2 - q3;
q = q >> 20;
// Ignore the error term -- it is measured in pico seconds
return q;
}
static uint32_t
sntppkt_us_to_frac(uint64_t us) {
return sntppkt_div1m(us << 32);
}
static uint32_t
sntppkt_long_s(uint64_t time) {
return time >> 32;
}
static uint32_t
sntppkt_long_us(uint64_t time) {
return ((time & 0xFFFFFFFF) * MICROSECONDS) >> 32;
}
/*
* Convert sec/usec to a Lua string suitable for depositing into a SNTP packet
* buffer. This is a little gross, but it's not the worst thing a C
* programmer's ever done, I'm sure.
*/
static int
sntppkt_make_ts(lua_State *L) {
ntp_timestamp_t ts;
ts.sec = htonl(luaL_checkinteger(L, 1) + NTP_TO_UNIX_EPOCH) ;
uint32_t usec = luaL_checkinteger(L, 2) ;
ts.frac = htonl(sntppkt_us_to_frac(usec));
lua_pushlstring(L, (const char *)&ts, sizeof(ts));
return 1;
}
/*
* Convert ntp_timestamp_t to uint64_t
*/
static uint64_t
sntppkt_ts2uint64(ntp_timestamp_t ts) {
return (((uint64_t)(ts.sec)) << 32) + (uint64_t)(ts.frac);
}
/*
* Process a SNTP packet as contained in a Lua string, given a cookie timestamp
* and local clock second*usecond pair. Generates a ntp_response_t userdata
* for later processing or a string if the server is telling us to go away.
*
* :: string (packet)
* -> string (cookie)
* -> int (local clock, sec component)
* -> int (local clock, usec component)
* -> sntppkt.resp
*
*/
static int
sntppkt_proc_pkt(lua_State *L) {
size_t pkts_len;
ntp_frame_t pktb;
const char *pkts = lua_tolstring(L, 1, &pkts_len);
luaL_argcheck(L, pkts && pkts_len == sizeof(pktb), 1, "Bad packet");
// make sure we have an aligned copy to work from
memcpy (&pktb, pkts, sizeof(pktb));
uint32_t now_sec = luaL_checkinteger(L, 3);
uint32_t now_usec = luaL_checkinteger(L, 4);
size_t cookie_len;
ntp_timestamp_t *cookie = (ntp_timestamp_t*) lua_tolstring(L, 2, &cookie_len);
/* Bad *length* is clearly something bogus */
luaL_argcheck(L, cookie && cookie_len == sizeof(*cookie), 2, "Bad cookie");
/* But mismatching value might just be a packet caught in the crossfire */
if (memcmp((const char *)cookie, (const char *)&pktb.origin, sizeof (*cookie))) {
/* bad cookie; return nil */
return 0;
}
/* KOD? Do this test *after* checking the cookie */
if (pktb.LI == 3) {
lua_pushlstring(L, (const char *)&pktb.refid, 4);
return 1;
}
ntp_response_t *ntpr = lua_newuserdata(L, sizeof(ntp_response_t));
luaL_getmetatable(L, NTP_RESPONSE_METATABLE);
lua_setmetatable(L, -2);
ntpr->LI = pktb.LI;
ntpr->stratum = pktb.stratum;
// NTP Short Format: 16 bit seconds, 16 bit fraction
ntpr->delta_r = ntohl(pktb.delta_r);
ntpr->epsilon_r = ntohl(pktb.epsilon_r);
/* Heavy time lifting time */
// NTP Long Format: 32 bit seconds, 32 bit fraction
pktb.origin.sec = ntohl(pktb.origin.sec);
pktb.origin.frac = ntohl(pktb.origin.frac);
pktb.recv.sec = ntohl(pktb.recv.sec);
pktb.recv.frac = ntohl(pktb.recv.frac);
pktb.xmit.sec = ntohl(pktb.xmit.sec);
pktb.xmit.frac = ntohl(pktb.xmit.frac);
uint64_t ntp_origin = sntppkt_ts2uint64(pktb.origin); // We sent it
uint64_t ntp_recv = sntppkt_ts2uint64(pktb.recv); // They got it
uint64_t ntp_xmit = sntppkt_ts2uint64(pktb.xmit); // They replied
// When we got it back (our clock)
uint64_t ntp_dest = (((uint64_t) now_sec + NTP_TO_UNIX_EPOCH ) << 32)
+ sntppkt_us_to_frac(now_usec);
ntpr->t_rx = ntp_dest;
// | outgoing offset | | incoming offset |
ntpr->theta = (int64_t)(ntp_recv - ntp_origin) / 2 + (int64_t)(ntp_xmit - ntp_dest) / 2;
// | our clock delta | | their clock delta |
ntpr->delta = (int64_t)(ntp_dest - ntp_origin) / 2 + (int64_t)(ntp_xmit - ntp_recv) / 2;
/* Used by sntppkt_resp_pick; bias towards closer clocks */
ntpr->score = ntpr->delta_r * 2 + ntpr->delta;
sntppkt_dbg("SNTPPKT n_o=%llx n_r=%llx n_x=%llx n_d=%llx th=%llx d=%llx\n",
ntp_origin, ntp_recv, ntp_xmit, ntp_dest, ntpr->theta, ntpr->delta);
return 1;
}
/*
* Left-biased selector of a "preferred" NTP response. Note that preference
* is rather subjective!
*
* Returns true iff we'd prefer the second response to the first.
*
* :: sntppkt.resp -> sntppkt.resp -> boolean
*/
static int
sntppkt_resp_pick(lua_State *L) {
ntp_response_t *a = luaL_checkudata(L, 1, NTP_RESPONSE_METATABLE);
ntp_response_t *b = luaL_checkudata(L, 2, NTP_RESPONSE_METATABLE);
int biased = 0;
biased = lua_toboolean(L, 3);
/*
* If we're "biased", prefer the second structure if the score is less than
* 3/4ths of the score of the first. An unbiased comparison just uses the
* raw score values.
*/
if (biased) {
lua_pushboolean(L, a->score * 3 > b->score * 4);
} else {
lua_pushboolean(L, a->score > b->score );
}
return 1;
}
static void
field_from_integer(lua_State *L, const char * field_name, lua_Integer value) {
lua_pushinteger(L, value);
lua_setfield(L, -2, field_name);
}
/*
* Inflate a NTP response into a Lua table
*
* :: sntppkt.resp -> { }
*/
static int
sntppkt_resp_totable(lua_State *L) {
ntp_response_t *r = luaL_checkudata(L, 1, NTP_RESPONSE_METATABLE);
lua_createtable(L, 0, 6);
sntppkt_dbg("SNTPPKT READ th=%llx\n", r->theta);
/*
* The raw response ends up in here, too, so that we can pass the
* tabular view to user callbacks and still have the internal large integers
* for use in drift_compensate.
*/
lua_pushvalue(L, 1);
lua_setfield(L, -2, "raw");
field_from_integer(L, "theta_s", (int32_t)sntppkt_long_s (r->theta));
field_from_integer(L, "theta_us", sntppkt_long_us(r->theta));
field_from_integer(L, "delta", r->delta >> 16);
field_from_integer(L, "delta_r", r->delta_r);
field_from_integer(L, "epsilon_r", r->epsilon_r);
field_from_integer(L, "leapind", r->LI);
field_from_integer(L, "stratum", r->stratum);
field_from_integer(L, "rx_s" , sntppkt_long_s (r->t_rx) - NTP_TO_UNIX_EPOCH);
field_from_integer(L, "rx_us" , sntppkt_long_us(r->t_rx));
return 1;
}
/*
* Compute local RTC drift rate given a SNTP response, previous sample time,
* and error integral value (i.e., this is a PI controller). Returns new rate
* and integral value.
*
* Likely only sensible if resp->theta is sufficiently small (so we require
* less than a quarter of a second) and the inter-sample duration must, of
* course, be positive.
*
* There's nothing magic about the constants of the PI loop here; someone with
* a better understanding of control theory is welcome to suggest improvements.
*
* :: sntppkt.resp (most recent sample)
* -> sntppkt.resp (prior sample)
* -> int (integral)
* -> int (rate), int (integral)
*
*/
static int
sntppkt_resp_drift_compensate(lua_State *L) {
ntp_response_t *resp = luaL_checkudata(L, 1, NTP_RESPONSE_METATABLE);
int32_t tsec = sntppkt_long_s(resp->theta);
uint32_t tfrac = resp->theta & 0xFFFFFFFF;
if ((tsec != 0 && tsec != -1) ||
((tsec == 0) && (tfrac > 0x40000000UL)) ||
((tsec == -1) && (tfrac < 0xC0000000UL))) {
return luaL_error(L, "Large deviation");
}
int32_t dsec = sntppkt_long_s(resp->delta);
if (dsec != 0 && dsec != -1) {
return luaL_error(L, "Large estimated error");
}
ntp_response_t *prior_resp = luaL_checkudata(L, 2, NTP_RESPONSE_METATABLE);
int64_t isdur = resp->t_rx - prior_resp->t_rx;
if (isdur <= 0) {
return luaL_error(L, "Negative time base");
}
int32_t err_int = luaL_checkinteger(L, 3);
/* P: Compute drift over 2, in parts per 2^32 as expected by the RTC. */
int32_t drift2 = (resp->theta << 31) / isdur;
/* PI: rate is drift/4 + integral error */
int32_t newrate = (drift2 >> 1) + err_int;
/* I: Adjust integral by drift/32, with a little bit of wind-up protection */
if ((newrate > ~0x40000) && (newrate < 0x40000)) {
err_int += (drift2 + 0xF) >> 4;
}
sntppkt_dbg("SNTPPKT drift: isdur=%llx drift2=%lx -> newrate=%lx err_int=%lx\n",
isdur, drift2, newrate, err_int);
lua_pushinteger(L, newrate);
lua_pushinteger(L, err_int);
return 2;
}
LROT_BEGIN(sntppkt_resp, NULL, LROT_MASK_INDEX)
LROT_TABENTRY( __index, sntppkt_resp )
LROT_FUNCENTRY( pick, sntppkt_resp_pick )
LROT_FUNCENTRY( totable, sntppkt_resp_totable )
LROT_FUNCENTRY( drift_compensate, sntppkt_resp_drift_compensate )
LROT_END(sntppkt_resp, NULL, LROT_MASK_INDEX)
static int
sntppkt_init(lua_State *L)
{
luaL_rometatable(L, NTP_RESPONSE_METATABLE, LROT_TABLEREF(sntppkt_resp));
return 0;
}
// Module function map
LROT_BEGIN(sntppkt, NULL, 0)
LROT_FUNCENTRY( make_ts , sntppkt_make_ts )
LROT_FUNCENTRY( proc_pkt , sntppkt_proc_pkt )
LROT_END( sntppkt, NULL, 0 )
NODEMCU_MODULE(SNTPPKT, "sntppkt", sntppkt, sntppkt_init);
Reference in New Issue View Git Blame Copy Permalink