Merge 104d9d5985eb72aef79eeb1b5cb9a92d0a888280 into 0461e2ff88369c3da0d4caced31e8488f53376cd

sending peer delay req, handle response/followup, and minor cleanup for consistency/readability; tested with Motu AVB switch and receiving syncs from switch which indicates that AVB is happy with peer delay

components/hal/emac_hal.c

@@ -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

examples/ethernet/ptp/components/ptpd/include/ptpd.h

@@ -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) */
 

examples/ethernet/ptp/components/ptpd/ptpd.c

@@ -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);
 

examples/ethernet/ptp/components/ptpd/ptpv2.h

@@ -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 */

examples/ethernet/ptp/sdkconfig.defaults

@@ -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