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

#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];
    uint8_t mac[6];                 // Ethernet source
    eth_mac_time_t ts;
} rx_rec_t;

static esp_eth_handle_t s_eth;
static esp_netif_t *s_netif;
static uint16_t s_ip_id;
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);
            memcpy(r->mac, buf + 6, 6);
            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)
{
    return ptp_hw_rx_ts_mac(msg_type, seq, src_port_id, ts, NULL);
}

bool ptp_hw_rx_ts_mac(uint8_t msg_type, uint16_t seq, const uint8_t *src_port_id, eth_mac_time_t *ts,
                      uint8_t src_mac[6])
{
    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;
            if (src_mac) {
                memcpy(src_mac, r->mac, 6);
            }
            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_adj_freq(double ppb)
{
    if (ppb > 500000) {
        ppb = 500000;       // +-500 ppm is far outside any sane crystal
    } else if (ppb < -500000) {
        ppb = -500000;
    }
    int32_t v = (int32_t)(ppb >= 0 ? ppb + 0.5 : ppb - 0.5);
    esp_err_t err = ESP_ERR_INVALID_STATE;
    // The previous addend update may still be in progress: retry briefly.
    for (int i = 0; i < 10 && err == ESP_ERR_INVALID_STATE; i++) {
        err = esp_eth_ioctl(s_eth, ETH_MAC_ESP_CMD_ADJ_PTP_TIME, &v);
    }
    return err;
}

esp_err_t ptp_hw_step(int64_t offset_ns)
{
    eth_mac_time_t t;
    esp_err_t err = ptp_hw_get_time(&t);
    if (err != ESP_OK) {
        return err;
    }
    int64_t ns = (int64_t)t.seconds * 1000000000LL + t.nanoseconds - offset_ns;
    if (ns < 0) {
        return ESP_ERR_INVALID_ARG;
    }
    t.seconds = ns / 1000000000LL;
    t.nanoseconds = ns % 1000000000LL;
    return esp_eth_ioctl(s_eth, ETH_MAC_ESP_CMD_S_PTP_TIME, &t);
}

static uint16_t ip_checksum(const uint8_t *h, size_t len)
{
    uint32_t sum = 0;
    for (size_t i = 0; i < len; i += 2) {
        sum += (h[i] << 8) | h[i + 1];
    }
    while (sum >> 16) {
        sum = (sum & 0xffff) + (sum >> 16);
    }
    return ~sum;
}

esp_err_t ptp_hw_send_event(const uint8_t dst_mac[6], uint32_t dst_ip, uint8_t dscp,
                            const uint8_t *msg, size_t len, eth_mac_time_t *tx_ts)
{
    uint8_t f[14 + 20 + 8 + 64];
    esp_netif_ip_info_t ip;
    if (len > 64 || esp_netif_get_ip_info(s_netif, &ip) != ESP_OK || !ip.ip.addr) {
        return ESP_ERR_INVALID_STATE;
    }
    // Ethernet
    memcpy(f, dst_mac, 6);
    esp_eth_ioctl(s_eth, ETH_CMD_G_MAC_ADDR, f + 6);
    f[12] = 0x08;
    f[13] = 0x00;
    // IPv4: no options, DF, TTL 1 (PTP stays in the subnet), UDP
    uint8_t *h = f + 14;
    size_t ip_len = 20 + 8 + len;
    uint16_t id = s_ip_id++;
    const uint8_t hdr[20] = { 0x45, (uint8_t)(dscp << 2), ip_len >> 8, ip_len & 0xff, id >> 8, id & 0xff,
                              0x40, 0x00, 1, 17, 0, 0 };
    memcpy(h, hdr, 12);
    memcpy(h + 12, &ip.ip.addr, 4);     // network byte order already
    memcpy(h + 16, &dst_ip, 4);
    uint16_t cs = ip_checksum(h, 20);
    h[10] = cs >> 8;
    h[11] = cs & 0xff;
    // UDP 319 -> 319, checksum 0 (allowed for IPv4)
    uint8_t *u = h + 20;
    u[0] = PTP_EVENT_PORT >> 8;
    u[1] = PTP_EVENT_PORT & 0xff;
    u[2] = PTP_EVENT_PORT >> 8;
    u[3] = PTP_EVENT_PORT & 0xff;
    u[4] = (8 + len) >> 8;
    u[5] = (8 + len) & 0xff;
    u[6] = u[7] = 0;
    memcpy(u + 8, msg, len);

    size_t total = 14 + ip_len;
    if (total < 60) {
        memset(f + total, 0, 60 - total);
        total = 60;
    }
    esp_err_t err = esp_eth_transmit_ctrl_vargs(s_eth, tx_ts, 2, f, (uint32_t)total);
    if (err == ESP_OK && !(tx_ts->seconds | tx_ts->nanoseconds)) {
        err = ESP_ERR_TIMEOUT;  // sent, but no TX timestamp
    }
    return err;
}

esp_err_t ptp_hw_init(esp_eth_handle_t eth, esp_netif_t *netif)
{
    s_eth = eth;
    s_netif = netif;
    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 PTP packet filter only stamps multicast PTP; unicast Delay_Req (hybrid mode) would get
    // no timestamp. Stamp every frame instead; rx_hook picks the PTP event messages.
    emac_ll_ts_all_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;
}