From 08b83a069c481c412ba0d72b8cf56831ddbd9869 Mon Sep 17 00:00:00 2001 From: Ben Nicholson Date: Fri, 25 Sep 2026 07:42:43 +1000 Subject: [PATCH] Step 6.1: PTP TimeTransmitter with BMCA (multicast) - Own dataset from config: slave -> class 255, never TimeTransmitter; auto/master -> class 248 with ptp.priority1/2; accuracy 0xFE, variance 0xFFFF, timeSource 0xA0, clockIdentity EUI-64 from MAC. - BMCA: a foreign TimeTransmitter is followed only if its Announce beats our dataset (always for slave-only); if the followed one turns worse we take over; a better one makes us step down. LISTENING -> MASTER after announceReceiptTimeout x our announce interval. - MASTER: Announce (PTP timescale flag) every 2^log_announce, two-step Sync every 2^log_sync (raw frame, HW TX time in Follow_Up), Delay_Resp for each Delay_Req with its HW RX time and logMessageInterval = log_delay_req. Frequency correction kept (holdover). - ptp config applies live (role, priorities, intervals, domain, DSCP). - status.ptp: state MASTER, gm_id = own, TX Sync/Announce intervals, Delay_Req/Resp counters; locked is true as master so AES67 TX keeps running; SDP ts-refclk and SAP follow. - Verified: with ptp4l GM p1 128 the board (auto, p1 250) stays SLAVE; ptp4l stopped -> board MASTER; ptp4l -s --priority1 255 locks to it (s2) in ~4 s, offset within +-320 ns, path delay 10.3 us. Known: Sync send times jitter ~10 ms (FreeRTOS tick); accuracy is not affected (two-step Follow_Up carries the exact HW time). Co-Authored-By: Claude Opus 5.5 --- components/aes67_ptp/aes67_ptp.c | 7 +- components/aes67_ptp/ptp_clock.c | 298 +++++++++++++++++++++++++++++-- components/aes67_ptp/ptp_clock.h | 4 +- 3 files changed, 293 insertions(+), 16 deletions(-) diff --git a/components/aes67_ptp/aes67_ptp.c b/components/aes67_ptp/aes67_ptp.c index 76051e0..6ff4497 100644 --- a/components/aes67_ptp/aes67_ptp.c +++ b/components/aes67_ptp/aes67_ptp.c @@ -27,9 +27,14 @@ static bool ptp_validate(const cJSON *g, char *err, size_t n) cfg_check_int(g, "dscp", 0, 63, err, n); } +static void ptp_apply(const cJSON *g) +{ + ptp_clock_reconfig(); +} + esp_err_t aes67_ptp_init(void) { - return cfg_register("ptp", PTP_DEFAULTS, ptp_validate, NULL); + return cfg_register("ptp", PTP_DEFAULTS, ptp_validate, ptp_apply); } esp_err_t aes67_ptp_start(esp_eth_handle_t eth) diff --git a/components/aes67_ptp/ptp_clock.c b/components/aes67_ptp/ptp_clock.c index d8c1372..da18800 100644 --- a/components/aes67_ptp/ptp_clock.c +++ b/components/aes67_ptp/ptp_clock.c @@ -26,6 +26,9 @@ #define MAX_DRIFT_PPB 500000.0 #define WINDOW 64 // samples for interval/delay statistics #define SUMMARY_US (60 * 1000000LL) +#define FLAG_PTP_TIMESCALE 0x0008 // flagField octet 1 bit 3 +#define UTC_OFFSET 37 // TAI - UTC (s), announced as information only +#define TIME_SOURCE_OSC 0xA0 // internal oscillator static const char *TAG = "ptp"; static const uint8_t PTP_MCAST_MAC[6] = { 0x01, 0x00, 0x5e, 0x00, 0x01, 0x81 }; @@ -54,7 +57,16 @@ static struct { int ev, gen; uint8_t domain, dscp, timeout; uint8_t port_id[10]; // our clockId (EUI-64 from MAC) + port 1 - master_t gm; + master_t gm; // foreign TimeTransmitter we follow (valid = TimeReceiver) + master_t own; // our own dataset for BMCA + bool slave_only; // role "slave": never TimeTransmitter + bool master; // we are the TimeTransmitter + int64_t listen_since_us; // start of LISTENING (for the announce receipt timeout) + volatile bool reconfig; + int8_t cfg_log_sync, cfg_log_announce, cfg_log_dreq; + // TimeTransmitter + uint16_t tx_sync_seq, tx_announce_seq; + int64_t next_sync_us, next_announce_us, prev_t1_tx; // Sync / Follow_Up uint16_t sync_seq; bool sync_pending; @@ -80,7 +92,7 @@ static struct { bool locked; // Statistics for status.ptp (guarded by lock) SemaphoreHandle_t lock; - window_t sync_iv, announce_iv, delays; + window_t sync_iv, announce_iv, delays; // RX intervals as receiver, TX intervals as transmitter int64_t last_announce_us; uint32_t delay_req, delay_resp; uint8_t own_class; @@ -122,6 +134,7 @@ static void win_stats(const window_t *w, double *mean, double *min, double *max, #define LOCKED(stmt) do { xSemaphoreTake(s.lock, portMAX_DELAY); stmt; xSemaphoreGive(s.lock); } while (0) static void set_locked(bool locked); +static void become_master(void); /* ----- helpers ----- */ @@ -205,6 +218,19 @@ static void on_announce(const uint8_t *b, int len, uint32_t src_ip) if (memcmp(m.port_id, s.port_id, 10) == 0) { return; // our own } + // BMCA: follow a foreign TimeTransmitter only if it beats our own dataset (always when slave-only). + bool better = s.slave_only || compare(&m, &s.own) < 0; + if (!better) { + if (from_gm(b)) { + ESP_LOGI(TAG, "TimeTransmitter's dataset is now worse than ours"); + become_master(); + } + return; // as TimeTransmitter a worse one should yield; nothing to do + } + if (s.master) { + s.master = false; + ESP_LOGI(TAG, "better TimeTransmitter seen: leaving TimeTransmitter state"); + } if (from_gm(b)) { LOCKED({ if (s.last_announce_us) { @@ -409,6 +435,151 @@ static void on_delay_resp(const uint8_t *b, int len) }); } +/* ----- TimeTransmitter ----- */ + +static int64_t log_us(int8_t log) +{ + return log >= 0 ? 1000000LL << log : 1000000LL >> -log; +} + +static void wr_ts(uint8_t *p, int64_t ns) +{ + uint64_t sec = ns / 1000000000LL; + uint32_t n = ns % 1000000000LL; + p[0] = sec >> 40; p[1] = sec >> 32; p[2] = sec >> 24; p[3] = sec >> 16; p[4] = sec >> 8; p[5] = sec; + p[6] = n >> 24; p[7] = n >> 16; p[8] = n >> 8; p[9] = n; +} + +static void hdr(uint8_t *m, uint8_t type, uint16_t len, uint16_t flags, uint16_t seq, uint8_t control, int8_t log) +{ + memset(m, 0, len); + m[0] = type; + m[1] = 2; + m[2] = len >> 8; + m[3] = len & 0xff; + m[4] = s.domain; + m[6] = flags >> 8; + m[7] = flags & 0xff; + memcpy(m + 20, s.port_id, 10); + m[30] = seq >> 8; + m[31] = seq & 0xff; + m[32] = control; + m[33] = (uint8_t)log; +} + +// General messages (Announce, Follow_Up, Delay_Resp) go through the lwIP socket on port 320. +static void send_general(const uint8_t *m, size_t len, uint32_t dst_ip) +{ + struct sockaddr_in dst = { .sin_family = AF_INET, .sin_port = htons(PTP_GENERAL_PORT), .sin_addr.s_addr = dst_ip }; + if (sendto(s.gen, m, len, 0, (struct sockaddr *)&dst, sizeof(dst)) < 0) { + ESP_LOGW(TAG, "send type %u failed (errno %d)", m[0] & 0x0f, errno); + } +} + +static uint32_t mcast_ip(void) +{ + struct in_addr a; + inet_aton(PTP_MCAST, &a); + return a.s_addr; +} + +static void send_announce(void) +{ + uint8_t m[64]; + hdr(m, PTP_MSG_ANNOUNCE, sizeof(m), FLAG_PTP_TIMESCALE, s.tx_announce_seq++, 5, s.cfg_log_announce); + m[44] = UTC_OFFSET >> 8; + m[45] = UTC_OFFSET & 0xff; + m[47] = s.own.p1; + m[48] = s.own.cls; + m[49] = s.own.acc; + m[50] = s.own.var >> 8; + m[51] = s.own.var & 0xff; + m[52] = s.own.p2; + memcpy(m + 53, s.own.gm_id, 8); + m[63] = TIME_SOURCE_OSC; + send_general(m, sizeof(m), mcast_ip()); + int64_t now = esp_timer_get_time(); + LOCKED({ + if (s.last_announce_us) { + win_add(&s.announce_iv, now - s.last_announce_us); + } + s.last_announce_us = now; + }); +} + +// Two-step: Sync as a raw frame for its hardware TX timestamp, then Follow_Up with that time. +static void send_sync(void) +{ + uint8_t m[44]; + uint16_t seq = s.tx_sync_seq++; + hdr(m, PTP_MSG_SYNC, sizeof(m), FLAG_TWO_STEP, seq, 0, s.cfg_log_sync); + eth_mac_time_t t1; + esp_err_t err = ptp_hw_send_event(PTP_MCAST_MAC, mcast_ip(), s.dscp, m, sizeof(m), &t1); + if (err != ESP_OK) { + ESP_LOGW(TAG, "Sync %u: %s", seq, esp_err_to_name(err)); + return; + } + hdr(m, PTP_MSG_FOLLOW_UP, sizeof(m), 0, seq, 2, s.cfg_log_sync); + int64_t t1n = mac_ns(&t1); + wr_ts(m + 34, t1n); + send_general(m, sizeof(m), mcast_ip()); + LOCKED({ + if (s.prev_t1_tx) { + win_add(&s.sync_iv, t1n - s.prev_t1_tx); + } + s.prev_t1_tx = t1n; + }); +} + +static void on_delay_req(const uint8_t *b, int len, uint32_t src_ip) +{ + if (!s.master || len < 44) { + return; + } + eth_mac_time_t t4; + uint16_t seq = rd16(b + 30); + if (!ptp_hw_rx_ts(PTP_MSG_DELAY_REQ, seq, b + 20, &t4)) { + ESP_LOGW(TAG, "Delay_Req %u: no HW RX timestamp", seq); + return; + } + uint8_t m[54]; + hdr(m, PTP_MSG_DELAY_RESP, sizeof(m), 0, seq, 3, s.cfg_log_dreq); + memcpy(m + 8, b + 8, 8); // correctionField of the Delay_Req + wr_ts(m + 34, mac_ns(&t4)); + memcpy(m + 44, b + 20, 10); // requestingPortIdentity + send_general(m, sizeof(m), mcast_ip()); + LOCKED({ + s.delay_req++; + s.delay_resp++; + }); +} + +static void become_master(void) +{ + char id[24]; + fmt_id(id, s.own.gm_id); + ESP_LOGI(TAG, "no better TimeTransmitter: this device is TimeTransmitter %s (p1 %u class %u p2 %u)", + id, s.own.p1, s.own.cls, s.own.p2); + LOCKED({ + memset(&s.gm, 0, sizeof(s.gm)); + s.master = true; + s.delay_ns = s.prev_t2 = 0; + s.stepped = false; + set_locked(false); // servo state; the frequency correction stays (holdover) + s.last_announce_us = s.prev_t1_tx = 0; + win_clear(&s.announce_iv); + win_clear(&s.sync_iv); + win_clear(&s.delays); + }); + s.sync_pending = s.dreq_pending = false; + s.next_sync_us = s.next_announce_us = esp_timer_get_time(); +} + +static void enter_listening(void) +{ + s.listen_since_us = esp_timer_get_time(); +} + /* ----- task ----- */ static void load_config(void) @@ -417,11 +588,28 @@ static void load_config(void) s.domain = cJSON_GetObjectItem(c, "domain")->valueint; s.dscp = cJSON_GetObjectItem(c, "dscp")->valueint; s.timeout = cJSON_GetObjectItem(c, "announce_timeout")->valueint; - // clockClass per role: slave-only 255, auto/master 248 (TimeTransmitter itself: step 6) - s.own_class = strcmp(cJSON_GetObjectItem(c, "role")->valuestring, "slave") == 0 ? 255 : 248; + s.cfg_log_sync = cJSON_GetObjectItem(c, "log_sync")->valueint; + s.cfg_log_announce = cJSON_GetObjectItem(c, "log_announce")->valueint; + s.cfg_log_dreq = cJSON_GetObjectItem(c, "log_delay_req")->valueint; + // Roles (docs): slave = clockClass 255, never transmits; auto/master = 248 with the configured + // priorities (auto 250/250 by default, master e.g. p1 100). + s.slave_only = strcmp(cJSON_GetObjectItem(c, "role")->valuestring, "slave") == 0; + s.own_class = s.slave_only ? 255 : 248; + s.own = (master_t){ + .valid = true, .p1 = cJSON_GetObjectItem(c, "priority1")->valueint, .cls = s.own_class, + .acc = 0xFE, .var = 0xFFFF, .p2 = cJSON_GetObjectItem(c, "priority2")->valueint, .steps = 0, + .log_announce = s.cfg_log_announce, + }; + memcpy(s.own.gm_id, s.port_id, 8); + memcpy(s.own.port_id, s.port_id, 10); cJSON_Delete(c); } +void ptp_clock_reconfig(void) +{ + s.reconfig = true; +} + static void ptp_task(void *arg) { esp_netif_ip_info_t ip = { 0 }; @@ -434,9 +622,18 @@ static void ptp_task(void *arg) if (s.ev < 0 || s.gen < 0) { vTaskDelete(NULL); } + // General messages we send as TimeTransmitter: subnet only, PTP DSCP, no loopback. + uint8_t ttl = 1, loop = 0; + int tos = s.dscp << 2; + setsockopt(s.gen, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(ttl)); + setsockopt(s.gen, IPPROTO_IP, IP_MULTICAST_LOOP, &loop, sizeof(loop)); + setsockopt(s.gen, IPPROTO_IP, IP_MULTICAST_IF, &ifaddr, sizeof(ifaddr)); + setsockopt(s.gen, IPPROTO_IP, IP_TOS, &tos, sizeof(tos)); + enter_listening(); char id[24]; fmt_id(id, s.port_id); - ESP_LOGI(TAG, "TimeReceiver on " IPSTR ", domain %u, clock %s, listening", IP2STR(&ip.ip), s.domain, id); + ESP_LOGI(TAG, "PTP on " IPSTR ", domain %u, clock %s, role %s, listening", IP2STR(&ip.ip), s.domain, id, + s.slave_only ? "TimeReceiver only" : "auto/TimeTransmitter capable"); uint8_t b[128]; while (1) { @@ -444,7 +641,14 @@ static void ptp_task(void *arg) FD_ZERO(&fds); FD_SET(s.ev, &fds); FD_SET(s.gen, &fds); - struct timeval tv = { .tv_sec = 0, .tv_usec = 100000 }; + // Sleep until the next message is due as TimeTransmitter, at most 100 ms. + int64_t wait = 100000; + if (s.master) { + int64_t now = esp_timer_get_time(); + int64_t due = s.next_sync_us < s.next_announce_us ? s.next_sync_us : s.next_announce_us; + wait = due - now < 0 ? 0 : due - now < wait ? due - now : wait; + } + struct timeval tv = { .tv_sec = 0, .tv_usec = wait }; if (select((s.ev > s.gen ? s.ev : s.gen) + 1, &fds, NULL, NULL, &tv) > 0) { for (int k = 0; k < 2; k++) { int fd = k ? s.gen : s.ev; @@ -462,13 +666,55 @@ static void ptp_task(void *arg) case PTP_MSG_SYNC: on_sync(b, len); break; case PTP_MSG_FOLLOW_UP: on_follow_up(b, len); break; case PTP_MSG_DELAY_RESP: on_delay_resp(b, len); break; + case PTP_MSG_DELAY_REQ: on_delay_req(b, len, src.sin_addr.s_addr); break; default: break; } } } + if (s.reconfig) { + s.reconfig = false; + bool was_slave_only = s.slave_only; + load_config(); + int tos2 = s.dscp << 2; + setsockopt(s.gen, IPPROTO_IP, IP_TOS, &tos2, sizeof(tos2)); + ESP_LOGI(TAG, "config applied: role %s, p1 %u p2 %u, domain %u", s.slave_only ? "slave" : "auto/master", + s.own.p1, s.own.p2, s.domain); + // Re-run the decision: a foreign GM worse than our new dataset is dropped; as + // TimeTransmitter with role slave we stop. + if (s.master && s.slave_only) { + s.master = false; + ESP_LOGI(TAG, "role slave: leaving TimeTransmitter state"); + enter_listening(); + } else if (s.gm.valid && !s.slave_only && compare(&s.gm, &s.own) > 0) { + become_master(); + } else if (!s.gm.valid && was_slave_only != s.slave_only) { + enter_listening(); + } + } + int64_t now = esp_timer_get_time(); - if (s.gm.valid) { + if (s.master) { + if (now >= s.next_announce_us) { + send_announce(); + s.next_announce_us += log_us(s.cfg_log_announce); + if (s.next_announce_us < now) { + s.next_announce_us = now + log_us(s.cfg_log_announce); + } + } + if (now >= s.next_sync_us) { + send_sync(); + s.next_sync_us += log_us(s.cfg_log_sync); + if (s.next_sync_us < now) { + s.next_sync_us = now + log_us(s.cfg_log_sync); + } + } + } else if (!s.gm.valid) { + // LISTENING: no better TimeTransmitter within announceReceiptTimeout -> become one + if (!s.slave_only && now - s.listen_since_us > (int64_t)s.timeout * log_us(s.cfg_log_announce)) { + become_master(); + } + } else if (s.gm.valid) { // announceReceiptTimeout x the GM's announce interval int64_t window = (int64_t)s.timeout * (s.gm.log_announce >= 0 ? 1000000LL << s.gm.log_announce : 1000000LL >> -s.gm.log_announce); @@ -480,6 +726,7 @@ static void ptp_task(void *arg) s.stepped = false; set_locked(false); // frequency correction stays (holdover) }); + enter_listening(); } 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 @@ -493,17 +740,18 @@ static void ptp_task(void *arg) bool ptp_clock_gm_id(char out[24]) { xSemaphoreTake(s.lock, portMAX_DELAY); - bool valid = s.gm.valid; + bool valid = s.gm.valid || s.master; if (valid) { - fmt_id(out, s.gm.gm_id); + fmt_id(out, s.master ? s.own.gm_id : s.gm.gm_id); } xSemaphoreGive(s.lock); return valid; } +// Media may be timed from our clock: locked to a GM, or we are the GM. bool ptp_clock_locked(void) { - return s.locked; + return s.locked || s.master; } void ptp_clock_status(cJSON *st) @@ -513,9 +761,9 @@ void ptp_clock_status(cJSON *st) double mean, min, max, sd; xSemaphoreTake(s.lock, portMAX_DELAY); - const char *state = !s.gm.valid ? "LISTENING" : s.locked ? "SLAVE" : "UNCALIBRATED"; + const char *state = s.master ? "MASTER" : !s.gm.valid ? "LISTENING" : s.locked ? "SLAVE" : "UNCALIBRATED"; cJSON_AddStringToObject(p, "state", state); - cJSON_AddBoolToObject(p, "locked", s.locked); + cJSON_AddBoolToObject(p, "locked", s.locked || s.master); cJSON_AddNumberToObject(p, "version", 2); cJSON_AddNumberToObject(p, "own_class", s.own_class); fmt_id(id, s.port_id); @@ -524,7 +772,29 @@ void ptp_clock_status(cJSON *st) cJSON_AddNumberToObject(p, "window", WINDOW); cJSON_AddNumberToObject(p, "delay_req", s.delay_req); cJSON_AddNumberToObject(p, "delay_resp", s.delay_resp); - if (s.gm.valid) { + if (s.master) { + // We are the GM: the UI shows "(this device)" and "-" for offset/frequency/delay. + fmt_id(id, s.own.gm_id); + cJSON_AddStringToObject(p, "gm_id", id); + cJSON_AddNumberToObject(p, "gm_class", s.own.cls); + cJSON_AddNumberToObject(p, "gm_accuracy", s.own.acc); + cJSON_AddNumberToObject(p, "gm_p1", s.own.p1); + cJSON_AddNumberToObject(p, "gm_p2", s.own.p2); + cJSON_AddNumberToObject(p, "steps_removed", 0); + cJSON_AddBoolToObject(p, "gm_time_traceable", false); + cJSON_AddBoolToObject(p, "gm_freq_traceable", false); + if (s.sync_iv.n) { + win_stats(&s.sync_iv, &mean, &min, &max, &sd); + cJSON_AddNumberToObject(p, "sync_avg_ms", mean / 1e6); + cJSON_AddNumberToObject(p, "sync_min_ms", min / 1e6); + cJSON_AddNumberToObject(p, "sync_max_ms", max / 1e6); + cJSON_AddNumberToObject(p, "sync_jitter_us", sd / 1e3); + } + if (s.announce_iv.n) { + win_stats(&s.announce_iv, &mean, NULL, NULL, NULL); + cJSON_AddNumberToObject(p, "announce_avg_ms", mean / 1e3); + } + } else if (s.gm.valid) { fmt_id(id, s.gm.gm_id); cJSON_AddStringToObject(p, "gm_id", id); cJSON_AddNumberToObject(p, "gm_class", s.gm.cls); @@ -562,10 +832,10 @@ esp_err_t ptp_clock_start(esp_netif_t *netif) { s.netif = netif; s.lock = xSemaphoreCreateMutex(); - load_config(); uint8_t mac[6]; esp_netif_get_mac(netif, mac); const uint8_t pid[10] = { mac[0], mac[1], mac[2], 0xff, 0xfe, mac[3], mac[4], mac[5], 0, 1 }; memcpy(s.port_id, pid, sizeof(pid)); + load_config(); // after port_id: the own dataset uses it return xTaskCreate(ptp_task, "ptp", 4096, NULL, 10, NULL) == pdPASS ? ESP_OK : ESP_ERR_NO_MEM; } diff --git a/components/aes67_ptp/ptp_clock.h b/components/aes67_ptp/ptp_clock.h index 6bcf2a0..6f45f34 100644 --- a/components/aes67_ptp/ptp_clock.h +++ b/components/aes67_ptp/ptp_clock.h @@ -1,4 +1,4 @@ -// PTPv2 ordinary clock over UDP/IPv4 (E2E). TimeReceiver (TimeTransmitter: step 6). +// PTPv2 ordinary clock over UDP/IPv4 (E2E). TimeReceiver and TimeTransmitter (BMCA). #pragma once #include "cJSON.h" @@ -6,6 +6,8 @@ #include "esp_netif.h" esp_err_t ptp_clock_start(esp_netif_t *netif); +// Re-read the "ptp" config group (role, priorities, intervals, domain, DSCP) in the PTP task. +void ptp_clock_reconfig(void); bool ptp_clock_gm_id(char out[24]); bool ptp_clock_locked(void); // Adds the "ptp" object to /api/status.