aes67-ESP32-P4/components/aes67_ptp/aes67_ptp.c

#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[] =
    "{\"mode\":\"multicast\",\"role\":\"auto\",\"domain\":0,\"priority1\":250,\"priority2\":250,"
    "\"log_sync\":0,\"log_announce\":1,\"announce_timeout\":3,\"log_delay_req\":0,"
    "\"dscp\":46,\"hw_ts\":true}";

static bool ptp_validate(const cJSON *g, char *err, size_t n)
{
    static const char *const modes[] = { "multicast", "hybrid", NULL };
    static const char *const roles[] = { "slave", "auto", "master", NULL };
    return cfg_check_enum(g, "mode", modes, err, n) &&
           cfg_check_enum(g, "role", roles, err, n) &&
           cfg_check_int(g, "domain", 0, 127, err, n) &&
           cfg_check_int(g, "priority1", 0, 255, err, n) &&
           cfg_check_int(g, "priority2", 0, 255, err, n) &&
           cfg_check_int(g, "log_sync", -4, 1, err, n) &&
           cfg_check_int(g, "log_announce", 0, 4, err, n) &&
           cfg_check_int(g, "announce_timeout", 2, 10, err, n) &&
           cfg_check_int(g, "log_delay_req", -4, 1, err, n) &&
           cfg_check_int(g, "dscp", 0, 63, err, n);
}

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;
}