Skip to content

Commit

Permalink
Added support for gPTP
Browse files Browse the repository at this point in the history 
This includes handling peer delay messages and TLVs in announce and follow up messages. Also, handles simpler BMCA in gPTP case. Currently it seems the HW timestamp is not showing in the IREC in the case of received frames, so that made it difficult to test interop with other gPTP devices.
  • Loading branch information
@scrambletools
scrambletools committed Dec 9, 2024
1 parent a2f420a commit 2c1ed44
Show file tree
Hide file tree
Showing 3 changed files with 207 additions and 61 deletions.
189 changes: 137 additions & 52 deletions examples/ethernet/ptp/components/ptpd/ptpd.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -132,6 +132,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
Expand Down Expand Up @@ -226,16 +238,7 @@ struct ptp_state_s
/* 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))];
Expand Down Expand Up @@ -288,10 +291,14 @@ static struct ptp_state_s *s_state;
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));
Expand Down Expand Up @@ -327,6 +334,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;
Expand Down Expand Up @@ -359,6 +367,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);
Expand Down Expand Up @@ -427,12 +437,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;
}
Expand Down Expand Up @@ -942,10 +957,21 @@ static int ptp_send_announce(FAR struct ptp_state_s *state)
msg.header.messagetype = PTP_MSGTYPE_ANNOUNCE;
msg.header.messagelength[1] = sizeof(msg);

#ifdef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
msg.header.flags[1] = PTP_FLAGS1_PTP_TIMESCALE;
#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
Expand Down Expand Up @@ -974,7 +1000,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
Expand All @@ -996,11 +1022,14 @@ 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.flags[1] = PTP_FLAGS1_PTP_TIMESCALE;
#endif

#ifndef ESP_PTP
txhdr.msg_name = &addr;
Expand All @@ -1017,10 +1046,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
Expand All @@ -1030,25 +1059,40 @@ 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
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)
{
Expand Down Expand Up @@ -1154,7 +1198,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;
Expand Down Expand Up @@ -1519,59 +1563,96 @@ 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;
addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);
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;
msg.header.messagetype = PTP_MSGTYPE_DELAY_RESP;
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.flags[1] = PTP_FLAGS1_PTP_TIMESCALE;
msg.header.controlfield = 5; // Other message
#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);
return ret;
}
else

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_DELAY_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)
{
clock_gettime(CLOCK_MONOTONIC, &state->last_transmitted_delayresp);
ptpinfo("Sent delay resp, seq %ld\n",
(long)ptp_get_sequence(&msg->header));
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 ret;
return OK;
}

static int ptp_process_delay_resp(FAR struct ptp_state_s *state,
Expand All @@ -1581,7 +1662,6 @@ 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;

if (!state->selected_source_valid ||
memcmp(msg->header.sourceidentity,
Expand All @@ -1606,6 +1686,8 @@ static int ptp_process_delay_resp(FAR struct ptp_state_s *state,
/* 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)
* In the case of gPTP (802.1AS), the delay is between peers, not
* between the server and the client. The calculation is still the same.
*/

ptp_format_to_timespec(msg->receivetimestamp, &remote_rxtime);
Expand Down Expand Up @@ -1633,8 +1715,10 @@ static int ptp_process_delay_resp(FAR struct ptp_state_s *state,
(long long)path_delay);
}

#ifdef CONFIG_NETUTILS_PTPD_GPTP_PROFILE
state->delayreq_interval = 0; // gPTP required value
#else
/* Calculate interval until next packet */

if (msg->header.logmessageinterval <= 12)
{
interval = (1 << msg->header.logmessageinterval);
Expand All @@ -1644,9 +1728,9 @@ 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;
}
Expand All @@ -1671,10 +1755,11 @@ static int ptp_process_rx_packet(FAR struct ptp_state_s *state,
}

clock_gettime(CLOCK_MONOTONIC, &state->last_received_multicast);
ESP_LOGI("proc_rx_packet", "rxtime: %lld.%09ld", (long long)state->rxtime.tv_sec, state->rxtime.tv_nsec);

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));
Expand All @@ -1696,7 +1781,7 @@ static int ptp_process_rx_packet(FAR struct ptp_state_s *state,
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:
ptpinfo("Got delay req, seq %ld\n",
(long)ptp_get_sequence(&state->rxbuf.header));
Expand Down Expand Up @@ -1945,7 +2030,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);

Expand Down
Loading

0 comments on commit 2c1ed44

Please sign in to comment.