nuttx-apps/netutils/ptpd/ptpd.c

/****************************************************************************
 * apps/netutils/ptpd/ptpd.c
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.  The
 * ASF licenses this file to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance with the
 * License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
 * License for the specific language governing permissions and limitations
 * under the License.
 *
 ****************************************************************************/

/****************************************************************************
 * Included Files
 ****************************************************************************/

#include <nuttx/config.h>

#include <stdbool.h>
#include <stdint.h>

#include <sys/types.h>
#include <sys/socket.h>
#include <sys/time.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <sched.h>
#include <assert.h>
#include <errno.h>
#include <nuttx/debug.h>
#include <unistd.h>
#include <fcntl.h>

#include <netinet/in.h>
#include <netinet/if_ether.h>
#include <netpacket/packet.h>
#include <arpa/inet.h>
#include <netutils/ipmsfilter.h>

#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/poll.h>
#include <nuttx/clock.h>
#include <nuttx/net/netconfig.h>
#include <netutils/ptpd.h>

#include "netutils/netlib.h"
#include "ptpv2.h"

/****************************************************************************
 * Private Types
 ****************************************************************************/

/* Carrier structure for querying PTPD status */

struct ptpd_statusreq_s
{
  FAR sem_t *done;
  FAR struct ptpd_status_s *dest;
};

/* Main PTPD state storage */

struct ptp_state_s
{
  /* Request for PTPD task to stop or report status */

  bool stop;
  struct ptpd_statusreq_s status_req;

  /* Address of network interface we are operating on */

  struct sockaddr_in interface_addr;

  /* Socket bound to interface for transmission */

  int tx_socket;

  /* Sockets for PTP event and information ports */

  int event_socket;
  int info_socket;

  /* The ptp device file descriptor */

  clockid_t clockid;

  /* Our own identity as a clock source */

  struct ptp_announce_s own_identity;

  /* Sequence number counters per message type */

  uint16_t announce_seq;
  uint16_t sync_seq;
  uint16_t delay_req_seq;

  /* Previous measurement and estimated clock drift rate */

  struct timespec last_delta_timestamp;
  int64_t last_delta_ns;
  int64_t last_adjtime_ns;
  long drift_avg_total_ms;
  long drift_ppb;

  /* Identity of currently selected clock source,
   * from the latest announcement message.
   *
   * The timestamps are used for timeout when a source disappears.
   * They are from the local CLOCK_MONOTONIC.
   */

  bool selected_source_valid;              /* True if operating as client */
  struct ptp_announce_s selected_source;   /* Currently selected server */
  struct timespec last_received_multicast; /* Any multicast packet */
  struct timespec last_received_announce;  /* Announce from any server */
  struct timespec last_received_sync;      /* Sync from selected source */

  /* Last transmitted packet timestamps (CLOCK_MONOTONIC)
   * Used to set transmission interval.
   */

  struct timespec last_transmitted_sync;
  struct timespec last_transmitted_announce;
  struct timespec last_transmitted_delayresp;
  struct timespec last_transmitted_delayreq;

  /* Timestamps related to path delay calculation (CLOCK_REALTIME) */

  bool can_send_delayreq;
  struct timespec delayreq_time;
  int path_delay_avgcount;
  long path_delay_ns;
  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;

  uint8_t rxcmsg[CMSG_LEN(sizeof(struct timespec))];

  /* Buffered sync packet for two-step clock setting where server sends
   * the accurate timestamp in a separate follow-up message.
   */

  struct ptp_sync_s twostep_packet;
  struct timespec twostep_rxtime;
  FAR const struct ptpd_config_s *config;
};

/****************************************************************************
 * Private Functions
 ****************************************************************************/

/* Convert from timespec to PTP format */

static void timespec_to_ptp_format(FAR const struct timespec *ts,
                                   FAR uint8_t *timestamp)
{
  /* IEEE 1588 uses 48 bits for seconds and 32 bits for nanoseconds,
   * both fields big-endian.
   */

  timestamp[0] = (uint8_t)(ts->tv_sec >> 40);
  timestamp[1] = (uint8_t)(ts->tv_sec >> 32);
  timestamp[2] = (uint8_t)(ts->tv_sec >> 24);
  timestamp[3] = (uint8_t)(ts->tv_sec >> 16);
  timestamp[4] = (uint8_t)(ts->tv_sec >>  8);
  timestamp[5] = (uint8_t)(ts->tv_sec >>  0);

  timestamp[6] = (uint8_t)(ts->tv_nsec >>  24);
  timestamp[7] = (uint8_t)(ts->tv_nsec >>  16);
  timestamp[8] = (uint8_t)(ts->tv_nsec >>   8);
  timestamp[9] = (uint8_t)(ts->tv_nsec >>   0);
}

/* Convert from PTP format to timespec */

static void ptp_format_to_timespec(FAR const uint8_t *timestamp,
                                   FAR struct timespec *ts)
{
  ts->tv_sec =
      (((int64_t)timestamp[0]) << 40)
    | (((int64_t)timestamp[1]) << 32)
    | (((int64_t)timestamp[2]) << 24)
    | (((int64_t)timestamp[3]) << 16)
    | (((int64_t)timestamp[4]) <<  8)
    | (((int64_t)timestamp[5]) <<  0);

  ts->tv_nsec =
      (((long)timestamp[6]) << 24)
    | (((long)timestamp[7]) << 16)
    | (((long)timestamp[8]) <<  8)
    | (((long)timestamp[9]) <<  0);
}

/* Returns true if A is a better clock source than B.
 * Implements Best Master Clock algorithm from IEEE-1588.
 */

static bool is_better_clock(FAR const struct ptp_announce_s *a,
                            FAR const struct ptp_announce_s *b)
{
  /* Main priority field */

  if (a->gm_priority1 < b->gm_priority1)
    {
      return true;
    }

  if (a->gm_priority1 > b->gm_priority1)
    {
      return false;
    }

  /* Clock class */

  if (a->gm_quality[0] < b->gm_quality[0])
    {
      return true;
    }

  if (a->gm_quality[0] > b->gm_quality[0])
    {
      return false;
    }

  /* Clock accuracy */

  if (a->gm_quality[1] < b->gm_quality[1])
    {
      return true;
    }

  if (a->gm_quality[1] > b->gm_quality[1])
    {
      return false;
    }

  /* Clock variance high byte */

  if (a->gm_quality[2] < b->gm_quality[2])
    {
      return true;
    }

  if (a->gm_quality[2] > b->gm_quality[2])
    {
      return false;
    }

  /* Clock variance low byte */

  if (a->gm_quality[3] < b->gm_quality[3])
    {
      return true;
    }

  if (a->gm_quality[3] > b->gm_quality[3])
    {
      return false;
    }

  /* Sub priority field */

  if (a->gm_priority2 < b->gm_priority2)
    {
      return true;
    }

  if (a->gm_priority2 > b->gm_priority2)
    {
      return false;
    }

  return memcmp(a->gm_identity, b->gm_identity, sizeof(a->gm_identity)) < 0;
}

static int64_t timespec_to_ms(FAR const struct timespec *ts)
{
  return ts->tv_sec * MSEC_PER_SEC + (ts->tv_nsec / NSEC_PER_MSEC);
}

/* Get positive or negative delta between two timespec values.
 * If value would exceed int64 limit (292 years), return INT64_MAX/MIN.
 */

static int64_t timespec_delta_ns(FAR const struct timespec *ts1,
                                 FAR const struct timespec *ts2)
{
  int64_t delta_s;

  delta_s = ts1->tv_sec - ts2->tv_sec;

  /* Conversion to nanoseconds could overflow if the system time is 64-bit */

  if (delta_s >= INT64_MAX / NSEC_PER_SEC)
    {
      return INT64_MAX;
    }
  else if (delta_s <= INT64_MIN / NSEC_PER_SEC)
    {
      return INT64_MIN;
    }

  return delta_s * NSEC_PER_SEC + (ts1->tv_nsec - ts2->tv_nsec);
}

/* Check if the currently selected source is still valid */

static bool is_selected_source_valid(FAR struct ptp_state_s *state)
{
  struct timespec time_now;
  struct timespec delta;

  if ((state->selected_source.header.messagetype & PTP_MSGTYPE_MASK)
      != PTP_MSGTYPE_ANNOUNCE)
    {
      return false; /* Uninitialized value */
    }

  /* Note: this uses monotonic clock to track the timeout even when
   *       system clock is adjusted.
   */

  clock_gettime(CLOCK_MONOTONIC, &time_now);
  clock_timespec_subtract(&time_now, &state->last_received_sync, &delta);

  if (timespec_to_ms(&delta) > CONFIG_NETUTILS_PTPD_TIMEOUT_MS)
    {
      return false; /* Too long time since received packet */
    }

  return true;
}

/* Increment sequence number for packet type, and copy to header */

static void ptp_increment_sequence(FAR uint16_t *sequence_num,
                                   FAR struct ptp_header_s *hdr)
{
  *sequence_num += 1;
  hdr->sequenceid[0] = (uint8_t)(*sequence_num >> 8);
  hdr->sequenceid[1] = (uint8_t)(*sequence_num);
}

/* Get sequence number from received packet */

static uint16_t ptp_get_sequence(FAR const struct ptp_header_s *hdr)
{
  return ((uint16_t)hdr->sequenceid[0] << 8) | hdr->sequenceid[1];
}

static clockid_t ptp_open(FAR const char *clock)
{
  int fd;

  if (!strcmp(clock, "realtime"))
    {
      return CLOCK_REALTIME;
    }

  fd = open(clock, O_RDWR | O_CLOEXEC);
  if (fd < 0)
    {
      ptperr("Failed to open PTP clock device:%s, %d\n", clock, errno);
      return fd;
    }

  return (fd << CLOCK_SHIFT) | CLOCK_FD;
}

static void ptp_close(clockid_t clockid)
{
  if (clockid > 0 && clockid != CLOCK_REALTIME)
    {
      close(clockid >> CLOCK_SHIFT);
    }
}

static int ptp_gettime(FAR struct ptp_state_s *state,
                       FAR struct timespec *ts)
{
  return clock_gettime(state->clockid, ts);
}

/* Change current system timestamp by jumping */

static int ptp_settime(FAR struct ptp_state_s *state,
                       FAR struct timespec *ts)
{
  return clock_settime(state->clockid, ts);
}

/* Smoothly adjust timestamp. */

static int ptp_adjtime(FAR struct ptp_state_s *state, int64_t delta_ns,
                       int64_t ppb)
{
  if (state->clockid == CLOCK_REALTIME)
    {
      struct timeval delta;

      delta.tv_sec = delta_ns / NSEC_PER_SEC;
      delta_ns -= delta.tv_sec * NSEC_PER_SEC;
      delta.tv_usec = delta_ns / NSEC_PER_USEC;
      return adjtime(&delta, NULL);
    }
  else
    {
      struct timex buf;

      memset(&buf, 0, sizeof(buf));
      buf.freq = (long)(-ppb * 65536 / 1000);
      buf.modes = ADJ_FREQUENCY;

      return clock_adjtime(state->clockid, &buf);
    }
}

/* Get timestamp of latest received packet */

static int ptp_getrxtime(FAR struct ptp_state_s *state,
                         FAR struct msghdr *rxhdr,
                         FAR struct timespec *ts)
{
  FAR struct cmsghdr *cmsg;

  /* Get hardware or kernel timestamp if available */

  if (!state->config->hardware_ts)
    {
      return ptp_gettime(state, ts);
    }

  for_each_cmsghdr(cmsg, rxhdr)
    {
      if (cmsg->cmsg_level == SOL_SOCKET &&
          cmsg->cmsg_type == SO_TIMESTAMPNS &&
          cmsg->cmsg_len == CMSG_LEN(sizeof(struct timespec)))
        {
          memcpy(ts, CMSG_DATA(cmsg), sizeof(*ts));

          /* Sanity-check the value */

          if (ts->tv_sec > 0 || ts->tv_nsec > 0)
            {
              return OK;
            }
        }
    }

  ptpwarn("CONFIG_NET_TIMESTAMP enabled but did not get packet timestamp\n");
  return ERROR;
}

/* Unsubscribe multicast and destroy sockets */

static int ptp_destroy_state(FAR struct ptp_state_s *state)
{
  struct in_addr mcast_addr;

  ptp_close(state->clockid);

  mcast_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);
  ipmsfilter(&state->interface_addr.sin_addr,
             &mcast_addr, MCAST_EXCLUDE);

  if (state->tx_socket > 0)
    {
      close(state->tx_socket);
      state->tx_socket = -1;
    }

  if (state->event_socket > 0)
    {
      close(state->event_socket);
      state->event_socket = -1;
    }

  if (state->info_socket > 0)
    {
      close(state->info_socket);
      state->info_socket = -1;
    }

  return OK;
}

/* Initialize PTP client/server state and create sockets */

static int ptp_initialize_state(FAR struct ptp_state_s *state)
{
  int ret;
  int arg = 1;
  struct ifreq req;

  state->clockid = ptp_open(state->config->clock);
  if (state->clockid < 0)
    {
      ptperr("Invalid clockid %d for ptp daemon\n", state->clockid);
      return ERROR;
    }

  /* Create sockets */

  if (state->config->af == AF_PACKET)
    {
      struct sockaddr_ll addr;

      state->tx_socket = socket(AF_PACKET, SOCK_RAW, 0);
      if (state->tx_socket < 0)
        {
          ptperr("Failed to create tx socket: %d\n", errno);
          goto errout;
        }

      state->event_socket = dup(state->tx_socket);
      state->info_socket = -1;

      addr.sll_family = AF_PACKET;
      addr.sll_ifindex = if_nametoindex(state->config->interface);
      addr.sll_protocol = htons(ETHERTYPE_PTP);
      ret = bind(state->tx_socket, (FAR struct sockaddr *)&addr,
                 sizeof(addr));
      if (ret < 0)
        {
          ptperr("ERROR: binding socket failed: %d\n", errno);
          goto errout;
        }
    }
  else if (state->config->af == AF_INET)
    {
      struct sockaddr_in bind_addr;

      state->tx_socket = socket(AF_INET, SOCK_DGRAM, 0);
      if (state->tx_socket < 0)
        {
          ptperr("Failed to create tx socket: %d\n", errno);
          goto errout;
        }

      state->event_socket = socket(AF_INET, SOCK_DGRAM, 0);
      if (state->event_socket < 0)
        {
          ptperr("Failed to create event socket: %d\n", errno);
          goto errout;
        }

      state->info_socket = socket(AF_INET, SOCK_DGRAM, 0);
      if (state->info_socket < 0)
        {
          ptperr("Failed to create info socket: %d\n", errno);
          goto errout;
        }

      /* Subscribe to PTP multicast address */

      bind_addr.sin_family = AF_INET;
      bind_addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);

      ret = ipmsfilter(&state->interface_addr.sin_addr,
                       &bind_addr.sin_addr, MCAST_INCLUDE);
      if (ret < 0)
        {
          ptperr("Failed to bind multicast address: %d\n", errno);
          goto errout;
        }

      /* Bind socket for events */

      bind_addr.sin_port = HTONS(PTP_UDP_PORT_EVENT);
      ret = bind(state->event_socket, (FAR struct sockaddr *)&bind_addr,
                 sizeof(bind_addr));
      if (ret < 0)
        {
          ptperr("Failed to bind to udp port %d\n", bind_addr.sin_port);
          goto errout;
        }

      /* Bind socket for announcements */

      bind_addr.sin_port = HTONS(PTP_UDP_PORT_INFO);
      ret = bind(state->info_socket, (FAR struct sockaddr *)&bind_addr,
                 sizeof(bind_addr));
      if (ret < 0)
        {
          ptperr("Failed to bind to udp port %d\n", bind_addr.sin_port);
          goto errout;
        }

      /* Bind TX socket to interface address (local addr cannot be
       * multicast)
       */

      bind_addr.sin_addr = state->interface_addr.sin_addr;
      ret = bind(state->tx_socket, (FAR struct sockaddr *)&bind_addr,
                 sizeof(bind_addr));
      if (ret < 0)
        {
          ptperr("Failed to bind tx to port %d\n", bind_addr.sin_port);
          goto errout;
        }
    }

  if (state->config->hardware_ts)
    {
      ret = setsockopt(state->event_socket, SOL_SOCKET, SO_TIMESTAMPNS,
                       &arg, sizeof(arg));

      if (ret < 0)
        {
          ptperr("Failed to enable SO_TIMESTAMPNS: %s\n", strerror(errno));
          goto errout;
        }
    }

  /* Get address information of the specified interface for binding socket
   * Only supports IPv4 currently.
   */

  memset(&req, 0, sizeof(req));
  strlcpy(req.ifr_name, state->config->interface, sizeof(req.ifr_name));

  if (ioctl(state->event_socket, SIOCGIFADDR, (unsigned long)&req) < 0)
    {
      ptperr("Failed to get IP address information for interface %s\n",
             state->config->interface);
      goto errout;
    }

  state->interface_addr = *(FAR struct sockaddr_in *)&req.ifr_ifru.ifru_addr;

  /* Get hardware address to initialize the identity field in header.
   * Clock identity is EUI-64, which we make from EUI-48.
   */

  if (ioctl(state->event_socket, SIOCGIFHWADDR, (unsigned long)&req) < 0)
    {
      ptperr("Failed to get HW address information for interface %s\n",
             state->config->interface);
      goto errout;
    }

  state->own_identity.header.version = 2;
  state->own_identity.header.domain = CONFIG_NETUTILS_PTPD_DOMAIN;
  state->own_identity.header.sourceidentity[0] = req.ifr_hwaddr.sa_data[0];
  state->own_identity.header.sourceidentity[1] = req.ifr_hwaddr.sa_data[1];
  state->own_identity.header.sourceidentity[2] = req.ifr_hwaddr.sa_data[2];
  state->own_identity.header.sourceidentity[3] = 0xff;
  state->own_identity.header.sourceidentity[4] = 0xfe;
  state->own_identity.header.sourceidentity[5] = req.ifr_hwaddr.sa_data[3];
  state->own_identity.header.sourceidentity[6] = req.ifr_hwaddr.sa_data[4];
  state->own_identity.header.sourceidentity[7] = req.ifr_hwaddr.sa_data[5];
  state->own_identity.header.sourceportindex[0] = 0;
  state->own_identity.header.sourceportindex[1] = 1;
  state->own_identity.gm_priority1 = CONFIG_NETUTILS_PTPD_PRIORITY1;
  state->own_identity.gm_quality[0] = CONFIG_NETUTILS_PTPD_CLASS;
  state->own_identity.gm_quality[1] = CONFIG_NETUTILS_PTPD_ACCURACY;
  state->own_identity.gm_quality[2] = 0xff; /* No variance estimate */
  state->own_identity.gm_quality[3] = 0xff;
  state->own_identity.gm_priority2 = CONFIG_NETUTILS_PTPD_PRIORITY2;
  memcpy(state->own_identity.gm_identity,
         state->own_identity.header.sourceidentity,
         sizeof(state->own_identity.gm_identity));
  state->own_identity.timesource = CONFIG_NETUTILS_PTPD_CLOCKSOURCE;

  clock_gettime(CLOCK_MONOTONIC, &state->last_received_multicast);

  return OK;

errout:
  ptp_destroy_state(state);
  return ERROR;
}

/* Re-subscribe multicast address.
 * This can become necessary if Ethernet interface gets reset or if external
 * IGMP-compliant Ethernet switch gets plugged in.
 */

static int ptp_check_multicast_status(FAR struct ptp_state_s *state)
{
#if CONFIG_NETUTILS_PTPD_MULTICAST_TIMEOUT_MS > 0
  struct in_addr mcast_addr;
  struct timespec time_now;
  struct timespec delta;

  clock_gettime(CLOCK_MONOTONIC, &time_now);
  clock_timespec_subtract(&time_now, &state->last_received_multicast,
                          &delta);

  if (timespec_to_ms(&delta) > CONFIG_NETUTILS_PTPD_MULTICAST_TIMEOUT_MS)
    {
      /* Remove and re-add the multicast group */

      state->last_received_multicast = time_now;

      mcast_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);
      ipmsfilter(&state->interface_addr.sin_addr,
                 &mcast_addr,
                 MCAST_EXCLUDE);

      return ipmsfilter(&state->interface_addr.sin_addr,
                        &mcast_addr,
                        MCAST_INCLUDE);
    }

#else
  UNUSED(state);
#endif /* CONFIG_NETUTILS_PTPD_MULTICAST_TIMEOUT_MS */

  return OK;
}

static int ptp_sendmsg(FAR struct ptp_state_s *state, FAR const void *buf,
                       size_t buflen, FAR const void *addr,
                       socklen_t addrlen, FAR struct timespec *sendts)
{
  int ret;

  if (state->config->af == AF_PACKET)
    {
      /* IEE802.1AS Multicast address for gptp */

      const uint8_t ptp_multicast_mac[ETHER_ADDR_LEN] =
      {
        0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e
      };

      char raw[sizeof(struct ether_header) + sizeof(struct ptp_announce_s)];
      FAR struct ether_header *header;
      struct msghdr msg;
      struct iovec iov;

      DEBUGASSERT(sizeof(struct ptp_announce_s) >= buflen);

      header = (FAR struct ether_header *)&raw;
      memcpy(header->ether_dhost, ptp_multicast_mac, ETHER_ADDR_LEN);
      netlib_getmacaddr(state->config->interface, header->ether_shost);
      header->ether_type = ETHERTYPE_PTP;
      memcpy(&raw[sizeof(*header)], buf, buflen);
      buflen += sizeof(*header);

      iov.iov_base = raw;
      iov.iov_len = buflen;

      msg.msg_name = (FAR void *)addr;
      msg.msg_namelen = addrlen;
      msg.msg_iov = &iov;
      msg.msg_iovlen = 1;
      msg.msg_flags = 0;
      msg.msg_control = NULL;
      msg.msg_controllen = 0;

      ret = sendmsg(state->tx_socket, &msg, 0);
      if (ret < 0)
        {
          return ERROR;
        }

      if (state->config->hardware_ts && sendts != NULL)
        {
          uint8_t rxcmsg[CMSG_LEN(sizeof(struct timespec))];

          msg.msg_control = &rxcmsg;
          msg.msg_controllen = CMSG_LEN(sizeof(struct timespec));
          ret = recvmsg(state->tx_socket, &msg, 0);
          if (ret >= 0)
            {
              ptp_getrxtime(state, &msg, sendts);
            }
        }
    }
  else
    {
      ret = sendto(state->tx_socket, buf, buflen, 0, addr, addrlen);
    }

  if (!state->config->hardware_ts && sendts != NULL)
    {
      ptp_gettime(state, sendts);
    }

  return ret;
}

/* Send PTP server announcement packet */

static int ptp_send_announce(FAR struct ptp_state_s *state)
{
  struct ptp_announce_s msg;
  struct sockaddr_in addr;
  struct timespec ts;
  int ret;

  addr.sin_family      = AF_INET;
  addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);
  addr.sin_port        = HTONS(PTP_UDP_PORT_INFO);

  memset(&msg, 0, sizeof(msg));
  msg = state->own_identity;
  msg.header.messagetype = PTP_MSGTYPE_ANNOUNCE;
  msg.header.messagelength[1] = sizeof(msg);

  ptp_increment_sequence(&state->announce_seq, &msg.header);
  ptp_gettime(state, &ts);
  timespec_to_ptp_format(&ts, msg.origintimestamp);

  ret = ptp_sendmsg(state, &msg, sizeof(msg), &addr, sizeof(addr), NULL);
  if (ret < 0)
    {
      ptperr("ptp sendmsg failed: %d", errno);
    }
  else
    {
      ptpinfo("Sent announce, seq %ld\n",
              (long)ptp_get_sequence(&msg.header));
    }

  return ret;
}

/* Send PTP server synchronization packet */

static int ptp_send_sync(FAR struct ptp_state_s *state)
{
  struct ptp_sync_s msg;
  struct sockaddr_in addr;
  struct timespec ts;
  int ret;

  addr.sin_family      = AF_INET;
  addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);
  addr.sin_port        = HTONS(PTP_UDP_PORT_EVENT);

  memset(&msg, 0, sizeof(msg));
  msg.header = state->own_identity.header;
  msg.header.messagetype = PTP_MSGTYPE_SYNC;
  msg.header.messagelength[1] = sizeof(msg);

#ifdef CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC
  msg.header.flags[0] = PTP_FLAGS0_TWOSTEP;
#endif

  /* Timestamp and send the sync message */

  ptp_increment_sequence(&state->sync_seq, &msg.header);
  ptp_gettime(state, &ts);
  timespec_to_ptp_format(&ts, msg.origintimestamp);

  ret = ptp_sendmsg(state, &msg, sizeof(msg), &addr, sizeof(addr), &ts);
  if (ret < 0)
    {
      ptperr("sendmsg for sync message failed: %d\n", errno);
      return ret;
    }

#ifdef CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC

  timespec_to_ptp_format(&ts, msg.origintimestamp);
  msg.header.messagetype = PTP_MSGTYPE_FOLLOW_UP;
  msg.header.flags[0] = 0;
  addr.sin_port = HTONS(PTP_UDP_PORT_INFO);

  ret = ptp_sendmsg(state, &msg, sizeof(msg), &addr, sizeof(addr), NULL);
  if (ret < 0)
    {
      ptperr("ptp sendmsg for follow-up message failed: %d\n", errno);
      return ret;
    }

  ptpinfo("Sent sync + follow-up, seq %ld\n",
          (long)ptp_get_sequence(&msg.header));
#else
  ptpinfo("Sent sync, seq %ld\n",
          (long)ptp_get_sequence(&msg.header));
#endif /* CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC */

  return OK;
}

/* Send delay request packet to selected source */

static int ptp_send_delay_req(FAR struct ptp_state_s *state)
{
  struct ptp_delay_req_s req;
  struct sockaddr_in addr;
  int ret;

  addr.sin_family      = AF_INET;
  addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);
  addr.sin_port        = HTONS(PTP_UDP_PORT_EVENT);

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

  ptp_gettime(state, &state->delayreq_time);
  timespec_to_ptp_format(&state->delayreq_time, req.origintimestamp);

  ret = ptp_sendmsg(state, &req, sizeof(req),
                    &addr, sizeof(addr), &state->delayreq_time);
  if (ret < 0)
    {
      ptperr("ptp sendmsg failed: %d", errno);
    }
  else
    {
      clock_gettime(CLOCK_MONOTONIC, &state->last_transmitted_delayreq);
      ptpinfo("Sent delay req, seq %ld\n",
              (long)ptp_get_sequence(&req.header));
    }

  return ret;
}

/* Check if we need to send packets */

static int ptp_periodic_send(FAR struct ptp_state_s *state)
{
  /* If there is no better master clock on the network,
   * act as the reference source and send server packets.
   */

  if (!state->config->client_only && !state->selected_source_valid)
    {
      struct timespec time_now;
      struct timespec delta;

      clock_gettime(CLOCK_MONOTONIC, &time_now);
      clock_timespec_subtract(&time_now,
        &state->last_transmitted_announce, &delta);
      if (state->config->bmca && timespec_to_ms(&delta)
          > CONFIG_NETUTILS_PTPD_ANNOUNCE_INTERVAL_MSEC)
        {
          state->last_transmitted_announce = time_now;
          ptp_send_announce(state);
        }

      clock_timespec_subtract(&time_now,
        &state->last_transmitted_sync, &delta);
      if (timespec_to_ms(&delta) > CONFIG_NETUTILS_PTPD_SYNC_INTERVAL_MSEC)
        {
          state->last_transmitted_sync = time_now;
          ptp_send_sync(state);
        }
    }

  if (state->config->delay_e2e && state->selected_source_valid &&
      state->can_send_delayreq)
    {
      struct timespec time_now;
      struct timespec delta;

      clock_gettime(CLOCK_MONOTONIC, &time_now);
      clock_timespec_subtract(&time_now,
                              &state->last_transmitted_delayreq, &delta);

      if (timespec_to_ms(&delta) > state->delayreq_interval * MSEC_PER_SEC)
        {
          ptp_send_delay_req(state);
        }
    }

  return OK;
}

/* Process received PTP announcement */

static int ptp_process_announce(FAR struct ptp_state_s *state,
                                FAR struct ptp_announce_s *msg)
{
  clock_gettime(CLOCK_MONOTONIC, &state->last_received_announce);

  if (state->conifg->bmca && is_better_clock(msg, &state->n_identity))
    {
      if (!state->selected_source_valid ||
          is_better_clock(msg, &state->selected_source))
        {
          ptpinfo("Switching to better PTP time source\n");

          state->selected_source = *msg;
          state->last_received_sync = state->last_received_announce;
          state->path_delay_avgcount = 0;
          state->path_delay_ns = 0;
          state->delayreq_time.tv_sec = 0;
        }
    }

  return OK;
}

/* Update local clock either by smooth adjustment or by jumping.
 * Remote time was remote_timestamp at local_timestamp.
 */

static int ptp_update_local_clock(FAR struct ptp_state_s *state,
                                  FAR struct timespec *remote_timestamp,
                                  FAR struct timespec *local_timestamp)
{
  int ret;
  int64_t delta_ns;
  int64_t absdelta_ns;
  const int64_t adj_limit_ns = CONFIG_NETUTILS_PTPD_SETTIME_THRESHOLD_MS
                               * (int64_t)NSEC_PER_MSEC;

  ptpinfo("Local time: %jd.%09ld, remote time %jd.%09ld\n",
          (intmax_t)local_timestamp->tv_sec,
          local_timestamp->tv_nsec,
          (intmax_t)remote_timestamp->tv_sec,
          remote_timestamp->tv_nsec);

  delta_ns = timespec_delta_ns(remote_timestamp, local_timestamp);
  delta_ns += state->path_delay_ns;
  absdelta_ns = (delta_ns < 0) ? -delta_ns : delta_ns;

  if (absdelta_ns > adj_limit_ns)
    {
      /* Large difference, move by jumping.
       * Account for delay since packet was received.
       */

      struct timespec new_time;
      ptp_gettime(state, &new_time);
      clock_timespec_subtract(&new_time, local_timestamp, &new_time);
      clock_timespec_add(&new_time, remote_timestamp, &new_time);
      ret = ptp_settime(state, &new_time);

      /* Reinitialize drift adjustment parameters */

      state->last_delta_timestamp = new_time;
      state->last_delta_ns = 0;
      state->last_adjtime_ns = 0;
      state->drift_avg_total_ms = 0;
      state->drift_ppb = 0;

      if (ret == OK)
        {
          ptpinfo("Jumped to timestamp %jd.%09ld s\n",
                  (intmax_t)new_time.tv_sec, new_time.tv_nsec);
        }
      else
        {
          ptperr("ptp_settime() failed: %d\n", errno);
        }
    }
  else
    {
      /* Track drift rate based on two consecutive measurements and
       * the adjustment that was made previously.
       */

      int64_t drift_ppb;
      struct timespec interval;
      int interval_ms;
      int max_avg_period_ms;
      int64_t adjustment_ns;

      clock_timespec_subtract(local_timestamp,
                              &state->last_delta_timestamp,
                              &interval);
      interval_ms = timespec_to_ms(&interval);

      if (interval_ms > 0 && interval_ms < CONFIG_NETUTILS_PTPD_TIMEOUT_MS)
        {
          drift_ppb = (delta_ns - state->last_delta_ns) * MSEC_PER_SEC
                      / interval_ms;
        }
      else
        {
          ptpwarn("Measurement interval out of range: %d ms\n", interval_ms);
          drift_ppb = 0;
          interval_ms = 1;
        }

      /* Account for the adjustment previously made */

      drift_ppb += state->last_adjtime_ns * MSEC_PER_SEC
                  / CONFIG_CLOCK_ADJTIME_PERIOD_MS;

      if (drift_ppb > CONFIG_CLOCK_ADJTIME_SLEWLIMIT_PPM * 1000 ||
          drift_ppb < -CONFIG_CLOCK_ADJTIME_SLEWLIMIT_PPM * 1000)
        {
          ptpwarn("Drift estimate out of range: %lld\n",
                  (long long)drift_ppb);
          drift_ppb = state->drift_ppb;
        }

      /* Take direct average of drift estimate for first measurements,
       * after that update the exponential sliding average.
       * Measurements are weighted according to the interval, because
       * drift estimate is more accurate over longer timespan.
       */

      state->drift_avg_total_ms += interval_ms;
      max_avg_period_ms = CONFIG_NETUTILS_PTPD_DRIFT_AVERAGE_S
                          * MSEC_PER_SEC;
      if (state->drift_avg_total_ms > max_avg_period_ms)
        {
          state->drift_avg_total_ms = max_avg_period_ms;
        }

      state->drift_ppb += (drift_ppb - state->drift_ppb) * interval_ms
                        / state->drift_avg_total_ms;

      /* Compute the value we need to give to adjtime() to match the
       * drift rate.
       */

      adjustment_ns = state->drift_ppb * CONFIG_CLOCK_ADJTIME_PERIOD_MS
                      / MSEC_PER_SEC;

      /* Drift estimation ensures local clock runs at same rate as remote.
       *
       * Adding the current clock offset to adjustment brings the clocks
       * to match. To avoid individual outliers from causing jitter, we
       * take the larger signed value of two previous deltas. This is based
       * on the logic that packets can get delayed in transit, but do not
       * travel backwards in time.
       *
       * Clock offset is applied over ADJTIME_PERIOD. If there is significant
       * noise in measurements, increasing ADJTIME_PERIOD will reduce its
       * effect on the local clock run rate.
       */

      if (state->last_delta_ns > delta_ns)
        {
          adjustment_ns += state->last_delta_ns;
        }
      else
        {
          adjustment_ns += delta_ns;
        }

      /* Apply adjustment and store information for next time */

      state->last_delta_ns = delta_ns;
      state->last_delta_timestamp = *local_timestamp;
      state->last_adjtime_ns = adjustment_ns;

      ptpinfo("Delta: %+lld ns, adjustment %+lld ns, drift rate %+lld ppb\n",
              (long long)delta_ns,
              (long long)state->last_adjtime_ns,
              (long long)state->drift_ppb);

      if (absdelta_ns > CONFIG_NETUTILS_PTPD_ADJTIME_THRESHOLD_NS)
        {
          ret = ptp_adjtime(state, delta_ns, drift_ppb);
        }
      else
        {
          ret = ptp_adjtime(state, adjustment_ns, state->drift_ppb);
        }

      if (ret != OK)
        {
          ptperr("ptp_adjtime() failed: %d\n", errno);
        }

      /* Check if clock is stable enough for sending delay requests */

      if (absdelta_ns < CONFIG_NETUTILS_PTPD_MAX_PATH_DELAY_NS)
        {
          state->can_send_delayreq = true;
        }
    }

  return ret;
}

/* Process received PTP sync packet */

static int ptp_process_sync(FAR struct ptp_state_s *state,
                            FAR struct ptp_sync_s *msg)
{
  struct timespec remote_time;

  if (state->config->bmca &&
      memcmp(msg->header.sourceidentity,
             state->selected_source.header.sourceidentity,
             sizeof(msg->header.sourceidentity)) != 0)
    {
      /* This packet wasn't from the currently selected source */

      return OK;
    }

  /* Update timeout tracking */

  clock_gettime(CLOCK_MONOTONIC, &state->last_received_sync);

  if (msg->header.flags[0] & PTP_FLAGS0_TWOSTEP)
    {
      /* We need to wait for a follow-up packet before setting the clock. */

      state->twostep_rxtime = state->rxtime;
      state->twostep_packet = *msg;
      ptpinfo("Waiting for follow-up\n");
      return OK;
    }

  /* Update local clock */

  ptp_format_to_timespec(msg->origintimestamp, &remote_time);
  return ptp_update_local_clock(state, &remote_time, &state->rxtime);
}

static void ptp_add_correction_time(FAR const uint8_t *correction,
                                    FAR struct timespec *ts)
{
  uint64_t correction_time = (((uint64_t)correction[0]) << 40)
                           | (((uint64_t)correction[1]) << 32)
                           | (((uint64_t)correction[2]) << 24)
                           | (((uint64_t)correction[3]) << 16)
                           | (((uint64_t)correction[4]) <<  8)
                           | (((uint64_t)correction[5]) <<  0);

  ptpinfo("correction before: %jd.%09ld\n", (intmax_t)ts->tv_sec,
          ts->tv_nsec);

  ts->tv_sec  += correction_time / NSEC_PER_SEC;
  ts->tv_nsec += correction_time % NSEC_PER_SEC;
  if (ts->tv_nsec >= NSEC_PER_SEC)
    {
      ts->tv_nsec -= NSEC_PER_SEC;
      ts->tv_sec  += 1;
    }

  ptpinfo("correction after: %jd.%09ld\n", (intmax_t)ts->tv_sec,
          ts->tv_nsec);
}

static int ptp_process_followup(FAR struct ptp_state_s *state,
                                FAR struct ptp_follow_up_s *msg)
{
  struct timespec remote_time;

  if (state->config->bmca &&
      memcmp(msg->header.sourceidentity,
             state->twostep_packet.header.sourceidentity,
             sizeof(msg->header.sourceidentity)) != 0)
    {
      return OK; /* This packet wasn't from the currently selected source */
    }

  if (ptp_get_sequence(&msg->header)
      != ptp_get_sequence(&state->twostep_packet.header))
    {
      ptpwarn("PTP 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_packet.header));
      return OK;
    }

  /* Update local clock based on the remote timestamp we received now
   * and the local timestamp of when the sync packet was received.
   */

  ptp_format_to_timespec(msg->origintimestamp, &remote_time);

  /* add correction time */

  ptp_add_correction_time(msg->header.correction, &remote_time);

  /* done */

  return ptp_update_local_clock(state, &remote_time, &state->twostep_rxtime);
}

static int ptp_process_delay_req(FAR struct ptp_state_s *state,
                                 FAR struct ptp_delay_req_s *msg)
{
  struct ptp_delay_resp_s resp;
  struct sockaddr_in addr;
  int ret;

  if (state->selected_source_valid)
    {
      /* We are operating as a client, ignore delay requests */

      return OK;
    }

  addr.sin_family      = AF_INET;
  addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);
  addr.sin_port        = HTONS(PTP_UDP_PORT_INFO);

  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;

  ret = ptp_sendmsg(state, &resp, sizeof(resp), &addr, sizeof(addr), NULL);
  if (ret < 0)
    {
      ptperr("ptp sendmsg 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;
}

static int ptp_process_delay_resp(FAR struct ptp_state_s *state,
                                  FAR struct ptp_delay_resp_s *msg)
{
  int64_t path_delay;
  int64_t sync_delay;
  struct timespec remote_rxtime;
  uint16_t sequence;
  int interval;

  if (!state->selected_source_valid ||
      memcmp(msg->header.sourceidentity,
             state->selected_source.header.sourceidentity,
             sizeof(msg->header.sourceidentity)) != 0 ||
      memcmp(msg->reqidentity,
             state->own_identity.header.sourceidentity,
             sizeof(msg->reqidentity)) != 0)
    {
      return OK; /* This packet wasn't for us */
    }

  sequence = ptp_get_sequence(&msg->header);

  if (sequence != state->delay_req_seq)
    {
      ptpwarn("Ignoring out-of-sequence delay resp (%d vs. expected %d)\n",
              (int)sequence, (int)state->delay_req_seq);
      return OK;
    }

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

  ptp_format_to_timespec(msg->receivetimestamp, &remote_rxtime);
  path_delay = timespec_delta_ns(&remote_rxtime, &state->delayreq_time);
  sync_delay = state->path_delay_ns - state->last_delta_ns;
  path_delay = (path_delay + sync_delay) / 2;

  if (path_delay >= 0 && path_delay < CONFIG_NETUTILS_PTPD_MAX_PATH_DELAY_NS)
    {
      if (state->path_delay_avgcount <
          CONFIG_NETUTILS_PTPD_DELAYREQ_AVGCOUNT)
        {
          state->path_delay_avgcount++;
        }

      state->path_delay_ns += (path_delay - state->path_delay_ns)
                              / state->path_delay_avgcount;

      ptpinfo("Path delay: %ld ns (avg: %ld ns)\n",
        (long)path_delay, (long)state->path_delay_ns);
    }
  else
    {
      ptpwarn("Path delay out of range: %lld ns\n",
              (long long)path_delay);
    }

  /* Calculate interval until next packet */

  if (msg->header.logmessageinterval <= 12)
    {
      interval = (1 << msg->header.logmessageinterval);
    }
  else
    {
      interval = 4096; /* Refuse to obey excessively long intervals */
    }

  /* Randomize up to 2x nominal delay) */

  state->delayreq_interval = interval + (random() % interval);

  return OK;
}

/* Determine received packet type and process it */

static int ptp_process_rx_packet(FAR struct ptp_state_s *state,
                                 ssize_t length)
{
  if (state->config->af == AF_PACKET)
    {
      /* Remove the header of ether message */

      FAR struct ethhdr *header = (FAR struct ethhdr *)state->rxbuf.raw;

      if (htons(header->h_proto) != ETHERTYPE_PTP)
        {
          return -EINVAL;
        }

      length -= sizeof(*header);
      memmove(&state->rxbuf.raw, header + 1, length);
    }

  if (length < sizeof(struct ptp_header_s))
    {
      ptpwarn("Ignoring invalid PTP packet, length only %d bytes\n",
              (int)length);
      return OK;
    }

  if (state->rxbuf.header.domain != CONFIG_NETUTILS_PTPD_DOMAIN)
    {
      /* Part of different clock domain, ignore */

      return OK;
    }

  clock_gettime(CLOCK_MONOTONIC, &state->last_received_multicast);

  switch (state->rxbuf.header.messagetype & PTP_MSGTYPE_MASK)
    {
    case PTP_MSGTYPE_ANNOUNCE:
      ptpinfo("Got announce packet, seq %ld\n",
              (long)ptp_get_sequence(&state->rxbuf.header));
      return ptp_process_announce(state, &state->rxbuf.announce);

    case PTP_MSGTYPE_SYNC:
      ptpinfo("Got sync packet, seq %ld\n",
              (long)ptp_get_sequence(&state->rxbuf.header));
      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));
      return ptp_process_followup(state, &state->rxbuf.follow_up);

    case PTP_MSGTYPE_DELAY_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);

    case PTP_MSGTYPE_DELAY_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);

    default:
      ptpinfo("Ignoring unknown PTP packet type: 0x%02x\n",
              state->rxbuf.header.messagetype);
      return OK;
    }
}

/* Signal handler for status / stop requests */

static void ptp_signal_handler(int signo, FAR siginfo_t *siginfo,
                               FAR void *context)
{
  FAR struct ptp_state_s *state = (FAR struct ptp_state_s *)siginfo->si_user;

  if (signo == SIGHUP)
    {
      state->stop = true;
    }
  else if (signo == SIGUSR1 && siginfo->si_value.sival_ptr)
    {
      state->status_req =
        *(FAR struct ptpd_statusreq_s *)siginfo->si_value.sival_ptr;
    }
}

static void ptp_setup_sighandlers(FAR struct ptp_state_s *state)
{
  struct sigaction act;

  act.sa_sigaction = ptp_signal_handler;
  sigfillset(&act.sa_mask);
  act.sa_flags = SA_SIGINFO;
  act.sa_user = state;

  sigaction(SIGHUP, &act, NULL);
  sigaction(SIGUSR1, &act, NULL);
}

/* Process status information request */

static void ptp_process_statusreq(FAR struct ptp_state_s *state)
{
  FAR struct ptpd_status_s *status;

  if (!state->status_req.dest)
    {
      return; /* No active request */
    }

  status = state->status_req.dest;
  status->clock_source_valid = state->selected_source_valid;

  if (status->clock_source_valid)
    {
      /* Copy relevant parts of announce info to status struct */

      FAR struct ptp_announce_s *s = &state->selected_source;

      memcpy(status->clock_source_info.id,
             s->header.sourceidentity,
             sizeof(status->clock_source_info.id));

      status->clock_source_info.utcoffset =
          (int16_t)(((uint16_t)s->utcoffset[0] << 8) | s->utcoffset[1]);
      status->clock_source_info.priority1 = s->gm_priority1;
      status->clock_source_info.clockclass = s->gm_quality[0];
      status->clock_source_info.accuracy = s->gm_quality[1];
      status->clock_source_info.priority2 = s->gm_priority2;
      status->clock_source_info.variance =
          ((uint16_t)s->gm_quality[2] << 8) | s->gm_quality[3];

      memcpy(status->clock_source_info.gm_id,
             s->gm_identity,
             sizeof(status->clock_source_info.gm_id));

      status->clock_source_info.stepsremoved =
          ((uint16_t)s->stepsremoved[0] << 8) | s->stepsremoved[1];
      status->clock_source_info.timesource = s->timesource;
    }

  /* Copy latest adjustment info */

  status->last_clock_update = state->last_delta_timestamp;
  status->last_delta_ns     = state->last_delta_ns;
  status->last_adjtime_ns   = state->last_adjtime_ns;
  status->drift_ppb         = state->drift_ppb;
  status->path_delay_ns     = state->path_delay_ns;

  /* Copy timestamps */

  status->last_received_multicast    = state->last_received_multicast;
  status->last_received_announce     = state->last_received_announce;
  status->last_received_sync         = state->last_received_sync;
  status->last_transmitted_sync      = state->last_transmitted_sync;
  status->last_transmitted_announce  = state->last_transmitted_announce;
  status->last_transmitted_delayresp = state->last_transmitted_delayresp;
  status->last_transmitted_delayreq  = state->last_transmitted_delayreq;

  /* Post semaphore to inform that we are done */

  if (state->status_req.done)
    {
      sem_post(state->status_req.done);
    }

  state->status_req.done = NULL;
  state->status_req.dest = NULL;
}

/****************************************************************************
 * Public Functions
 ****************************************************************************/

/****************************************************************************
 * Name: ptpd_start
 *
 * Description:
 *   Start the PTP daemon and bind it to specified interface.
 *
 * Input Parameters:
 *   interface - Name of the network interface to bind to, e.g. "eth0"
 *
 * Returned Value:
 *   On success, the non-negative task ID of the PTP daemon is returned;
 *   On failure, a negated errno value is returned.
 *
 ****************************************************************************/

int ptpd_start(FAR const struct ptpd_config_s *config)
{
  FAR struct ptp_state_s *state;
  struct pollfd pollfds[2];
  struct msghdr rxhdr;
  struct iovec rxiov;
  int timeout;
  int idx = 1;
  int ret;

  memset(&rxhdr, 0, sizeof(rxhdr));
  memset(&rxiov, 0, sizeof(rxiov));

  state = calloc(1, sizeof(struct ptp_state_s));
  if (state == NULL)
    {
      return -ENOMEM;
    }

  state->config = config;
  if (ptp_initialize_state(state) != OK)
    {
      ptperr("Failed to initialize PTP state, exiting\n");
      goto errout;
    }

  if (config->client_only)
    {
      timeout = CONFIG_NETUTILS_PTPD_TIMEOUT_MS;
    }
  else
    {
      timeout = CONFIG_NETUTILS_PTPD_SYNC_INTERVAL_MSEC;
    }

  ptp_setup_sighandlers(state);

  pollfds[0].events = POLLIN;
  pollfds[0].fd = state->event_socket;
  if (state->info_socket > 0)
    {
      pollfds[1].events = POLLIN;
      pollfds[1].fd = state->info_socket;
      idx++;
    }

  while (!state->stop)
    {
      state->can_send_delayreq = false;

      rxhdr.msg_name = NULL;
      rxhdr.msg_namelen = 0;
      rxhdr.msg_iov = &rxiov;
      rxhdr.msg_iovlen = 1;
      rxhdr.msg_control = &state->rxcmsg;
      rxhdr.msg_controllen = sizeof(state->rxcmsg);
      rxhdr.msg_flags = 0;
      rxiov.iov_base = &state->rxbuf;
      rxiov.iov_len = sizeof(state->rxbuf);

      pollfds[0].revents = 0;
      pollfds[1].revents = 0;
      ret = poll(pollfds, idx, timeout);

      if (pollfds[0].revents)
        {
          /* Receive time-critical packet, potentially with cmsg
           * indicating the timestamp.
           */

          ret = recvmsg(state->event_socket, &rxhdr, MSG_DONTWAIT);
          if (ret > 0)
            {
              ptp_getrxtime(state, &rxhdr, &state->rxtime);
              ptp_process_rx_packet(state, ret);
            }
        }

      if (pollfds[1].revents)
        {
          /* Receive non-time-critical packet. */

          ret = recv(state->info_socket, &state->rxbuf, sizeof(state->rxbuf),
                     MSG_DONTWAIT);
          if (ret > 0)
            {
              ptp_process_rx_packet(state, ret);
            }
        }

      if (pollfds[0].revents == 0 && pollfds[1].revents == 0)
        {
          /* No packets received, check for multicast timeout */

          ptp_check_multicast_status(state);
        }

      ptp_periodic_send(state);

      state->selected_source_valid = is_selected_source_valid(state);
      ptp_process_statusreq(state);
    }

errout:
  ptp_destroy_state(state);
  free(state);

  return 0;
}

/****************************************************************************
 * Name: ptpd_status
 *
 * Description:
 *   Query status from a running PTP daemon.
 *
 * Input Parameters:
 *   pid     - Process ID previously returned by ptpd_start()
 *   status  - Pointer to storage for status information.
 *
 * Returned Value:
 *   On success, returns OK.
 *   On failure, a negated errno value is returned.
 *
 * Assumptions/Limitations:
 *   Multiple threads with priority less than CONFIG_NETUTILS_PTPD_SERVERPRIO
 *   can request status simultaneously. If higher priority threads request
 *   status simultaneously, some of the requests may timeout.
 *
 ****************************************************************************/

int ptpd_status(int pid, FAR struct ptpd_status_s *status)
{
#ifndef CONFIG_BUILD_FLAT

  /* TODO: Use SHM memory to pass the status information if processes
   * do not share the same memory space.
   */

  return -ENOTSUP;

#else

  int ret = OK;
  sem_t donesem;
  struct ptpd_statusreq_s req;
  union sigval val;
  struct timespec timeout;

  /* Fill in the status request */

  memset(status, 0, sizeof(struct ptpd_status_s));
  sem_init(&donesem, 0, 0);
  req.done = &donesem;
  req.dest = status;
  val.sival_ptr = &req;

  if (sigqueue(pid, SIGUSR1, val) != OK)
    {
      return -errno;
    }

  /* Wait for status request to be handled */

  clock_gettime(CLOCK_MONOTONIC, &timeout);
  timeout.tv_sec += 1;
  if (sem_clockwait(&donesem, CLOCK_MONOTONIC, &timeout) != 0)
    {
      ret = -errno;
    }

  sem_destroy(&donesem);
  return ret;

#endif /* CONFIG_BUILD_FLAT */
}

/****************************************************************************
 * Name: ptpd_stop
 *
 * Description:
 *   Stop PTP daemon
 *
 * Input Parameters:
 *   pid     - Process ID previously returned by ptpd_start()
 *
 * Returned Value:
 *   On success, returns OK.
 *   On failure, a negated errno value is returned.
 *
 ****************************************************************************/

int ptpd_stop(int pid)
{
  if (kill(pid, SIGHUP) == OK)
    {
      return OK;
    }
  else
    {
      return -errno;
    }
}