diff --git a/components/aes67_ptp/CMakeLists.txt b/components/aes67_ptp/CMakeLists.txt index df17558..3853ea7 100644 --- a/components/aes67_ptp/CMakeLists.txt +++ b/components/aes67_ptp/CMakeLists.txt @@ -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) diff --git a/components/aes67_ptp/aes67_ptp.c b/components/aes67_ptp/aes67_ptp.c index 898d72a..d718e24 100644 --- a/components/aes67_ptp/aes67_ptp.c +++ b/components/aes67_ptp/aes67_ptp.c @@ -1,6 +1,19 @@ #include "aes67_ptp.h" +#include + #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; +} diff --git a/components/aes67_ptp/include/aes67_ptp.h b/components/aes67_ptp/include/aes67_ptp.h index f997ae7..9b6a89f 100644 --- a/components/aes67_ptp/include/aes67_ptp.h +++ b/components/aes67_ptp/include/aes67_ptp.h @@ -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); diff --git a/components/aes67_ptp/ptp_hw.c b/components/aes67_ptp/ptp_hw.c new file mode 100644 index 0000000..5851ee1 --- /dev/null +++ b/components/aes67_ptp/ptp_hw.c @@ -0,0 +1,96 @@ +#include "ptp_hw.h" + +#include + +#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; +} diff --git a/components/aes67_ptp/ptp_hw.h b/components/aes67_ptp/ptp_hw.h new file mode 100644 index 0000000..7814d52 --- /dev/null +++ b/components/aes67_ptp/ptp_hw.h @@ -0,0 +1,22 @@ +// EMAC IEEE 1588 clock and hardware timestamps for PTP over UDP/IPv4. +#pragma once + +#include +#include + +#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); diff --git a/main/main.c b/main/main.c index 989253b..d692bc5 100644 --- a/main/main.c +++ b/main/main.c @@ -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();