Step 3.3a: PTP servo, clock locks to the GM

- ptp_hw: ptp_hw_adj_freq() (ETH_MAC_ESP_CMD_ADJ_PTP_TIME, absolute ppb
  vs nominal, clamped +-500 ppm, retried while the addend update is
  busy) and ptp_hw_step() (read + write time).
- ptp_clock: first Sync after a GM is selected seeds the integral with
  the measured rate and steps the clock; then linuxptp-style PI (kp/ki
  from the Sync interval: 0.7/0.3 at 1 s). Re-step above 1 ms. Locked
  after 8 Syncs with |offset| < 1 us, unlocked after 3 above; GM change
  or loss unlocks and keeps the frequency (holdover).
- Verified vs ptp4l: stepped to GM time, locked after ~19 s; locked
  offset within +-510 ns (mostly < 300 ns), correction +39.9 ppm
  (crystal -39.8 ppm), path delay ~10.2 us.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

components/aes67_ptp/ptp_clock.c

@@ -1,5 +1,7 @@
 #include "ptp_clock.h"
 
+#include <math.h>
+#include <stdlib.h>
 #include <string.h>
 
 #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;
+    servo(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,
+    ESP_LOGI(TAG, "seq %u: offset %+lld ns, freq %+.3f ppm, path delay %lld ns%s", s.sync_seq, s.offset_ns,
-             s.rate * 1e6);
+             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

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;

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,