ESP32: SNTP for logging

ESP32/FvHNTP/FvHNTP.cpp

@@ -0,0 +1,139 @@
+// (C) 2024 by Folkert van Heusden
+// Released under MIT license
+
+#include <Arduino.h>
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+
+#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<uint64_t> ntp::get_unix_epoch_ms()
+{
+	std::unique_lock<std::mutex> 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<sockaddr *>(&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<std::mutex> 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);
+}

ESP32/FvHNTP/FvHNTP.h

@@ -0,0 +1,32 @@
+// (C) 2024 by Folkert van Heusden
+// Released under MIT license
+
+#pragma once
+
+#include <atomic>
+#include <mutex>
+#include <optional>
+#include <string>
+#include <thread>
+
+
+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<uint64_t> get_unix_epoch_ms();
+
+	void operator()();
+};

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_);
 	}
 }
 

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
 				};
 

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<char *>(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;
 }

log.h

@@ -6,6 +6,9 @@
 #include <string>
 
 #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)