Step 3.2: TimeReceiver measurement (GM selection, Delay_Req/Resp, path delay)
- ptp_clock.c replaces the 3.1 logger: UDP/IPv4 multicast, E2E. Announce: IEEE 1588 dataset comparison picks the best GM, dropped after announceReceiptTimeout x its announce interval. Sync/Follow_Up (two-step and one-step) with HW t2 and correctionField. Delay_Req sent as a raw frame (DSCP ptp.dscp, TTL 1, clockIdentity = EUI-64 from MAC) with HW TX timestamp t3, randomised at the GM's Delay_Resp interval; Delay_Resp matched on requestingPortIdentity + seq. - Path delay corrected for offset drift between t2 and t3 (rate from consecutive Syncs) so it is right before the clock is syntonised. - ptp_hw: ptp_hw_send_event() builds Eth/IPv4/UDP 319 and returns the HW TX timestamp. - Verified vs ptp4l (i210 GM, HP 2530 non-PTP switch, PC 1G / board 100M): GM selected, path delay settles at ~10.3 us and stays flat, rate -39.8 +-0.4 ppm, offset drifts at -40 us/s (no servo yet). Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
@@ -1,4 +1,4 @@
|
||||
idf_component_register(SRCS "aes67_ptp.c" "ptp_hw.c"
|
||||
idf_component_register(SRCS "aes67_ptp.c" "ptp_clock.c" "ptp_hw.c"
|
||||
INCLUDE_DIRS "include"
|
||||
REQUIRES esp_eth
|
||||
PRIV_REQUIRES aes67_web esp_netif lwip hal soc)
|
||||
PRIV_REQUIRES aes67_web esp_netif esp_timer lwip hal soc)
|
||||
|
||||
@@ -1,20 +1,9 @@
|
||||
#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_clock.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,"
|
||||
@@ -42,103 +31,6 @@ 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");
|
||||
@@ -146,6 +38,5 @@ esp_err_t aes67_ptp_start(esp_eth_handle_t eth)
|
||||
if (err != ESP_OK) {
|
||||
return err;
|
||||
}
|
||||
xTaskCreate(ptp_log_task, "ptp", 4096, netif, 10, NULL);
|
||||
return ESP_OK;
|
||||
return ptp_clock_start(netif);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,331 @@
|
||||
#include "ptp_clock.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#include "aes67_cfg.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_random.h"
|
||||
#include "esp_timer.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
|
||||
#define HDR_LEN 34
|
||||
#define FLAG_TWO_STEP 0x0200
|
||||
|
||||
static const char *TAG = "ptp";
|
||||
static const uint8_t PTP_MCAST_MAC[6] = { 0x01, 0x00, 0x5e, 0x00, 0x01, 0x81 };
|
||||
|
||||
typedef struct {
|
||||
bool valid;
|
||||
uint8_t port_id[10]; // sourcePortIdentity of the GM (clockId + port)
|
||||
uint32_t ip; // source address, for hybrid mode later
|
||||
uint8_t p1, cls, acc, p2;
|
||||
uint16_t var;
|
||||
uint8_t gm_id[8];
|
||||
uint16_t steps;
|
||||
int8_t log_announce;
|
||||
int64_t last_us; // last Announce (esp_timer)
|
||||
} master_t;
|
||||
|
||||
static struct {
|
||||
esp_netif_t *netif;
|
||||
int ev, gen;
|
||||
uint8_t domain, dscp, timeout;
|
||||
uint8_t port_id[10]; // our clockId (EUI-64 from MAC) + port 1
|
||||
master_t gm;
|
||||
// Sync / Follow_Up
|
||||
uint16_t sync_seq;
|
||||
bool sync_pending;
|
||||
int64_t t1, t2, sync_corr;
|
||||
// Delay_Req / Delay_Resp
|
||||
uint16_t dreq_seq;
|
||||
bool dreq_pending;
|
||||
int64_t t3;
|
||||
int8_t log_dreq; // from Delay_Resp logMessageInterval
|
||||
int64_t next_dreq_us;
|
||||
int64_t delay_ns; // mean path delay, 0 = not measured yet
|
||||
// Drift of (t2 - t1) between Syncs: corrects the delay for the time between t2 and t3
|
||||
// while the local clock is not yet syntonised.
|
||||
int64_t raw, prev_raw, prev_t2;
|
||||
double rate;
|
||||
} s;
|
||||
|
||||
/* ----- helpers ----- */
|
||||
|
||||
static uint16_t rd16(const uint8_t *p) { return (p[0] << 8) | p[1]; }
|
||||
|
||||
static int64_t rd_ts(const uint8_t *p) // 48-bit seconds + 32-bit ns
|
||||
{
|
||||
uint64_t sec = ((uint64_t)rd16(p) << 32) | ((uint32_t)p[2] << 24) | (p[3] << 16) | (p[4] << 8) | p[5];
|
||||
uint32_t ns = ((uint32_t)p[6] << 24) | (p[7] << 16) | (p[8] << 8) | p[9];
|
||||
return (int64_t)sec * 1000000000LL + ns;
|
||||
}
|
||||
|
||||
static int64_t rd_corr_ns(const uint8_t *p) // correctionField: ns * 2^16
|
||||
{
|
||||
int64_t v = 0;
|
||||
for (int i = 0; i < 8; i++) {
|
||||
v = (v << 8) | p[i];
|
||||
}
|
||||
return v >> 16;
|
||||
}
|
||||
|
||||
static int64_t mac_ns(const eth_mac_time_t *t)
|
||||
{
|
||||
return (int64_t)t->seconds * 1000000000LL + t->nanoseconds;
|
||||
}
|
||||
|
||||
static void fmt_id(char *out, const uint8_t *id)
|
||||
{
|
||||
sprintf(out, "%02x-%02x-%02x-%02x-%02x-%02x-%02x-%02x", id[0], id[1], id[2], id[3], id[4], id[5], id[6], id[7]);
|
||||
}
|
||||
|
||||
// IEEE 1588 dataset comparison (without the topology part): <0 if a is better.
|
||||
static int compare(const master_t *a, const master_t *b)
|
||||
{
|
||||
if (a->p1 != b->p1) return a->p1 - b->p1;
|
||||
if (a->cls != b->cls) return a->cls - b->cls;
|
||||
if (a->acc != b->acc) return a->acc - b->acc;
|
||||
if (a->var != b->var) return a->var - b->var;
|
||||
if (a->p2 != b->p2) return a->p2 - b->p2;
|
||||
int c = memcmp(a->gm_id, b->gm_id, 8);
|
||||
if (c) return c;
|
||||
return a->steps - b->steps;
|
||||
}
|
||||
|
||||
static int open_socket(uint16_t port, struct in_addr ifaddr)
|
||||
{
|
||||
int fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
|
||||
int one = 1;
|
||||
setsockopt(fd, 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(fd, (struct sockaddr *)&a, sizeof(a)) < 0 ||
|
||||
setsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &m, sizeof(m)) < 0) {
|
||||
ESP_LOGE(TAG, "socket %u: bind/join failed (errno %d)", port, errno);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
return fd;
|
||||
}
|
||||
|
||||
static bool from_gm(const uint8_t *b)
|
||||
{
|
||||
return s.gm.valid && memcmp(b + 20, s.gm.port_id, 10) == 0;
|
||||
}
|
||||
|
||||
/* ----- message handling ----- */
|
||||
|
||||
static void on_announce(const uint8_t *b, int len, uint32_t src_ip)
|
||||
{
|
||||
if (len < 64) {
|
||||
return;
|
||||
}
|
||||
master_t m = {
|
||||
.valid = true, .ip = src_ip, .p1 = b[47], .cls = b[48], .acc = b[49], .var = rd16(b + 50),
|
||||
.p2 = b[52], .steps = rd16(b + 61), .log_announce = (int8_t)b[33], .last_us = esp_timer_get_time(),
|
||||
};
|
||||
memcpy(m.port_id, b + 20, 10);
|
||||
memcpy(m.gm_id, b + 53, 8);
|
||||
if (memcmp(m.port_id, s.port_id, 10) == 0) {
|
||||
return; // our own
|
||||
}
|
||||
if (from_gm(b)) {
|
||||
s.gm = m; // refresh dataset and timeout
|
||||
} else if (!s.gm.valid || compare(&m, &s.gm) < 0) {
|
||||
char id[24];
|
||||
fmt_id(id, m.gm_id);
|
||||
ESP_LOGI(TAG, "TimeTransmitter %s (p1 %u class %u p2 %u, %u hops) from " IPSTR,
|
||||
id, m.p1, m.cls, m.p2, m.steps, IP2STR((esp_ip4_addr_t *)&src_ip));
|
||||
s.gm = m;
|
||||
s.sync_pending = s.dreq_pending = false;
|
||||
s.delay_ns = s.prev_t2 = 0;
|
||||
s.log_dreq = 0;
|
||||
s.next_dreq_us = 0;
|
||||
}
|
||||
}
|
||||
|
||||
static void sync_complete(void)
|
||||
{
|
||||
s.sync_pending = false;
|
||||
s.raw = s.t2 - s.t1 - s.sync_corr;
|
||||
if (s.prev_t2 && s.t2 > s.prev_t2) {
|
||||
s.rate = (double)(s.raw - s.prev_raw) / (double)(s.t2 - s.prev_t2);
|
||||
}
|
||||
s.prev_raw = s.raw;
|
||||
s.prev_t2 = s.t2;
|
||||
if (!s.delay_ns) {
|
||||
return;
|
||||
}
|
||||
// offset = t2 - t1 - corrections - mean path delay
|
||||
int64_t offset = s.raw - s.delay_ns;
|
||||
ESP_LOGI(TAG, "seq %u: offset %+lld ns, path delay %lld ns, rate %+.3f ppm", s.sync_seq, offset, s.delay_ns,
|
||||
s.rate * 1e6);
|
||||
}
|
||||
|
||||
static void on_sync(const uint8_t *b, int len)
|
||||
{
|
||||
if (len < 44 || !from_gm(b)) {
|
||||
return;
|
||||
}
|
||||
eth_mac_time_t t2;
|
||||
uint16_t seq = rd16(b + 30);
|
||||
if (!ptp_hw_rx_ts(PTP_MSG_SYNC, seq, b + 20, &t2)) {
|
||||
ESP_LOGW(TAG, "Sync %u: no HW RX timestamp", seq);
|
||||
return;
|
||||
}
|
||||
s.sync_seq = seq;
|
||||
s.t2 = mac_ns(&t2);
|
||||
s.sync_corr = rd_corr_ns(b + 8);
|
||||
if (rd16(b + 6) & FLAG_TWO_STEP) {
|
||||
s.sync_pending = true; // wait for Follow_Up
|
||||
} else {
|
||||
s.t1 = rd_ts(b + 34);
|
||||
sync_complete();
|
||||
}
|
||||
}
|
||||
|
||||
static void on_follow_up(const uint8_t *b, int len)
|
||||
{
|
||||
if (len < 44 || !from_gm(b) || !s.sync_pending || rd16(b + 30) != s.sync_seq) {
|
||||
return;
|
||||
}
|
||||
s.t1 = rd_ts(b + 34);
|
||||
s.sync_corr += rd_corr_ns(b + 8);
|
||||
sync_complete();
|
||||
}
|
||||
|
||||
static void send_delay_req(void)
|
||||
{
|
||||
uint8_t m[44] = { 0 };
|
||||
m[0] = PTP_MSG_DELAY_REQ;
|
||||
m[1] = 2;
|
||||
m[3] = sizeof(m);
|
||||
m[4] = s.domain;
|
||||
memcpy(m + 20, s.port_id, 10);
|
||||
s.dreq_seq++;
|
||||
m[30] = s.dreq_seq >> 8;
|
||||
m[31] = s.dreq_seq & 0xff;
|
||||
m[32] = 1; // controlField: Delay_Req
|
||||
m[33] = 0x7f;
|
||||
uint32_t dst;
|
||||
inet_aton(PTP_MCAST, (struct in_addr *)&dst);
|
||||
eth_mac_time_t t3;
|
||||
esp_err_t err = ptp_hw_send_event(PTP_MCAST_MAC, dst, s.dscp, m, sizeof(m), &t3);
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGW(TAG, "Delay_Req %u: %s", s.dreq_seq, esp_err_to_name(err));
|
||||
s.dreq_pending = false;
|
||||
return;
|
||||
}
|
||||
s.t3 = mac_ns(&t3);
|
||||
s.dreq_pending = true;
|
||||
}
|
||||
|
||||
static void on_delay_resp(const uint8_t *b, int len)
|
||||
{
|
||||
if (len < 54 || !from_gm(b) || !s.dreq_pending || rd16(b + 30) != s.dreq_seq ||
|
||||
memcmp(b + 44, s.port_id, 10) != 0) {
|
||||
return;
|
||||
}
|
||||
s.dreq_pending = false;
|
||||
int64_t t4 = rd_ts(b + 34) - rd_corr_ns(b + 8);
|
||||
s.log_dreq = (int8_t)b[33];
|
||||
if (!s.prev_t2) {
|
||||
return;
|
||||
}
|
||||
// mean path delay = ((t2 - t1 - corr) + (t4 - t3)) / 2, plus the offset drift between t2 and t3
|
||||
int64_t d = (s.raw + (t4 - s.t3) + (int64_t)(s.rate * (double)(s.t3 - s.t2))) / 2;
|
||||
s.delay_ns = s.delay_ns ? (s.delay_ns * 7 + d) / 8 : d; // light smoothing
|
||||
}
|
||||
|
||||
/* ----- task ----- */
|
||||
|
||||
static void load_config(void)
|
||||
{
|
||||
cJSON *c = cfg_get("ptp");
|
||||
s.domain = cJSON_GetObjectItem(c, "domain")->valueint;
|
||||
s.dscp = cJSON_GetObjectItem(c, "dscp")->valueint;
|
||||
s.timeout = cJSON_GetObjectItem(c, "announce_timeout")->valueint;
|
||||
cJSON_Delete(c);
|
||||
}
|
||||
|
||||
static void ptp_task(void *arg)
|
||||
{
|
||||
esp_netif_ip_info_t ip = { 0 };
|
||||
while (esp_netif_get_ip_info(s.netif, &ip) != ESP_OK || !ip.ip.addr) {
|
||||
vTaskDelay(pdMS_TO_TICKS(500));
|
||||
}
|
||||
struct in_addr ifaddr = { .s_addr = ip.ip.addr };
|
||||
s.ev = open_socket(PTP_EVENT_PORT, ifaddr);
|
||||
s.gen = open_socket(PTP_GENERAL_PORT, ifaddr);
|
||||
if (s.ev < 0 || s.gen < 0) {
|
||||
vTaskDelete(NULL);
|
||||
}
|
||||
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);
|
||||
|
||||
uint8_t b[128];
|
||||
while (1) {
|
||||
fd_set fds;
|
||||
FD_ZERO(&fds);
|
||||
FD_SET(s.ev, &fds);
|
||||
FD_SET(s.gen, &fds);
|
||||
struct timeval tv = { .tv_sec = 0, .tv_usec = 100000 };
|
||||
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;
|
||||
if (!FD_ISSET(fd, &fds)) {
|
||||
continue;
|
||||
}
|
||||
struct sockaddr_in src;
|
||||
socklen_t sl = sizeof(src);
|
||||
int len = recvfrom(fd, b, sizeof(b), 0, (struct sockaddr *)&src, &sl);
|
||||
if (len < HDR_LEN || (b[1] & 0x0f) != 2 || b[4] != s.domain) {
|
||||
continue;
|
||||
}
|
||||
switch (b[0] & 0x0f) {
|
||||
case PTP_MSG_ANNOUNCE: on_announce(b, len, src.sin_addr.s_addr); break;
|
||||
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;
|
||||
default: break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int64_t now = esp_timer_get_time();
|
||||
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);
|
||||
if (now - s.gm.last_us > window) {
|
||||
ESP_LOGW(TAG, "TimeTransmitter lost (no Announce for %lld ms), listening", window / 1000);
|
||||
memset(&s.gm, 0, sizeof(s.gm));
|
||||
s.delay_ns = s.prev_t2 = 0;
|
||||
} 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
|
||||
int64_t iv = s.log_dreq >= 0 ? 1000000LL << s.log_dreq : 1000000LL >> -s.log_dreq;
|
||||
s.next_dreq_us = now + iv / 2 + (esp_random() % (uint32_t)iv);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
esp_err_t ptp_clock_start(esp_netif_t *netif)
|
||||
{
|
||||
s.netif = netif;
|
||||
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));
|
||||
return xTaskCreate(ptp_task, "ptp", 4096, NULL, 10, NULL) == pdPASS ? ESP_OK : ESP_ERR_NO_MEM;
|
||||
}
|
||||
@@ -0,0 +1,7 @@
|
||||
// PTPv2 ordinary clock over UDP/IPv4 (E2E). Step 3.2: TimeReceiver measurement only.
|
||||
#pragma once
|
||||
|
||||
#include "esp_err.h"
|
||||
#include "esp_netif.h"
|
||||
|
||||
esp_err_t ptp_clock_start(esp_netif_t *netif);
|
||||
@@ -22,6 +22,8 @@ typedef struct {
|
||||
} 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;
|
||||
@@ -75,9 +77,70 @@ 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);
|
||||
}
|
||||
|
||||
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) {
|
||||
|
||||
@@ -20,3 +20,7 @@ esp_err_t ptp_hw_init(esp_eth_handle_t eth, esp_netif_t *netif);
|
||||
// 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);
|
||||
// Send a PTP event message as a raw Ethernet/IPv4/UDP frame (port 319 -> 319) and return its
|
||||
// hardware TX timestamp. dst_ip/dst_mac: 224.0.1.129 / 01:00:5e:00:01:81 or the GM unicast.
|
||||
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);
|
||||
|
||||
Reference in New Issue
Block a user