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

Step 3.1: EMAC hardware timestamps for PTP over UDP/IPv4

Browse Source 
- aes67_ptp/ptp_hw.c: start the EMAC IEEE 1588 clock (IDF
  ETH_MAC_ESP_CMD_PTP_ENABLE) and additionally enable snapshots for
  PTP over UDP/IPv4 (IDF only enables PTP over Ethernet/L2). An RX hook
  on the driver's info input path records {type, seq, sourcePortId, HW
  timestamp} of port-319 PTPv2 event messages, then hands every frame to
  lwIP unchanged.
- aes67_ptp.c: temporary 3.1 logger joins 224.0.1.129:319/320 and pairs
  Sync HW RX timestamps with Follow_Up origin timestamps.
- Checked against linuxptp ptp4l (-4 -E -H, Intel igb) as GM: every Sync
  has a HW timestamp; HW Sync intervals track the GM intervals with a
  constant -39.8 us/s (+-0.3 us), i.e. local clock -39.8 ppm vs GM.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
Ben Nicholson
2026-09-24 23:44:11 +10:00
parent f259c5d554
commit 6090ceb239
6 changed files with 247 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
components/aes67_ptp/CMakeLists.txt
+3 -2
View File
@@ -1,3 +1,4 @@
idf_component_register(SRCS "aes67_ptp.c"
idf_component_register(SRCS "aes67_ptp.c" "ptp_hw.c"
INCLUDE_DIRS "include"
PRIV_REQUIRES aes67_web)
REQUIRES esp_eth
PRIV_REQUIRES aes67_web esp_netif lwip hal soc)
components/aes67_ptp/aes67_ptp.c
+121
View File
@@ -1,6 +1,19 @@
#include "aes67_ptp.h"
#include <string.h>
#include "aes67_cfg.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "lwip/sockets.h"
#include "ptp_hw.h"
#define PTP_MCAST "224.0.1.129"
#define PTP_EVENT_PORT 319
#define PTP_GENERAL_PORT 320
static const char *TAG = "ptp";
// Core defaults: Riedel SIC intervals, role auto with fallback priorities (docs/aes67-core-base.md).
static const char PTP_DEFAULTS[] =
@@ -28,3 +41,111 @@ esp_err_t aes67_ptp_init(void)
{
return cfg_register("ptp", PTP_DEFAULTS, ptp_validate, NULL);
}
/* ----- Step 3.1: hardware timestamp check (temporary logger) ----- */
static int open_ptp_socket(uint16_t port, struct in_addr ifaddr)
{
int s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
int one = 1;
setsockopt(s, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
struct sockaddr_in a = { .sin_family = AF_INET, .sin_port = htons(port), .sin_addr.s_addr = htonl(INADDR_ANY) };
struct ip_mreq m = { .imr_interface = ifaddr };
inet_aton(PTP_MCAST, &m.imr_multiaddr);
if (bind(s, (struct sockaddr *)&a, sizeof(a)) < 0 ||
setsockopt(s, IPPROTO_IP, IP_ADD_MEMBERSHIP, &m, sizeof(m)) < 0) {
ESP_LOGE(TAG, "socket %u: bind/join failed (errno %d)", port, errno);
close(s);
return -1;
}
return s;
}
static int64_t ts_ns(uint64_t sec, uint32_t ns)
{
return (int64_t)sec * 1000000000LL + ns;
}
static void ptp_log_task(void *arg)
{
esp_netif_t *netif = arg;
esp_netif_ip_info_t ip = { 0 };
while (esp_netif_get_ip_info(netif, &ip) != ESP_OK || !ip.ip.addr) {
vTaskDelay(pdMS_TO_TICKS(500));
}
struct in_addr ifaddr = { .s_addr = ip.ip.addr };
int ev = open_ptp_socket(PTP_EVENT_PORT, ifaddr);
int gen = open_ptp_socket(PTP_GENERAL_PORT, ifaddr);
if (ev < 0 || gen < 0) {
vTaskDelete(NULL);
}
ESP_LOGI(TAG, "joined " PTP_MCAST " on " IPSTR ", waiting for Sync/Follow_Up", IP2STR(&ip.ip));
uint8_t b[128];
uint16_t sync_seq = 0;
uint8_t sync_port[10];
eth_mac_time_t t2 = { 0 };
bool have_sync = false;
int64_t prev_t1 = 0, prev_t2 = 0, first_t1 = 0, first_diff = 0;
int n = 0;
while (1) {
fd_set fds;
FD_ZERO(&fds);
FD_SET(ev, &fds);
FD_SET(gen, &fds);
if (select((ev > gen ? ev : gen) + 1, &fds, NULL, NULL, NULL) <= 0) {
continue;
}
if (FD_ISSET(ev, &fds)) {
int len = recv(ev, b, sizeof(b), 0);
if (len >= 34 && (b[0] & 0x0f) == PTP_MSG_SYNC) {
sync_seq = (b[30] << 8) | b[31];
memcpy(sync_port, b + 20, 10);
have_sync = ptp_hw_rx_ts(PTP_MSG_SYNC, sync_seq, sync_port, &t2);
if (!have_sync) {
ESP_LOGW(TAG, "Sync seq %u: no hardware RX timestamp", sync_seq);
}
}
}
if (FD_ISSET(gen, &fds)) {
int len = recv(gen, b, sizeof(b), 0);
if (len >= 44 && (b[0] & 0x0f) == PTP_MSG_FOLLOW_UP && have_sync &&
((b[30] << 8) | b[31]) == sync_seq && memcmp(b + 20, sync_port, 10) == 0) {
// preciseOriginTimestamp: 48-bit seconds + 32-bit nanoseconds at offset 34
uint64_t sec = ((uint64_t)b[34] << 40) | ((uint64_t)b[35] << 32) | ((uint64_t)b[36] << 24) |
((uint64_t)b[37] << 16) | ((uint64_t)b[38] << 8) | b[39];
uint32_t nsec = ((uint32_t)b[40] << 24) | (b[41] << 16) | (b[42] << 8) | b[43];
int64_t t1 = ts_ns(sec, nsec), t2n = ts_ns(t2.seconds, t2.nanoseconds);
if (n == 0) {
first_t1 = t1;
first_diff = t2n - t1;
}
if (n > 0) {
int64_t d1 = t1 - prev_t1, d2 = t2n - prev_t2;
// Drift of (t2 - t1) since the first Sync: our free-running clock vs the GM.
int64_t drift = (t2n - t1) - first_diff;
int64_t span = t1 - first_t1;
ESP_LOGI(TAG, "Sync %u: t1 %llu.%09lu t2(hw) %lu.%09lu interval GM %lld ns, HW %lld ns (diff %+lld) drift %+lld ns = %+.2f ppm",
sync_seq, sec, (unsigned long)nsec, (unsigned long)t2.seconds, (unsigned long)t2.nanoseconds,
d1, d2, d2 - d1, drift, span > 0 ? drift * 1e6 / span : 0.0);
}
prev_t1 = t1;
prev_t2 = t2n;
n++;
have_sync = false;
}
}
}
}
esp_err_t aes67_ptp_start(esp_eth_handle_t eth)
{
esp_netif_t *netif = esp_netif_get_handle_from_ifkey("ETH_DEF");
esp_err_t err = ptp_hw_init(eth, netif);
if (err != ESP_OK) {
return err;
}
xTaskCreate(ptp_log_task, "ptp", 4096, netif, 10, NULL);
return ESP_OK;
}
components/aes67_ptp/include/aes67_ptp.h
+4 -1
View File
@@ -3,6 +3,9 @@
#pragma once
#include "esp_err.h"
#include "esp_eth_driver.h"
// Registers the "ptp" config group. (Clock itself: step 3.)
// Registers the "ptp" config group. Call early, before the config is used.
esp_err_t aes67_ptp_init(void);
// Start the EMAC PTP clock and the PTP task on this Ethernet interface.
esp_err_t aes67_ptp_start(esp_eth_handle_t eth);
components/aes67_ptp/ptp_hw.c
+96
View File
@@ -0,0 +1,96 @@
#include "ptp_hw.h"
#include <string.h>
#include "esp_eth_mac_esp.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "hal/emac_ll.h"
#include "soc/emac_ptp_struct.h"
#define RX_RING 16
#define PTP_EVENT_PORT 319
static const char *TAG = "ptp_hw";
typedef struct {
bool used;
uint8_t type;
uint16_t seq;
uint8_t port_id[10];
eth_mac_time_t ts;
} rx_rec_t;
static esp_eth_handle_t s_eth;
static rx_rec_t s_rx[RX_RING];
static int s_rx_next;
static portMUX_TYPE s_lock = portMUX_INITIALIZER_UNLOCKED;
// Runs in the EMAC RX task for every frame: note PTP event timestamps, then pass to lwIP.
static esp_err_t rx_hook(esp_eth_handle_t eth, uint8_t *buf, uint32_t len, void *priv, void *info)
{
const eth_mac_time_t *ts = info;
// Ethernet (untagged) + IPv4 + UDP to port 319 + PTP header (34 bytes)
if (ts && (ts->seconds | ts->nanoseconds) && len >= 14 + 20 + 8 + 34 &&
buf[12] == 0x08 && buf[13] == 0x00 && buf[23] == 17) {
const uint8_t *ip = buf + 14;
size_t ihl = (ip[0] & 0x0f) * 4;
const uint8_t *udp = ip + ihl;
const uint8_t *ptp = udp + 8;
if (ihl >= 20 && ptp + 34 <= buf + len && ((udp[2] << 8) | udp[3]) == PTP_EVENT_PORT &&
(ptp[1] & 0x0f) == 2) {
portENTER_CRITICAL(&s_lock);
rx_rec_t *r = &s_rx[s_rx_next];
s_rx_next = (s_rx_next + 1) % RX_RING;
r->used = true;
r->type = ptp[0] & 0x0f;
r->seq = (ptp[30] << 8) | ptp[31];
memcpy(r->port_id, ptp + 20, 10);
r->ts = *ts;
portEXIT_CRITICAL(&s_lock);
}
}
return esp_netif_receive((esp_netif_t *)priv, buf, len, NULL);
}
bool ptp_hw_rx_ts(uint8_t msg_type, uint16_t seq, const uint8_t *src_port_id, eth_mac_time_t *ts)
{
bool found = false;
portENTER_CRITICAL(&s_lock);
for (int i = 0; i < RX_RING; i++) {
rx_rec_t *r = &s_rx[i];
if (r->used && r->type == msg_type && r->seq == seq && memcmp(r->port_id, src_port_id, 10) == 0) {
*ts = r->ts;
r->used = false;
found = true;
break;
}
}
portEXIT_CRITICAL(&s_lock);
return found;
}
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_init(esp_eth_handle_t eth, esp_netif_t *netif)
{
s_eth = eth;
bool on = true;
esp_err_t err = esp_eth_ioctl(eth, ETH_MAC_ESP_CMD_PTP_ENABLE, &on);
if (err != ESP_OK) {
ESP_LOGE(TAG, "EMAC PTP enable failed: %s", esp_err_to_name(err));
return err;
}
// IDF enables timestamping for PTP over Ethernet (L2) only; AES67 uses UDP/IPv4.
emac_ll_ts_ptp_ip4_enable(&EMAC_PTP, true);
// The netif glue registered its own input path; take it over and forward to the netif.
err = esp_eth_update_input_path_info(eth, rx_hook, netif);
if (err == ESP_OK) {
ESP_LOGI(TAG, "EMAC IEEE 1588 clock running, HW timestamps for PTP over UDP/IPv4");
}
return err;
}
components/aes67_ptp/ptp_hw.h
+22
View File
@@ -0,0 +1,22 @@
// EMAC IEEE 1588 clock and hardware timestamps for PTP over UDP/IPv4.
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include "esp_eth_driver.h"
#include "esp_eth_mac.h"
#include "esp_netif.h"
#define PTP_MSG_SYNC 0x0
#define PTP_MSG_DELAY_REQ 0x1
#define PTP_MSG_FOLLOW_UP 0x8
#define PTP_MSG_DELAY_RESP 0x9
#define PTP_MSG_ANNOUNCE 0xB
// Start the EMAC PTP clock, enable IPv4/UDP timestamping and hook the RX path.
esp_err_t ptp_hw_init(esp_eth_handle_t eth, esp_netif_t *netif);
// Hardware RX timestamp of an event message (UDP port 319), looked up by type,
// 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);
main/main.c
+1
View File
@@ -31,6 +31,7 @@ void app_main(void)
ESP_ERROR_CHECK(aes67_net_init(eth));
ESP_ERROR_CHECK(aes67_health_init());
ESP_ERROR_CHECK(aes67_ptp_init());
ESP_ERROR_CHECK(aes67_ptp_start(eth));
ESP_ERROR_CHECK(aes67_tx_init());
ESP_ERROR_CHECK(aes67_syslog_init());
project_cfg_register();