diff --git a/components/aes67_ptp/ptp_clock.c b/components/aes67_ptp/ptp_clock.c index 30f3ee0..2640a40 100644 --- a/components/aes67_ptp/ptp_clock.c +++ b/components/aes67_ptp/ptp_clock.c @@ -1,5 +1,7 @@ #include "ptp_clock.h" +#include +#include #include #include "aes67_cfg.h" @@ -16,6 +18,11 @@ #define PTP_GENERAL_PORT 320 #define HDR_LEN 34 #define FLAG_TWO_STEP 0x0200 +#define STEP_NS 1000000 // re-step instead of slewing above 1 ms +#define LOCK_NS 1000 // |offset| below this counts as good +#define LOCK_GOOD 8 // consecutive good Syncs to lock +#define LOCK_BAD 3 // consecutive bad Syncs to unlock +#define MAX_DRIFT_PPB 500000.0 static const char *TAG = "ptp"; static const uint8_t PTP_MCAST_MAC[6] = { 0x01, 0x00, 0x5e, 0x00, 0x01, 0x81 }; @@ -53,8 +60,18 @@ static struct { // while the local clock is not yet syntonised. int64_t raw, prev_raw, prev_t2; double rate; + int8_t log_sync; // GM's Sync interval (from Sync logMessageInterval) + // Servo (PI, linuxptp-style gains) + bool stepped; // clock set to GM time since this GM was selected + double drift_ppb; // integral term + double freq_ppb; // correction currently applied (positive = faster) + int64_t offset_ns; + int good, bad; + bool locked; } s; +static void set_locked(bool locked); + /* ----- helpers ----- */ static uint16_t rd16(const uint8_t *p) { return (p[0] << 8) | p[1]; } @@ -146,11 +163,71 @@ static void on_announce(const uint8_t *b, int len, uint32_t src_ip) s.gm = m; s.sync_pending = s.dreq_pending = false; s.delay_ns = s.prev_t2 = 0; + s.stepped = false; + set_locked(false); s.log_dreq = 0; s.next_dreq_us = 0; } } +static void set_locked(bool locked) +{ + if (locked != s.locked) { + s.locked = locked; + if (locked) { + ESP_LOGI(TAG, "locked: offset %+lld ns, frequency %+.3f ppm, path delay %lld ns", + s.offset_ns, s.freq_ppb / 1000, s.delay_ns); + } else { + ESP_LOGW(TAG, "unlocked"); + } + } + if (!locked) { + s.good = 0; + } +} + +// offset = local - GM (ns) +static void servo(int64_t offset) +{ + s.offset_ns = offset; + if (!s.stepped || llabs(offset) > STEP_NS) { + if (!s.stepped) { + // Seed the integral with the measured rate error (measured with the current correction applied). + s.drift_ppb += s.rate * 1e9; + } + s.freq_ppb = -s.drift_ppb; + ptp_hw_adj_freq(s.freq_ppb); + esp_err_t err = ptp_hw_step(offset); + ESP_LOGI(TAG, "clock stepped by %+lld ns (%s), frequency %+.3f ppm", -offset, esp_err_to_name(err), + s.freq_ppb / 1000); + s.stepped = true; + s.prev_t2 = 0; // rate across the step is meaningless + s.bad = 0; + set_locked(false); + return; + } + // linuxptp PI gains for hardware timestamps, scaled by the Sync interval + double iv = ldexp(1.0, s.log_sync); + double kp = fmin(0.7 * pow(iv, -0.3), 0.7 / iv); + double ki = fmin(0.3 * pow(iv, 0.4), 0.3 / iv); + double ppb = kp * offset + s.drift_ppb; + s.drift_ppb = fmax(-MAX_DRIFT_PPB, fmin(MAX_DRIFT_PPB, s.drift_ppb + ki * offset)); + s.freq_ppb = -ppb; + ptp_hw_adj_freq(s.freq_ppb); + + if (llabs(offset) < LOCK_NS) { + s.bad = 0; + if (++s.good >= LOCK_GOOD) { + set_locked(true); + } + } else { + s.good = 0; + if (s.locked && ++s.bad >= LOCK_BAD) { + set_locked(false); + } + } +} + static void sync_complete(void) { s.sync_pending = false; @@ -164,9 +241,9 @@ static void sync_complete(void) return; } // offset = t2 - t1 - corrections - mean path delay - int64_t offset = s.raw - s.delay_ns; - ESP_LOGI(TAG, "seq %u: offset %+lld ns, path delay %lld ns, rate %+.3f ppm", s.sync_seq, offset, s.delay_ns, - s.rate * 1e6); + servo(s.raw - s.delay_ns); + ESP_LOGI(TAG, "seq %u: offset %+lld ns, freq %+.3f ppm, path delay %lld ns%s", s.sync_seq, s.offset_ns, + s.freq_ppb / 1000, s.delay_ns, s.locked ? ", locked" : ""); } static void on_sync(const uint8_t *b, int len) @@ -181,6 +258,7 @@ static void on_sync(const uint8_t *b, int len) return; } s.sync_seq = seq; + s.log_sync = (int8_t)b[33]; s.t2 = mac_ns(&t2); s.sync_corr = rd_corr_ns(b + 8); if (rd16(b + 6) & FLAG_TWO_STEP) { @@ -309,6 +387,8 @@ static void ptp_task(void *arg) ESP_LOGW(TAG, "TimeTransmitter lost (no Announce for %lld ms), listening", window / 1000); memset(&s.gm, 0, sizeof(s.gm)); s.delay_ns = s.prev_t2 = 0; + s.stepped = false; + set_locked(false); // frequency correction stays (holdover) } else if (now >= s.next_dreq_us && s.prev_t2) { send_delay_req(); // Delay_Req interval from the GM's Delay_Resp; randomised 0.5..1.5x diff --git a/components/aes67_ptp/ptp_hw.c b/components/aes67_ptp/ptp_hw.c index 22518c6..dd730b7 100644 --- a/components/aes67_ptp/ptp_hw.c +++ b/components/aes67_ptp/ptp_hw.c @@ -77,6 +77,38 @@ esp_err_t ptp_hw_get_time(eth_mac_time_t *t) return esp_eth_ioctl(s_eth, ETH_MAC_ESP_CMD_G_PTP_TIME, t); } +esp_err_t ptp_hw_adj_freq(double ppb) +{ + if (ppb > 500000) { + ppb = 500000; // +-500 ppm is far outside any sane crystal + } else if (ppb < -500000) { + ppb = -500000; + } + int32_t v = (int32_t)(ppb >= 0 ? ppb + 0.5 : ppb - 0.5); + esp_err_t err = ESP_ERR_INVALID_STATE; + // The previous addend update may still be in progress: retry briefly. + for (int i = 0; i < 10 && err == ESP_ERR_INVALID_STATE; i++) { + err = esp_eth_ioctl(s_eth, ETH_MAC_ESP_CMD_ADJ_PTP_TIME, &v); + } + return err; +} + +esp_err_t ptp_hw_step(int64_t offset_ns) +{ + eth_mac_time_t t; + esp_err_t err = ptp_hw_get_time(&t); + if (err != ESP_OK) { + return err; + } + int64_t ns = (int64_t)t.seconds * 1000000000LL + t.nanoseconds - offset_ns; + if (ns < 0) { + return ESP_ERR_INVALID_ARG; + } + t.seconds = ns / 1000000000LL; + t.nanoseconds = ns % 1000000000LL; + return esp_eth_ioctl(s_eth, ETH_MAC_ESP_CMD_S_PTP_TIME, &t); +} + static uint16_t ip_checksum(const uint8_t *h, size_t len) { uint32_t sum = 0; diff --git a/components/aes67_ptp/ptp_hw.h b/components/aes67_ptp/ptp_hw.h index bcb54df..5a72610 100644 --- a/components/aes67_ptp/ptp_hw.h +++ b/components/aes67_ptp/ptp_hw.h @@ -20,6 +20,10 @@ esp_err_t ptp_hw_init(esp_eth_handle_t eth, esp_netif_t *netif); // sequence ID and sourcePortIdentity (10 bytes). Each record is returned once. bool ptp_hw_rx_ts(uint8_t msg_type, uint16_t seq, const uint8_t *src_port_id, eth_mac_time_t *ts); esp_err_t ptp_hw_get_time(eth_mac_time_t *t); +// Frequency correction relative to the nominal rate, in ppb (positive = faster). +esp_err_t ptp_hw_adj_freq(double ppb); +// Step the clock back by offset_ns (offset = local - GM), via read + write. +esp_err_t ptp_hw_step(int64_t offset_ns); // Send a PTP event message as a raw Ethernet/IPv4/UDP frame (port 319 -> 319) and return its // hardware TX timestamp. dst_ip/dst_mac: 224.0.1.129 / 01:00:5e:00:01:81 or the GM unicast. esp_err_t ptp_hw_send_event(const uint8_t dst_mac[6], uint32_t dst_ip, uint8_t dscp,