diff --git a/ESP32/FvHNTP/FvHNTP.cpp b/ESP32/FvHNTP/FvHNTP.cpp new file mode 100644 index 0000000..5cfca58 --- /dev/null +++ b/ESP32/FvHNTP/FvHNTP.cpp @@ -0,0 +1,139 @@ +// (C) 2024 by Folkert van Heusden +// Released under MIT license + +#include +#include +#include +#include + +#include "FvHNTP.h" + + +#define NTP_EPOCH uint64_t(86400ll * (365ll * 70ll + 17ll)) + +struct sntp_datagram +{ + uint8_t mode : 3; + uint8_t vn : 3; + uint8_t li : 2; + uint8_t stratum; + int8_t poll; + int8_t precision; + uint32_t root_delay; + uint32_t root_dispersion; + uint32_t reference_identifier; + uint32_t reference_timestamp_secs; + uint32_t reference_timestamp_fraq; + uint32_t originate_timestamp_secs; + uint32_t originate_timestamp_fraq; + uint32_t receive_timestamp_seqs; + uint32_t receive_timestamp_fraq; + uint32_t transmit_timestamp_secs; + uint32_t transmit_timestamp_fraq; +}; + +ntp::ntp(const std::string & server): server(server) +{ +} + +ntp::~ntp() +{ + stop = true; + + if (th) { + th->join(); + delete th; + } +} + +void ntp::begin() +{ + th = new std::thread(std::ref(*this)); +} + +std::optional ntp::get_unix_epoch_ms() +{ + std::unique_lock lck(lock); + + if (ntp_at_ts == 0) + return { }; + + auto now = millis(); + + return ntp_at_ts + now - millis_at_ts - NTP_EPOCH * 1000; +} + +uint64_t get_ms_from_ntp(const uint32_t high, const uint32_t low) +{ + return uint64_t(ntohl(high)) * 1000ll + ntohl(low) / (4295 * 1000); +} + +void ntp::operator()() +{ + int fd = socket(PF_INET, SOCK_DGRAM, 0); + sockaddr_in server_addr { }; + server_addr.sin_family = AF_INET; + server_addr.sin_addr.s_addr = inet_addr(server.c_str()); + server_addr.sin_port = htons(123); + + sntp_datagram packet_out; + + while(!stop) { + int s = 5; + + memset(&packet_out, 0x00, sizeof(packet_out)); + packet_out.vn = 4; + packet_out.mode = 3; + packet_out.stratum = 14; + packet_out.poll = 2; + + auto now = get_unix_epoch_ms(); + + if (now.has_value()) { + uint64_t sec = now.value() / 1000; + uint64_t usec = (now.value() % 1000) * 1000; + + packet_out.originate_timestamp_secs = htonl(sec + NTP_EPOCH); // T1 + packet_out.originate_timestamp_fraq = htonl(usec * 4295); + } + + if (sendto(fd, &packet_out, sizeof(packet_out), 0, reinterpret_cast(&server_addr), sizeof(server_addr)) == sizeof(packet_out)) { + sntp_datagram packet_in { 0 }; + + // TODO verify source address + if (recvfrom(fd, &packet_in, sizeof(packet_in), 0, nullptr, nullptr) == sizeof(packet_in)) { + // TODO verify version etc + uint32_t now = millis(); + auto t_t4 = get_unix_epoch_ms(); + + std::unique_lock lck(lock); + millis_at_ts = now; + + s = 60; + + if (t_t4.has_value()) { + int64_t t1 = get_ms_from_ntp(packet_out.originate_timestamp_secs, packet_out.originate_timestamp_fraq); + int64_t t2 = get_ms_from_ntp(packet_in.receive_timestamp_seqs, packet_in.receive_timestamp_fraq ); + int64_t t3 = get_ms_from_ntp(packet_in.transmit_timestamp_secs, packet_in.transmit_timestamp_fraq ); + int64_t t4 = t_t4.value() + NTP_EPOCH * 1000; + + auto offset = ((t2 - t1) + (t3 - t4)) / (2 * 1000); + + if (millis_at_ts > offset) + millis_at_ts -= offset; + else { + millis_at_ts = 0; + s = 4; + } + } + else { + ntp_at_ts = get_ms_from_ntp(packet_in.transmit_timestamp_secs, packet_in.transmit_timestamp_fraq); + } + } + } + + sleep(s); + } + + close(fd); +} diff --git a/ESP32/FvHNTP/FvHNTP.h b/ESP32/FvHNTP/FvHNTP.h new file mode 100644 index 0000000..64e3210 --- /dev/null +++ b/ESP32/FvHNTP/FvHNTP.h @@ -0,0 +1,32 @@ +// (C) 2024 by Folkert van Heusden +// Released under MIT license + +#pragma once + +#include +#include +#include +#include +#include + + +class ntp +{ +private: + std::atomic_bool stop { false }; + std::mutex lock; + std::string server; + std::thread *th { nullptr }; + uint32_t millis_at_ts { 0 }; + uint64_t ntp_at_ts { 0 }; // milliseconds! + +public: + ntp(const std::string & upstream_server); + virtual ~ntp(); + + void begin(); + + std::optional get_unix_epoch_ms(); + + void operator()(); +}; diff --git a/ESP32/main.ino b/ESP32/main.ino index 375ed13..f97ef9c 100644 --- a/ESP32/main.ino +++ b/ESP32/main.ino @@ -47,6 +47,7 @@ #include "tty.h" #include "utils.h" #include "version.h" +#include "FvHNTP/FvHNTP.h" constexpr const char SERIAL_CFG_FILE[] = "/serial.json"; @@ -70,9 +71,11 @@ SdFs SD; std::atomic_uint32_t stop_event { EVENT_NONE }; -std::atomic_bool *running { nullptr }; +std::atomic_bool *running { nullptr }; -bool trace_output { false }; +bool trace_output { false }; + +ntp *ntp_ { nullptr }; void console_thread_wrapper_panel(void *const c) { @@ -208,6 +211,12 @@ void start_network(console *const c) Serial.println(F("* Adding DC11")); dc11 *dc11_ = new dc11(1100, b); b->add_DC11(dc11_); + + Serial.println(F("* Starting (NTP-) clock")); + ntp_ = new ntp("188.212.113.203"); + ntp_->begin(); + + set_clock_reference(ntp_); } } diff --git a/debugger.cpp b/debugger.cpp index 6d88cd6..cf57419 100644 --- a/debugger.cpp +++ b/debugger.cpp @@ -982,6 +982,11 @@ void debugger(console *const cnsl, bus *const b, std::atomic_uint32_t *const sto continue; } + else if (parts[0] == "log") { + DOLOG(info, true, cmd.c_str()); + + continue; + } else if (parts[0] == "lt") { if (parts.size() == 2) tm11_load_tape(cnsl, b, parts[1]); @@ -1067,6 +1072,7 @@ void debugger(console *const cnsl, bus *const b, std::atomic_uint32_t *const sto #endif #endif "cfgdisk - configure disk", + "log - log a message to the logfile", nullptr }; diff --git a/log.cpp b/log.cpp index bffb442..a6445ac 100644 --- a/log.cpp +++ b/log.cpp @@ -37,6 +37,9 @@ static bool l_timestamp = true; static thread_local int log_buffer_size = 128; static thread_local char *log_buffer = reinterpret_cast(malloc(log_buffer_size)); bool log_trace_enabled = false; +#if defined(ESP32) +static ntp *ntp_clock = nullptr; +#endif #if defined(ESP32) int gettid() @@ -45,6 +48,13 @@ int gettid() } #endif +#if defined(ESP32) +void set_clock_reference(ntp *const ntp_) +{ + ntp_clock = ntp_; +} +#endif + void settrace(const bool on) { log_trace_enabled = on; @@ -153,8 +163,18 @@ void dolog(const log_level_t ll, const char *fmt, ...) } if (l_timestamp) { - uint64_t now = get_us(); - time_t t_now = now / 1000000; +#if defined(ESP32) + uint64_t now = 0; + + if (ntp_clock) { + auto temp = ntp_clock->get_unix_epoch_ms(); + if (temp.has_value()) + now = temp.value() * 1000; + } +#else + uint64_t now = get_us(); +#endif + time_t t_now = now / 1000000; tm tm { 0 }; #if defined(_WIN32) @@ -212,7 +232,9 @@ log_level_t parse_ll(const std::string & str) if (str == "none") return none; +#if !defined(ESP32) error_exit(false, "Log level \"%s\" not understood", str.c_str()); +#endif return debug; } diff --git a/log.h b/log.h index b3f0fe0..7b2ca12 100644 --- a/log.h +++ b/log.h @@ -6,6 +6,9 @@ #include #include "config.h" +#if defined(ESP32) +#include "FvHNTP/FvHNTP.h" +#endif typedef enum { ll_emerg = 0, ll_alert, ll_critical, ll_error, warning, notice, info, debug, none } log_level_t; // TODO ll_ prefix @@ -20,6 +23,9 @@ void closelog(); void dolog(const log_level_t ll, const char *fmt, ...); void settrace(const bool on); bool gettrace(); +#if defined(ESP32) +void set_clock_reference(ntp *const ntp_); +#endif #ifdef TURBO #define DOLOG(ll, always, fmt, ...) do { } while(0)