bsncubed/aes67-ESP32-P4
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Step 6.1: PTP TimeTransmitter with BMCA (multicast)

Browse Source 
- 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 <noreply@anthropic.com>
This commit is contained in:
Ben Nicholson
2026-09-25 07:42:43 +10:00
parent 6d85eb49f8
commit 08b83a069c
3 changed files with 293 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
components/aes67_ptp/ptp_clock.c
+284 -14
View File
@@ -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;
}