Merge 104d9d5985eb72aef79eeb1b5cb9a92d0a888280 into 0461e2ff88369c3da0d4caced31e8488f53376cd

This commit is contained in:
Scramble Tools 2025-02-27 13:29:40 -08:00 committed by GitHub
commit dbc05626fb
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 403 additions and 79 deletions

View File

@ -258,6 +258,8 @@ esp_err_t emac_hal_ptp_start(emac_hal_context_t *hal, const emac_hal_ptp_config_
{
uint8_t base_increment;
emac_ll_ts_ptp_snap_type_sel(hal->ptp_regs, 1);
// Enable time stamping frame filtering (applicable to receive)
emac_ll_ts_ptp_ether_enable(hal->ptp_regs, true);
// Process frames with v2 format

View File

@ -89,9 +89,10 @@ struct ptpd_status_s
long drift_ppb;
/* Averaged path delay */
/* Averaged delay */
long path_delay_ns;
long peer_delay_ns;
/* Timestamps of latest received packets (CLOCK_MONOTONIC) */

View File

@ -112,6 +112,15 @@
#ifndef CONFIG_NETUTILS_PTPD_PATH_DELAY_STABILITY_NS
#define CONFIG_NETUTILS_PTPD_PATH_DELAY_STABILITY_NS 0
#endif
#ifndef CONFIG_NETUTILS_PTPD_PEER_DELAY_STABILITY_NS
#define CONFIG_NETUTILS_PTPD_PEER_DELAY_STABILITY_NS 0
#endif
#ifndef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
#define CONFIG_NETUTILS_PTPD_GPTP_PROFILE 1
#endif
#ifndef CONFIG_NETUTILS_PTPD_MAX_PEER_DELAY_NS
#define CONFIG_NETUTILS_PTPD_MAX_PEER_DELAY_NS 100000LL // 100us
#endif
#define clock_timespec_subtract(ts1, ts2, ts3) timespecsub(ts1, ts2, ts3)
#define clock_timespec_add(ts1, ts2, ts3) timespecadd(ts1, ts2, ts3)
@ -132,6 +141,18 @@ typedef struct
} pi_cntrl_t;
#endif // ESP_PTP
typedef union
{
struct ptp_header_s header;
struct ptp_announce_s announce;
struct ptp_sync_s sync;
struct ptp_follow_up_s follow_up;
struct ptp_delay_req_s delay_req;
struct ptp_delay_resp_s delay_resp;
struct ptp_delay_resp_follow_up_s delay_resp_follow_up;
uint8_t raw[128];
} ptp_msgbuf;
/* Carrier structure for querying PTPD status */
struct ptpd_statusreq_s
@ -157,6 +178,7 @@ struct ptp_state_s
int64_t local_time_ns_prev;
int64_t last_offset_ns;
double correction; // from last offset message
pi_cntrl_t offset_pi;
#else
@ -218,24 +240,17 @@ struct ptp_state_s
/* Timestamps related to path delay calculation (CLOCK_REALTIME) */
bool can_send_delayreq;
struct timespec delayreq_time;
struct timespec delayreq_time; // time of last delay_req sent
int path_delay_avgcount;
int peer_delay_avgcount;
long path_delay_ns;
long peer_delay_ns; // used for gPTP peer delay measurement
long delayreq_interval;
/* Latest received packet and its timestamp (CLOCK_REALTIME) */
struct timespec rxtime;
union
{
struct ptp_header_s header;
struct ptp_announce_s announce;
struct ptp_sync_s sync;
struct ptp_follow_up_s follow_up;
struct ptp_delay_req_s delay_req;
struct ptp_delay_resp_s delay_resp;
uint8_t raw[128];
} rxbuf;
ptp_msgbuf rxbuf;
#ifndef ESP_PTP
uint8_t rxcmsg[CMSG_LEN(sizeof(struct timeval))];
@ -247,6 +262,14 @@ struct ptp_state_s
struct ptp_sync_s twostep_packet;
struct timespec twostep_rxtime;
/* Buffered delay_resp packet for two-step peer delay measurement where server sends
* the accurate response origin timestamp in a separate follow-up message.
*/
struct ptp_delay_resp_s twostep_delay_resp_packet;
struct timespec twostep_delay_resp_rxtime;
};
#ifdef CONFIG_NETUTILS_PTPD_SERVER
@ -285,13 +308,40 @@ static struct ptp_state_s *s_state;
* Private Functions
****************************************************************************/
#ifdef ESP_PTP
static int64_t get_correction_ns(uint8_t *correction_field)
{
// Convert 6 bytes to 64-bit signed integer (nanoseconds << 16)
int64_t correction = 0;
// Handle sign extension for negative numbers
if (correction_field[0] & 0x80) {
correction = -1LL; // Fill with 1's for negative number
}
// Build the value byte by byte
correction = (correction << 8) | correction_field[0];
correction = (correction << 8) | correction_field[1];
correction = (correction << 8) | correction_field[2];
correction = (correction << 8) | correction_field[3];
correction = (correction << 8) | correction_field[4];
correction = (correction << 8) | correction_field[5];
// Convert from 2^16 scale to nanoseconds
return correction >> 16;
}
static void ptp_create_eth_frame(struct ptp_state_s *state, uint8_t *eth_frame, void *ptp_msg, uint16_t ptp_msg_len)
{
struct eth_hdr eth_hdr = {
//.dest.addr = {0x01, 0x80, 0xC2, 0x00, 0x00, 0x0E}, // TODO only for Pdelay_Req, Pdelay_Resp and Pdelay_Resp_Follow_Up
.dest.addr = {0x01, 0x1B, 0x19, 0x00, 0x00, 0x00}, // All except peer delay messages, ptp4l sends everything at this addr
.type = htons(ETH_TYPE_PTP)
};
#ifdef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
memcpy(&eth_hdr.dest.addr, LLDP_MULTICAST_ADDR, ETH_ADDR_LEN);
#else
memcpy(&eth_hdr.dest.addr, PTP4L_MULTICAST_ADDR, ETH_ADDR_LEN);
#endif
memcpy(&eth_hdr.src.addr, state->intf_hw_addr, ETH_ADDR_LEN);
memcpy(eth_frame, &eth_hdr, sizeof(eth_hdr));
@ -327,6 +377,7 @@ static int ptp_net_send(FAR struct ptp_state_s *state, void *ptp_msg, uint16_t p
if (ret > 0 && ts && ts_info->type == L2TAP_IREC_TIME_STAMP)
{
*ts = *(struct timespec *)ts_info->data;
ESP_LOGD("net_send", "ts is %lld.%09ld", (long long)ts->tv_sec, ts->tv_nsec);
}
return ret;
@ -359,6 +410,8 @@ static int ptp_net_recv(FAR struct ptp_state_s *state, void *ptp_msg, uint16_t p
if (ret > 0 && ts && ts_info->type == L2TAP_IREC_TIME_STAMP)
{
*ts = *(struct timespec *)ts_info->data;
ESP_LOGD("net_recv", "ts is %lld.%09ld", (long long)ts->tv_sec, ts->tv_nsec);
/* GETTING ZERO VALUES FOR TS !!! */
}
memcpy(ptp_msg, &eth_frame[ETH_HEADER_LEN], ret);
@ -427,12 +480,17 @@ static bool is_better_clock(FAR const struct ptp_announce_s *a,
FAR const struct ptp_announce_s *b)
{
if (a->gm_priority1 < b->gm_priority1 /* Main priority field */
#ifndef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
|| a->gm_quality[0] < b->gm_quality[0] /* Clock class */
|| a->gm_quality[1] < b->gm_quality[1] /* Clock accuracy */
|| a->gm_quality[2] < b->gm_quality[2] /* Clock variance high byte */
|| a->gm_quality[3] < b->gm_quality[3] /* Clock variance low byte */
|| a->gm_priority2 < b->gm_priority2 /* Sub priority field */
|| memcmp(a->gm_identity, b->gm_identity, sizeof(a->gm_identity)) < 0)
#endif
|| memcmp(a->gm_identity, b->gm_identity, sizeof(a->gm_identity)) < 0
#ifdef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
|| ((a->stepsremoved[0] << 8) | a->stepsremoved[1]) < ((b->stepsremoved[0] << 8) | b->stepsremoved[1]))
#endif
{
return true;
}
@ -940,12 +998,24 @@ static int ptp_send_announce(FAR struct ptp_state_s *state)
memset(&msg, 0, sizeof(msg));
msg = state->own_identity;
msg.header.messagetype = PTP_MSGTYPE_ANNOUNCE;
msg.header.messagelength[1] = sizeof(msg);
msg.header.messagelength[1] = sizeof(struct ptp_announce_s);
#ifdef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
msg.header.messagetype |= PTP_MSGTYPE_SDOID_GPTP; // gPTP profile message
msg.header.flags[1] = PTP_FLAGS1_PTP_TIMESCALE; // gPTP required flag
#endif
ptp_increment_sequence(&state->announce_seq, &msg.header);
ptp_gettime(state, &ts);
timespec_to_ptp_format(&ts, msg.origintimestamp);
/* Add the path trace TLV */
struct ptp_pathtrace_tlv_s pathtrace_tlv;
pathtrace_tlv.type[1] = 8; // Path trace
pathtrace_tlv.length[1] = 8; // 8 bytes
memcpy(pathtrace_tlv.pathsequence, state->own_identity.gm_identity, sizeof(state->own_identity.gm_identity));
msg.pathtracetlv = pathtrace_tlv;
#ifdef ESP_PTP
ret = ptp_net_send(state, &msg, sizeof(msg), NULL);
#else
@ -974,7 +1044,7 @@ static int ptp_send_sync(FAR struct ptp_state_s *state)
struct msghdr txhdr;
struct iovec txiov;
#endif // !ESP_PTP
struct ptp_sync_s msg;
ptp_msgbuf msg;
#ifndef ESP_PTP
struct sockaddr_in addr;
#endif // !ESP_PTP
@ -996,11 +1066,15 @@ static int ptp_send_sync(FAR struct ptp_state_s *state)
memset(&msg, 0, sizeof(msg));
msg.header = state->own_identity.header;
msg.header.messagetype = PTP_MSGTYPE_SYNC;
msg.header.messagelength[1] = sizeof(msg);
msg.header.messagelength[1] = sizeof(struct ptp_sync_s);
#ifdef CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC
#if defined(CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC) || defined(CONFIG_NETUTILS_PTPD_GPTP_PROFILE) // gPTP always uses two-step sync
msg.header.flags[0] = PTP_FLAGS0_TWOSTEP;
#endif
#ifdef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
msg.header.messagetype |= PTP_MSGTYPE_SDOID_GPTP; // gPTP profile message
msg.header.flags[1] = PTP_FLAGS1_PTP_TIMESCALE; // gPTP required flag
#endif
#ifndef ESP_PTP
txhdr.msg_name = &addr;
@ -1017,10 +1091,10 @@ static int ptp_send_sync(FAR struct ptp_state_s *state)
ptp_increment_sequence(&state->sync_seq, &msg.header);
ptp_gettime(state, &ts);
timespec_to_ptp_format(&ts, msg.origintimestamp);
timespec_to_ptp_format(&ts, msg.sync.origintimestamp);
#ifdef ESP_PTP
ret = ptp_net_send(state, &msg, sizeof(msg), &ts);
ret = ptp_net_send(state, &msg, sizeof(struct ptp_sync_s), &ts);
#else
ret = sendmsg(state->tx_socket, &txhdr, 0);
#endif // ESP_PTP
@ -1030,25 +1104,43 @@ static int ptp_send_sync(FAR struct ptp_state_s *state)
return ret;
}
#ifdef CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC
/* gPTP profile requires 2-step sync */
#if defined(CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC) || defined(CONFIG_NETUTILS_PTPD_GPTP_PROFILE)
#ifndef ESP_PTP
/* Get timestamp after send completes and send follow-up message
*
* TODO: Implement SO_TIMESTAMPING and use the actual tx timestamp here.
*/
ptp_gettime(state, &ts);
#endif // !ESP_PTP
timespec_to_ptp_format(&ts, msg.origintimestamp);
timespec_to_ptp_format(&ts, msg.follow_up.origintimestamp);
msg.header.messagetype = PTP_MSGTYPE_FOLLOW_UP;
msg.header.flags[0] = 0;
msg.header.messagelength[1] = sizeof(struct ptp_follow_up_s);
msg.header.flags[0] = 0; // Reset 2-step flag
msg.header.controlfield = 2; // Follow-up message
/* Add the information TLV (required for gPTP and ignored otherwise) */
struct ptp_info_tlv_s info_tlv;
memset(&info_tlv, 0, sizeof(info_tlv));
info_tlv.type[1] = 3; // Organization extension
info_tlv.length[1] = 0x1c; // 28 bytes
uint8_t orgidentity[] = {0x00,0x80,0xc2}; // 32962 (gPTP required value)
memcpy(info_tlv.orgidentity, orgidentity, sizeof(orgidentity));
info_tlv.orgsubtype[2] = 1; // gPTP required value
/* Remaining fields not used yet */
memcpy(msg.follow_up.informationtlv, &info_tlv, sizeof(info_tlv));
#ifndef ESP_PTP
addr.sin_port = HTONS(PTP_UDP_PORT_INFO);
ret = sendto(state->tx_socket, &msg, sizeof(msg), 0,
(struct sockaddr *)&addr, sizeof(addr));
#else
ret = ptp_net_send(state, &msg, sizeof(msg), NULL);
/* Send the follow up message */
ret = ptp_net_send(state, &msg, sizeof(struct ptp_follow_up_s), NULL);
#endif // !ESP_PTP
if (ret < 0)
{
@ -1070,7 +1162,7 @@ static int ptp_send_sync(FAR struct ptp_state_s *state)
static int ptp_send_delay_req(FAR struct ptp_state_s *state)
{
struct ptp_delay_req_s req;
struct ptp_delay_req_s msg;
#ifndef ESP_PTP
struct sockaddr_in addr;
#endif // !ESP_PTP
@ -1082,19 +1174,30 @@ static int ptp_send_delay_req(FAR struct ptp_state_s *state)
addr.sin_port = HTONS(PTP_UDP_PORT_EVENT);
#endif // !ESP_PTP
memset(&req, 0, sizeof(req));
req.header = state->own_identity.header;
req.header.messagetype = PTP_MSGTYPE_DELAY_REQ;
req.header.messagelength[1] = sizeof(req);
ptp_increment_sequence(&state->delay_req_seq, &req.header);
memset(&msg, 0, sizeof(msg));
msg.header = state->own_identity.header;
#ifdef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
msg.header.messagetype = PTP_MSGTYPE_PDELAY_REQ;
#else
msg.header.messagetype = PTP_MSGTYPE_DELAY_REQ;
#endif
msg.header.messagelength[1] = sizeof(struct ptp_delay_req_s);
#ifdef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
msg.header.messagetype |= PTP_MSGTYPE_SDOID_GPTP; // gPTP profile message
msg.header.flags[1] = PTP_FLAGS1_PTP_TIMESCALE; // gPTP required flag
msg.header.controlfield = 5;
#endif
ptp_increment_sequence(&state->delay_req_seq, &msg.header);
ptp_gettime(state, &state->delayreq_time);
timespec_to_ptp_format(&state->delayreq_time, req.origintimestamp);
timespec_to_ptp_format(&state->delayreq_time, msg.origintimestamp);
#ifdef ESP_PTP
ret = ptp_net_send(state, &req, sizeof(req), &state->delayreq_time);
ret = ptp_net_send(state, &msg, sizeof(struct ptp_delay_req_s), &state->delayreq_time);
#else
ret = sendto(state->tx_socket, &req, sizeof(req), 0,
ret = sendto(state->tx_socket, &msg, sizeof(struct ptp_delay_req_s), 0,
(FAR struct sockaddr *)&addr, sizeof(addr));
#endif // ESP_PTP
@ -1114,7 +1217,7 @@ static int ptp_send_delay_req(FAR struct ptp_state_s *state)
{
clock_gettime(CLOCK_MONOTONIC, &state->last_transmitted_delayreq);
ptpinfo("Sent delay req, seq %ld\n",
(long)ptp_get_sequence(&req.header));
(long)ptp_get_sequence(&msg.header));
}
return ret;
@ -1154,7 +1257,7 @@ static int ptp_periodic_send(FAR struct ptp_state_s *state)
}
#endif /* CONFIG_NETUTILS_PTPD_SERVER */
#ifdef CONFIG_NETUTILS_PTPD_SEND_DELAYREQ
#if defined(CONFIG_NETUTILS_PTPD_SEND_DELAYREQ) || defined(CONFIG_NETUTILS_PTPD_GPTP_PROFILE) // gPTP always sends delay requests
if (state->selected_source_valid && state->can_send_delayreq)
{
struct timespec time_now;
@ -1192,6 +1295,8 @@ static int ptp_process_announce(FAR struct ptp_state_s *state,
state->last_received_sync = state->last_received_announce;
state->path_delay_avgcount = 0;
state->path_delay_ns = 0;
state->peer_delay_avgcount = 0;
state->peer_delay_ns = 0;
state->delayreq_time.tv_sec = 0;
}
}
@ -1206,7 +1311,11 @@ static void ptp_lock_local_clock_freq(FAR struct ptp_state_s *state,
{
// Compute how off we are against master
int64_t offset_ns = timespec_delta_ns(remote_timestamp, local_timestamp);
#if CONFIG_NETUTILS_PTPD_GPTP_PROFILE
offset_ns += state->peer_delay_ns;
#else
offset_ns += state->path_delay_ns;
#endif
// TODO add offset filter
// Execute PI controller to elimitate the offset
@ -1248,7 +1357,11 @@ static void ptp_lock_local_clock_freq(FAR struct ptp_state_s *state,
// we would get incorrect delay
int64_t diff = llabs(offset_ns) - llabs(state->last_offset_ns);
static int cnt = 0;
#if CONFIG_NETUTILS_PTPD_GPTP_PROFILE
if (llabs(diff) < CONFIG_NETUTILS_PTPD_PEER_DELAY_STABILITY_NS) {
#else
if (llabs(diff) < CONFIG_NETUTILS_PTPD_PATH_DELAY_STABILITY_NS) {
#endif
if (cnt <= 3)
cnt++;
} else {
@ -1292,7 +1405,11 @@ static int ptp_update_local_clock(FAR struct ptp_state_s *state,
(long)remote_timestamp->tv_nsec);
delta_ns = timespec_delta_ns(remote_timestamp, local_timestamp);
#ifdef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
delta_ns += state->peer_delay_ns;
#else
delta_ns += state->path_delay_ns;
#endif
absdelta_ns = (delta_ns < 0) ? -delta_ns : delta_ns;
if (absdelta_ns > adj_limit_ns)
@ -1510,8 +1627,15 @@ static int ptp_process_followup(FAR struct ptp_state_s *state,
return OK;
}
/* Update correction field */
double correction_ns;
correction_ns = get_correction_ns(msg->header.correction);
memcpy(&state->correction, &correction_ns, sizeof(correction_ns));
/* Update local clock based on the remote timestamp we received now
* and the local timestamp of when the sync packet was received.
* For gPTP, we can also examine the information TLV for other changes
*/
ptp_format_to_timespec(msg->origintimestamp, &remote_time);
@ -1519,20 +1643,23 @@ static int ptp_process_followup(FAR struct ptp_state_s *state,
}
static int ptp_process_delay_req(FAR struct ptp_state_s *state,
FAR struct ptp_delay_req_s *msg)
FAR struct ptp_delay_req_s *req)
{
struct ptp_delay_resp_s resp;
ptp_msgbuf msg;
struct timespec ts;
#ifndef ESP_PTP
struct sockaddr_in addr;
#endif // !ESP_PTP
int ret;
#ifndef CONFIG_NETUTILS_PTPD_GPTP_PROFILE // gPTP always responds to delay requests
if (state->selected_source_valid)
{
/* We are operating as a client, ignore delay requests */
return OK;
}
#endif // !CONFIG_NETUTILS_PTPD_GPTP_PROFILE
#ifndef ESP_PTP
addr.sin_family = AF_INET;
@ -1540,38 +1667,80 @@ static int ptp_process_delay_req(FAR struct ptp_state_s *state,
addr.sin_port = HTONS(PTP_UDP_PORT_INFO);
#endif // !ESP_PTP
memset(&resp, 0, sizeof(resp));
resp.header = state->own_identity.header;
resp.header.messagetype = PTP_MSGTYPE_DELAY_RESP;
resp.header.messagelength[1] = sizeof(resp);
timespec_to_ptp_format(&state->rxtime, resp.receivetimestamp);
memcpy(resp.reqidentity, msg->header.sourceidentity,
sizeof(resp.reqidentity));
memcpy(resp.reqportindex, msg->header.sourceportindex,
sizeof(resp.reqportindex));
memcpy(resp.header.sequenceid, msg->header.sequenceid,
sizeof(resp.header.sequenceid));
resp.header.logmessageinterval = CONFIG_NETUTILS_PTPD_DELAYRESP_INTERVAL;
memset(&msg, 0, sizeof(msg));
msg.header = state->own_identity.header;
#ifdef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
msg.header.messagetype = PTP_MSGTYPE_PDELAY_RESP;
#else
msg.header.messagetype = PTP_MSGTYPE_DELAY_RESP;
#endif
msg.header.messagelength[1] = sizeof(struct ptp_delay_resp_s);
#if defined(CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC) || defined(CONFIG_NETUTILS_PTPD_GPTP_PROFILE)
msg.header.flags[0] = PTP_FLAGS0_TWOSTEP;
#endif
#ifdef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
msg.header.messagetype |= PTP_MSGTYPE_SDOID_GPTP; // gPTP profile message
msg.header.flags[1] = PTP_FLAGS1_PTP_TIMESCALE; // gPTP required flag
msg.header.controlfield = 5;
#endif
timespec_to_ptp_format(&state->rxtime, msg.delay_resp.receivetimestamp);
memcpy(msg.delay_resp.reqidentity, req->header.sourceidentity,
sizeof(req->header.sourceidentity));
memcpy(msg.delay_resp.reqportindex, req->header.sourceportindex,
sizeof(req->header.sourceportindex));
memcpy(msg.header.sequenceid, req->header.sequenceid,
sizeof(req->header.sequenceid));
#ifdef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
msg.header.logmessageinterval = 0; // gPTP required value
#else
msg.header.logmessageinterval = CONFIG_NETUTILS_PTPD_DELAYRESP_INTERVAL;
#endif // CONFIG_NETUTILS_PTPD_GPTP_PROFILE
/* Send the response message */
#ifdef ESP_PTP
ret = ptp_net_send(state, &resp, sizeof(resp), NULL);
ret = ptp_net_send(state, &msg, sizeof(struct ptp_delay_resp_s), &ts);
#else
ret = sendto(state->tx_socket, &resp, sizeof(resp), 0,
ret = sendto(state->tx_socket, &msg, sizeof(msg), 0,
(FAR struct sockaddr *)&addr, sizeof(addr));
#endif // ESP_PTP
if (ret < 0)
{
ptperr("sendto failed: %d", errno);
}
else
{
clock_gettime(CLOCK_MONOTONIC, &state->last_transmitted_delayresp);
ptpinfo("Sent delay resp, seq %ld\n",
(long)ptp_get_sequence(&msg->header));
return ret;
}
return ret;
clock_gettime(CLOCK_MONOTONIC, &state->last_transmitted_delayresp);
/* gPTP profile requires response follow-up message */
#ifdef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
timespec_to_ptp_format(&ts, msg.delay_resp_follow_up.origintimestamp);
msg.header.messagetype = PTP_MSGTYPE_PDELAY_RESP_FOLLOW_UP;
msg.header.messagelength[1] = sizeof(struct ptp_delay_resp_follow_up_s);
msg.header.flags[0] = 0; // Reset 2-step flag
/* Send the response follow-up message */
ret = ptp_net_send(state, &msg, sizeof(struct ptp_delay_resp_follow_up_s), NULL);
if (ret < 0)
{
ptperr("sendto for delay response follow-up message failed: %d\n", errno);
return ret;
}
ptpinfo("Sent response + response follow-up, seq %ld\n",
(long)ptp_get_sequence(&msg.header));
#else
ptpinfo("Sent delay resp, seq %ld\n",
(long)ptp_get_sequence(&req->header));
#endif /* CONFIG_NETUTILS_PTPD_GPTP_PROFILE */
return OK;
}
static int ptp_process_delay_resp(FAR struct ptp_state_s *state,
@ -1581,8 +1750,12 @@ static int ptp_process_delay_resp(FAR struct ptp_state_s *state,
int64_t sync_delay;
struct timespec remote_rxtime;
uint16_t sequence;
int interval;
#ifdef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
if (memcmp(msg->reqidentity,
state->own_identity.header.sourceidentity,
sizeof(msg->reqidentity)) != 0)
#else
if (!state->selected_source_valid ||
memcmp(msg->header.sourceidentity,
state->selected_source.header.sourceidentity,
@ -1590,6 +1763,7 @@ static int ptp_process_delay_resp(FAR struct ptp_state_s *state,
memcmp(msg->reqidentity,
state->own_identity.header.sourceidentity,
sizeof(msg->reqidentity)) != 0)
#endif // CONFIG_NETUTILS_PTPD_GPTP_PROFILE
{
return OK; /* This packet wasn't for us */
}
@ -1603,6 +1777,15 @@ static int ptp_process_delay_resp(FAR struct ptp_state_s *state,
return OK;
}
#ifdef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
/* We need to wait for a resp follow-up to calc peer delay. */
state->twostep_delay_resp_rxtime = state->rxtime;
state->twostep_delay_resp_packet = *msg;
ptpinfo("Waiting for delay response follow-up\n");
#else
/* Path delay is calculated as the average between delta for sync
* message and delta for delay req message.
* (IEEE-1588 section 11.3: Delay request-response mechanism)
@ -1634,7 +1817,6 @@ static int ptp_process_delay_resp(FAR struct ptp_state_s *state,
}
/* Calculate interval until next packet */
if (msg->header.logmessageinterval <= 12)
{
interval = (1 << msg->header.logmessageinterval);
@ -1644,9 +1826,84 @@ static int ptp_process_delay_resp(FAR struct ptp_state_s *state,
interval = 4096; /* Refuse to obey excessively long intervals */
}
/* Randomize up to 2x nominal delay) */
/* Randomize up to 2x nominal delay */
state->delayreq_interval = interval + (random() % interval);
#endif // CONFIG_NETUTILS_PTPD_GPTP_PROFILE
return OK;
}
static int ptp_process_delay_resp_follow_up(FAR struct ptp_state_s *state,
FAR struct ptp_delay_resp_follow_up_s *msg)
{
int64_t peer_delay_roundtrip;
int64_t peer_delay_reflection;
int64_t peer_delay;
struct timespec remote_txtime;
struct timespec remote_rxtime;
if (memcmp(msg->reqidentity,
state->own_identity.header.sourceidentity,
sizeof(msg->reqidentity)) != 0)
{
return OK; /* This packet wasn't for us */
}
if (ptp_get_sequence(&msg->header)
!= ptp_get_sequence(&state->twostep_delay_resp_packet.header))
{
ptpwarn("PTP delay response follow-up packet sequence %ld does not "
"match initial sync packet sequence %ld, ignoring\n",
(long)ptp_get_sequence(&msg->header),
(long)ptp_get_sequence(&state->twostep_delay_resp_packet.header));
return OK;
}
/* In gPTP (802.1AS), delay is measured between peers, not
* between the server and the client. It is calculated as follows:
Peer A Peer B
| |
| Peer delay_req |
t1 |----------------------------------->| t2
| |
| Peer delay_resp (t2) |
t4 |<-----------------------------------| t3
| |
| Peer delay_resp_follow_up (t3) |
|<-----------------------------------|
Peer A calculates peer_delay = ((t4 - t1) - (t3 - t2))/2
*/
/* Calculate peer delay */
peer_delay_roundtrip = timespec_delta_ns(&state->twostep_delay_resp_rxtime, &state->delayreq_time);
ptp_format_to_timespec(state->twostep_delay_resp_packet.receivetimestamp, &remote_rxtime);
ptp_format_to_timespec(msg->origintimestamp, &remote_txtime);
peer_delay_reflection = timespec_delta_ns(&remote_txtime, &remote_rxtime);
peer_delay = (peer_delay_roundtrip - peer_delay_reflection) / 2;
if (peer_delay >= 0 && peer_delay < CONFIG_NETUTILS_PTPD_MAX_PEER_DELAY_NS)
{
if (state->peer_delay_avgcount <
CONFIG_NETUTILS_PTPD_DELAYREQ_AVGCOUNT)
{
state->peer_delay_avgcount++;
}
state->peer_delay_ns += (peer_delay - state->peer_delay_ns)
/ state->peer_delay_avgcount;
ptpinfo("Peer delay: %ld ns (avg: %ld ns)\n",
(long)peer_delay, (long)state->peer_delay_ns);
}
else
{
ptpwarn("Peer delay out of range: %lld ns\n",
(long long)peer_delay);
}
return OK;
}
@ -1672,9 +1929,11 @@ static int ptp_process_rx_packet(FAR struct ptp_state_s *state,
clock_gettime(CLOCK_MONOTONIC, &state->last_received_multicast);
/* Rout the packet to the appropriate handler */
switch (state->rxbuf.header.messagetype & PTP_MSGTYPE_MASK)
{
#ifdef CONFIG_NETUTILS_PTPD_CLIENT
#if defined(CONFIG_NETUTILS_PTPD_CLIENT) || defined(CONFIG_NETUTILS_PTPD_GPTP_PROFILE) // gPTP always acts as a client
case PTP_MSGTYPE_ANNOUNCE:
ptpinfo("Got announce packet, seq %ld\n",
(long)ptp_get_sequence(&state->rxbuf.header));
@ -1683,26 +1942,35 @@ static int ptp_process_rx_packet(FAR struct ptp_state_s *state,
case PTP_MSGTYPE_SYNC:
ptpinfo("Got sync packet, seq %ld\n",
(long)ptp_get_sequence(&state->rxbuf.header));
if (!state->selected_source_valid) { return OK; } // ignore if operating as a server
return ptp_process_sync(state, &state->rxbuf.sync);
case PTP_MSGTYPE_FOLLOW_UP:
ptpinfo("Got follow-up packet, seq %ld\n",
(long)ptp_get_sequence(&state->rxbuf.header));
if (!state->selected_source_valid) { return OK; } // ignore if operating as a server
return ptp_process_followup(state, &state->rxbuf.follow_up);
case PTP_MSGTYPE_DELAY_RESP:
case PTP_MSGTYPE_PDELAY_RESP:
ptpinfo("Got delay-resp, seq %ld\n",
(long)ptp_get_sequence(&state->rxbuf.header));
return ptp_process_delay_resp(state, &state->rxbuf.delay_resp);
#endif
#ifdef CONFIG_NETUTILS_PTPD_SERVER
#if defined(CONFIG_NETUTILS_PTPD_SERVER) || defined(CONFIG_NETUTILS_PTPD_GPTP_PROFILE) // gPTP always responds to delay requests
case PTP_MSGTYPE_DELAY_REQ:
case PTP_MSGTYPE_PDELAY_REQ:
ptpinfo("Got delay req, seq %ld\n",
(long)ptp_get_sequence(&state->rxbuf.header));
return ptp_process_delay_req(state, &state->rxbuf.delay_req);
#endif
case PTP_MSGTYPE_PDELAY_RESP_FOLLOW_UP: //
ptpinfo("Got peer delay resp follow-up, seq %ld\n",
(long)ptp_get_sequence(&state->rxbuf.header));
return ptp_process_delay_resp_follow_up(state, &state->rxbuf.delay_resp_follow_up);
default:
ptpinfo("Ignoring unknown PTP packet type: 0x%02x\n",
state->rxbuf.header.messagetype);
@ -1791,6 +2059,7 @@ static void ptp_process_statusreq(FAR struct ptp_state_s *state)
status->last_adjtime_ns = state->last_adjtime_ns;
status->drift_ppb = state->drift_ppb;
status->path_delay_ns = state->path_delay_ns;
status->peer_delay_ns = state->peer_delay_ns;
/* Copy timestamps */
@ -1945,7 +2214,7 @@ static int ptp_daemon(int argc, FAR char** argv)
state->selected_source_valid = is_selected_source_valid(state);
ptp_process_statusreq(state);
}
} // while (!state->stop)
ptp_destroy_state(state);
free(state);

View File

@ -50,27 +50,60 @@
#define PTP_MULTICAST_ADDR ((in_addr_t)0xE0000181)
/* Multicast MAC addresses for PTP */
#define LLDP_MULTICAST_ADDR (uint8_t[6]){0x01, 0x80, 0xC2, 0x00, 0x00, 0x0e} // for all messages in case of gPTP
#define PTP4L_MULTICAST_ADDR (uint8_t[6]){0x01, 0x1B, 0x19, 0x00, 0x00, 0x00} // for sync, announce, follow_up (non-gPTP)
/* Message types */
#define PTP_MSGTYPE_MASK 0x0F
#define PTP_MSGTYPE_SYNC 0
#define PTP_MSGTYPE_DELAY_REQ 1
#define PTP_MSGTYPE_FOLLOW_UP 8
#define PTP_MSGTYPE_DELAY_RESP 9
#define PTP_MSGTYPE_ANNOUNCE 11
#define PTP_MSGTYPE_MASK 0x0F
#define PTP_MSGTYPE_SYNC 0x00
#define PTP_MSGTYPE_FOLLOW_UP 0x08
#define PTP_MSGTYPE_ANNOUNCE 0x0b
#define PTP_MSGTYPE_DELAY_REQ 0x01
#define PTP_MSGTYPE_DELAY_RESP 0x09
#define PTP_MSGTYPE_PDELAY_REQ 0x02 // only used in gPTP
#define PTP_MSGTYPE_PDELAY_RESP 0x03 // only used in gPTP
#define PTP_MSGTYPE_PDELAY_RESP_FOLLOW_UP 0x1a // only used in gPTP
/* Message flags */
#define PTP_FLAGS0_TWOSTEP (1 << 1)
#define PTP_FLAGS0_TWOSTEP (1 << 1) // flag indicating there will be a follow-up message
#define PTP_FLAGS1_PTP_TIMESCALE (1 << 3) // flag indicating use of PTP timescale (gPTP required)
#define PTP_MSGTYPE_SDOID_GPTP (1 << 4) // flag indicating a gPTP message
/****************************************************************************
* Public Types
****************************************************************************/
/* Defined in IEEE 1588-2008 Precision Time Protocol
/* Defined in IEEE 1588-2008 Precision Time Protocol and IEEE 802.1AS-2011
* All multi-byte fields are big-endian.
*/
/* Path trace TLV for gPTP follow up messages */
struct ptp_pathtrace_tlv_s
{
uint8_t type[2];
uint8_t length[2];
uint8_t pathsequence[8]; // this can have more but gPTP endpoints will ignore
};
/* Information TLV for gPTP announce messages */
struct ptp_info_tlv_s
{
uint8_t type[2];
uint8_t length[2];
uint8_t orgidentity[3];
uint8_t orgsubtype[3];
uint8_t cumulativescaledrateoffset[4];
uint8_t gmtimebaseindicator[2];
uint8_t lastgmphasechange[12];
uint8_t scaledlastgmfreqchange[4];
};
/* Common header for all message types */
struct ptp_header_s
@ -104,6 +137,7 @@ struct ptp_announce_s
uint8_t gm_identity[8];
uint8_t stepsremoved[2];
uint8_t timesource;
struct ptp_pathtrace_tlv_s pathtracetlv; // gPTP required
};
/* Sync: transmit timestamp from master clock */
@ -111,7 +145,7 @@ struct ptp_announce_s
struct ptp_sync_s
{
struct ptp_header_s header;
uint8_t origintimestamp[10];
uint8_t origintimestamp[10]; // in gPTP profile, this will be ignored
};
/* FollowUp: actual timestamp of when sync message was sent */
@ -120,17 +154,18 @@ struct ptp_follow_up_s
{
struct ptp_header_s header;
uint8_t origintimestamp[10];
uint8_t informationtlv[32]; // gPTP required
};
/* DelayReq: request delay measurement */
/* DelayReq: request delay measurement (path delay or peer delay) */
struct ptp_delay_req_s
{
struct ptp_header_s header;
uint8_t origintimestamp[10];
uint8_t origintimestamp[10]; // in gPTP profile, this will be ignored
};
/* DelayResp: response to DelayReq */
/* DelayResp: response to DelayReq (path delay or peer delay)*/
struct ptp_delay_resp_s
{
@ -140,4 +175,14 @@ struct ptp_delay_resp_s
uint8_t reqportindex[2];
};
/* DelayResp: follow up to DelayResp (gPTP only)*/
struct ptp_delay_resp_follow_up_s
{
struct ptp_header_s header;
uint8_t origintimestamp[10];
uint8_t reqidentity[8];
uint8_t reqportindex[2];
};
#endif /* __APPS_NETUTILS_PTPD_PTPV2_H */

View File

@ -8,3 +8,10 @@ CONFIG_EXAMPLE_ETH_PHY_IP101=y
CONFIG_NETUTILS_PTPD=y
CONFIG_NETUTILS_PTPD_CLIENT=y
CONFIG_NETUTILS_PTPD_SERVER=y
CONFIG_NETUTILS_PTPD_PRIORITY1=246
CONFIG_NETUTILS_PTPD_SERVERPRIO=100
CONFIG_NETUTILS_PTPD_TIMEOUT_MS=10000
CONFIG_NETUTILS_PTPD_SETTIME_THRESHOLD_MS=100
CONFIG_EXAMPLE_PTP_PULSE_GPIO=20
CONFIG_EXAMPLE_PTP_PULSE_WIDTH_NS=250000000