path: root/test/regression/ptp_test/netx_ptp_utility.c

/***************************************************************************/
/* Copyright (c) 2024 Microsoft Corporation                                */
/* Copyright (c) 2026 Eclipse ThreadX contributors                         */
/*                                                                         */
/* This program and the accompanying materials are made available under    */
/* the terms of the MIT License which is available at                      */
/* https://opensource.org/licenses/MIT.                                    */
/*                                                                         */
/* SPDX-License-Identifier: MIT                                            */
/***************************************************************************/


#include "netx_ptp_utility.h"

#define NANOSECONDS_PER_SEC             1000000000L

extern VOID _nx_ram_network_driver(struct NX_IP_DRIVER_STRUCT *driver_req);

/*
Precision Time Protocol (IEEE1588)
    0000 .... = transportSpecific: 0x0
    .... 1011 = messageId: Announce Message (0xb)
    0000 .... = Reserved: 0
    .... 0010 = versionPTP: 2
    messageLength: 64
    subdomainNumber: 0
    Reserved: 0
    flags: 0x0000
    correction: 0.000000 nanoseconds
    Reserved: 0
    ClockIdentity: 0x46e7c8fffe7161a1
    SourcePortID: 1
    sequenceId: 61
    control: Other Message (5)
    logMessagePeriod: 1
    originTimestamp (seconds): 0
    originTimestamp (nanoseconds): 0
    originCurrentUTCOffset: 0
    priority1: 128
    grandmasterClockClass: 127
    grandmasterClockAccuracy: Accuracy Unknown (0xfe)
    grandmasterClockVariance: 28768
    priority2: 128
    grandmasterClockIdentity: 0x46e7c8fffe7161a1
    localStepsRemoved: 0
    TimeSource: INTERNAL_OSCILLATOR (0xa0)
*/
static UCHAR announce_data[] = \
"\x0b\x02\x00\x40\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
"\x00\x00\x00\x00\x46\xe7\xc8\xff\xfe\x71\x61\xa1\x00\x01\x00\x3d" \
"\x05\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x22\x80" \
"\x7f\xfe\x70\x60\x80\x46\xe7\xc8\xff\xfe\x71\x61\xa1\x00\x00\xa0";

/*
Precision Time Protocol (IEEE1588)
    0000 .... = transportSpecific: 0x0
    .... 0000 = messageId: Sync Message (0x0)
    0000 .... = Reserved: 0
    .... 0010 = versionPTP: 2
    messageLength: 44
    subdomainNumber: 0
    Reserved: 0
    flags: 0x0200
    correction: 0.000000 nanoseconds
    Reserved: 0
    ClockIdentity: 0x46e7c8fffe7161a1
    SourcePortID: 1
    sequenceId: 123
    control: Sync Message (0)
    logMessagePeriod: 0
    originTimestamp (seconds): 1603784044
    originTimestamp (nanoseconds): 604899854
*/
static UCHAR sync_data[] = \
"\x00\x02\x00\x2c\x00\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
"\x00\x00\x00\x00\x46\xe7\xc8\xff\xfe\x71\x61\xa1\x00\x01\x00\x7b" \
"\x00\x00\x00\x00\x5f\x97\xcd\x6c\x24\x0e\x0a\x0e";

/*
Precision Time Protocol (IEEE1588)
    0000 .... = transportSpecific: 0x0
    .... 1000 = messageId: Follow_Up Message (0x8)
    0000 .... = Reserved: 0
    .... 0010 = versionPTP: 2
    messageLength: 44
    subdomainNumber: 0
    Reserved: 0
    flags: 0x0400
    correction: 0.000000 nanoseconds
    Reserved: 0
    ClockIdentity: 0x46e7c8fffe7161a1
    SourcePortID: 1
    sequenceId: 123
    control: Follow_Up Message (2)
    logMessagePeriod: 0
    preciseOriginTimestamp (seconds): 1603784044
    preciseOriginTimestamp (nanoseconds): 604908000
*/
static UCHAR follow_up_data[] = \
"\x08\x02\x00\x2c\x00\x00\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
"\x00\x00\x00\x00\x46\xe7\xc8\xff\xfe\x71\x61\xa1\x00\x01\x00\x7b" \
"\x02\x00\x00\x00\x5f\x97\xcd\x6c\x24\x0e\x29\xe0";

/*
Precision Time Protocol (IEEE1588)
    0000 .... = transportSpecific: 0x0
    .... 1001 = messageId: Delay_Resp Message (0x9)
    0000 .... = Reserved: 0
    .... 0010 = versionPTP: 2
    messageLength: 54
    subdomainNumber: 0
    Reserved: 0
    flags: 0x0000
    correction: 0.000000 nanoseconds
    Reserved: 0
    ClockIdentity: 0x46e7c8fffe7161a1
    SourcePortID: 1
    sequenceId: 1
    control: Delay_Resp Message (3)
    logMessagePeriod: 0
    receiveTimestamp (seconds): 1603784044
    receiveTimestamp (nanoseconds): 613726000
    requestingSourcePortIdentity: 0x001122fffe334457
    requestingSourcePortId: 1
*/
static UCHAR delay_resp_data[] = \
"\x09\x02\x00\x36\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
"\x00\x00\x00\x00\x46\xe7\xc8\xff\xfe\x71\x61\xa1\x00\x01\x00\x01" \
"\x03\x00\x00\x00\x5f\x97\xcd\x6c\x24\x94\xb7\x30\x00\x11\x22\xff" \
"\xfe\x33\x44\x57\x00\x01";

static UCHAR use_two_steps = NX_TRUE;
static NX_PTP_CLIENT *ptp_client_ptr = NX_NULL;
static NX_PTP_TIME ptp_timestamp = {0};
static UCHAR use_ipv4 = NX_TRUE;

static VOID send_udp_data(NX_UDP_SOCKET *socket_ptr, NX_PACKET_POOL *pool_ptr,
                          UINT port, UCHAR *data, UINT size)
{
NX_PACKET *packet_ptr;
UINT status;
#ifndef NX_DISABLE_IPV6
NXD_ADDRESS ipv6_addr;
#endif

    /* Allocate a packet.  */
    status =  nx_packet_allocate(pool_ptr, &packet_ptr, NX_UDP_PACKET, NX_NO_WAIT);
    ASSERT_SUCCESS(status);

    /* Append announce data.  */
    status = nx_packet_data_append(packet_ptr, data, size, pool_ptr, NX_NO_WAIT);
    ASSERT_SUCCESS(status);

    /* Send out the packet.  */
    if (use_ipv4)
    {
        status = nx_udp_socket_send(socket_ptr, packet_ptr, PTP_IPV4_MULTICAST_ADDR, port);
    }
#if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
    else
    {
        PTP_IPV6_MULTICAST_ADDR_SET(&ipv6_addr);
        status = nxd_udp_socket_send(socket_ptr, packet_ptr, &ipv6_addr, port);
    }
#endif
    
    ASSERT_SUCCESS(status);
}

static VOID fill_timestamp(UCHAR *buffer, NX_PTP_TIME *ts)
{
    buffer[0] = (ts -> second_high >> 8) & 0xFF;
    buffer[1] = ts -> second_high & 0xFF;
    buffer[2] = (ts -> second_low >> 24) & 0xFF;
    buffer[3] = (ts -> second_low >> 16) & 0xFF;
    buffer[4] = (ts -> second_low >> 8) & 0xFF;
    buffer[5] = ts -> second_low & 0xFF;
    buffer[6] = (ts -> nanosecond >> 24) & 0xFF;
    buffer[7] = (ts -> nanosecond >> 16) & 0xFF;
    buffer[8] = (ts -> nanosecond >> 8) & 0xFF;
    buffer[9] = ts -> nanosecond & 0xFF;
}

VOID create_socket(NX_IP *ip_ptr, NX_UDP_SOCKET *socket_ptr, UINT port)
{
UINT status;
#if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
NXD_ADDRESS ipv6_addr;
#endif

    /* Create a UDP socket.  */
    status = nx_udp_socket_create(ip_ptr, socket_ptr, "UDP Socket", NX_IP_NORMAL,
                                  NX_FRAGMENT_OKAY, 0x80, 5);
    ASSERT_SUCCESS(status);

    /* Bind the UDP socket to the IP port.  */
    status = nx_udp_socket_bind(socket_ptr, port, NX_NO_WAIT);
    ASSERT_SUCCESS(status);

    /* Join multicast group.  */
    status = nx_ipv4_multicast_interface_join(ip_ptr, PTP_IPV4_MULTICAST_ADDR, 0);
    ASSERT_SUCCESS(status);

#if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
    PTP_IPV6_MULTICAST_ADDR_SET(&ipv6_addr);
    status = nxd_ipv6_multicast_interface_join(ip_ptr, &ipv6_addr, 0);
    ASSERT_SUCCESS(status);
#endif
}

VOID send_announce(NX_UDP_SOCKET *socket_ptr, NX_PACKET_POOL *pool_ptr, USHORT utc_offset)
{

    /* Adjust utc offset.  */
    NX_CHANGE_USHORT_ENDIAN(utc_offset);
    memcpy(&announce_data[44], &utc_offset, sizeof(utc_offset));

    /* Send out announce packet.  */
    send_udp_data(socket_ptr, pool_ptr, PTP_GENERAL_UDP_PORT,
                  announce_data, sizeof(announce_data));
}

VOID send_sync(NX_UDP_SOCKET *socket_ptr, NX_PACKET_POOL *pool_ptr, NX_PTP_TIME *ts)
{
    if (use_two_steps)
    {
        sync_data[6] |= 0x2;
    }
    else
    {
        sync_data[6] &= 0xFD;
    }
    

    /* Adjust timestamp. */
    fill_timestamp(&sync_data[34], ts);

    /* Send out sync packet.  */
    send_udp_data(socket_ptr, pool_ptr, PTP_EVENT_UDP_PORT,
                  sync_data, sizeof(sync_data));
}

VOID send_follow_up(NX_UDP_SOCKET *socket_ptr, NX_PACKET_POOL *pool_ptr, NX_PTP_TIME *ts)
{

    /* Adjust timestamp. */
    fill_timestamp(&follow_up_data[34], ts);

    /* Send out follow up packet.  */
    send_udp_data(socket_ptr, pool_ptr, PTP_GENERAL_UDP_PORT,
                  follow_up_data, sizeof(follow_up_data));
}

VOID send_delay_response(NX_UDP_SOCKET *socket_ptr, NX_PACKET_POOL *pool_ptr,
                         DELAY_REQUEST_CONTEXT *context, NX_PTP_TIME *ts)
{

    /* Adjust timestamp. */
    fill_timestamp(&delay_resp_data[34], ts);

    /* Adjust clock ID and sequence ID.  */
    memcpy(&delay_resp_data[30], context -> sequence_id, 2);

    memcpy(&delay_resp_data[sizeof(delay_resp_data) - 11], context -> clock_id,
           NX_PTP_CLOCK_PORT_IDENTITY_SIZE);

    /* Send out delay resp packet.  */
    send_udp_data(socket_ptr, pool_ptr, PTP_GENERAL_UDP_PORT,
                  delay_resp_data, sizeof(delay_resp_data));
}

UINT receive_delay_request(NX_UDP_SOCKET *socket_ptr, DELAY_REQUEST_CONTEXT *context,
                           UINT wait_option)
{
NX_PACKET *packet_ptr;
NXD_ADDRESS ip_address;
ULONG bytes_copied;
UINT port;
UINT status;


    status = nx_udp_socket_receive(socket_ptr, &packet_ptr, wait_option);
    if (status)
    {
        return(status);
    }

    status = nxd_udp_source_extract(packet_ptr, &ip_address, &port);
    ASSERT_SUCCESS(status);
    ASSERT_TRUE(port == PTP_EVENT_UDP_PORT);

    status = nx_packet_data_extract_offset(packet_ptr, 20, context -> clock_id,
                                           NX_PTP_CLOCK_PORT_IDENTITY_SIZE,
                                           &bytes_copied);
    ASSERT_SUCCESS(status);
    ASSERT_TRUE(bytes_copied == NX_PTP_CLOCK_PORT_IDENTITY_SIZE);

    status = nx_packet_data_extract_offset(packet_ptr, 30, context -> sequence_id,
                                           2, &bytes_copied);
    ASSERT_SUCCESS(status);
    ASSERT_TRUE(bytes_copied == 2);

    return(NX_SUCCESS);
}


VOID set_two_steps(UCHAR two_steps)
{
    use_two_steps = two_steps;
}


VOID set_clock_id(UCHAR *clock_id)
{

    /* Adjust clock ID.  */
    memcpy(&announce_data[20], clock_id, NX_PTP_CLOCK_PORT_IDENTITY_SIZE);
    memcpy(&sync_data[20], clock_id, NX_PTP_CLOCK_PORT_IDENTITY_SIZE);
    memcpy(&follow_up_data[20], clock_id, NX_PTP_CLOCK_PORT_IDENTITY_SIZE);
    memcpy(&delay_resp_data[20], clock_id, NX_PTP_CLOCK_PORT_IDENTITY_SIZE);
}


VOID set_ip_version(UCHAR ip_version)
{
    if (ip_version == NX_IP_VERSION_V4)
    {
        use_ipv4 = NX_TRUE;
    }
#if defined(NX_ENABLE_IPV6_MULTICAST) && defined(FEATURE_NX_IPV6)
    else
    {
        use_ipv4 = NX_FALSE;
    }    
#endif
}


VOID calibrate_timestamp(TEST_TIMESTAMP *test_ts, NX_PTP_TIME *ts)
{
ULONG64 second_low;
ULONG64 carry;
long long nanosecond;
NX_PTP_TIME *t1;

    /* Means of following symbols,
         t1 is timestamp of Sync if two step is disabled,
            or else, it is timestamp of FollowUp.
         t2 is timestamp of Sync received.
         t3 is timestamp of DelayRequest sent. Also equals client_clock in the test
         t4 is timestamp of DelayRequest in DelayResponse packet.

       meanPathDelay between client and server is,
         [(t2-t1)+(t4-t3)]/2

       offsetFromMaster = client_clock - master_clock
                        = t2 - t1 - meanPathDelay - correctionField

        master_clock = [(t4+t3)-(t2-t1)]/2

        Note, The recommendation that the timestamps themselves be
        the best possible estimate of the time enables simple devices
        that only need approximate time to ignore the correctionField.
    */

    if ((test_ts -> follow_up.second_high == 0) && 
        (test_ts -> follow_up.second_low == 0) && 
        (test_ts -> follow_up.nanosecond == 0))
    {

        /* No FollowUp timestamp. Use Sync timestamp.  */
        t1 = &test_ts -> sync;
    }
    else
    {

        /* Use FollowUp timestamp.  */
        t1 = &test_ts -> follow_up;
    }

    ts -> second_high = (test_ts -> delay_response.second_high + test_ts -> delay_request.second_high) -
                        (test_ts -> sync_received.second_high - t1 -> second_high);
    if (ts -> second_high & 1)
    {
        carry = 0x100000000;
    }
    else
    {
        carry = 0;
    }
    ts -> second_high /= 2;
    
    second_low = carry + ((ULONG64)test_ts -> delay_response.second_low + (ULONG64)test_ts -> delay_request.second_low) -
                 ((ULONG64)test_ts -> sync_received.second_low - (ULONG64)t1 -> second_low);
    if (second_low & 1)
    {
        carry = NANOSECONDS_PER_SEC;
    }
    else
    {
        carry = 0;
    }
    second_low /= 2;
    
    nanosecond = (carry + ((long long)test_ts -> delay_response.nanosecond + (long long)test_ts -> delay_request.nanosecond) -
                  ((long long)test_ts -> sync_received.nanosecond - (long long)t1 -> nanosecond)) / 2;

    /* Adjust according to carry. */
    while (nanosecond >= NANOSECONDS_PER_SEC)
    {
        nanosecond -= NANOSECONDS_PER_SEC;
        second_low++;
    }

    if (nanosecond < 0)
    {
        nanosecond += NANOSECONDS_PER_SEC;
        if (second_low == 0)
        {
            ts -> second_high--;
            second_low = 0xFFFFFFFF;
        }
        else
        {
            second_low--;
        }
        
    }

    while (second_low > 0xFFFFFFFF)
    {
        second_low -= 0xFFFFFFFF;
        ts -> second_high++;
    }

    ts -> second_low = (ULONG)second_low;
    ts -> nanosecond = (LONG)nanosecond;
}

VOID _netx_ptp_network_driver(struct NX_IP_DRIVER_STRUCT *driver_req)
{
NX_PACKET *packet_ptr;

#ifdef NX_ENABLE_INTERFACE_CAPABILITY
    if (driver_req -> nx_ip_driver_command == NX_LINK_PACKET_SEND)
    {
        packet_ptr = driver_req -> nx_ip_driver_packet;
        if (packet_ptr -> nx_packet_interface_capability_flag & NX_INTERFACE_CAPABILITY_PTP_TIMESTAMP)
        {

            /* call notification callback */
            nx_ptp_client_packet_timestamp_notify(ptp_client_ptr, packet_ptr, &ptp_timestamp);
        }
    }
#endif

    _nx_ram_network_driver(driver_req);
}

#ifdef NX_ENABLE_INTERFACE_CAPABILITY
UINT _netx_ptp_clock_callback(NX_PTP_CLIENT *client_ptr, UINT operation,
                              NX_PTP_TIME *time_ptr, NX_PACKET *packet_ptr,
                              VOID *callback_data)
{

    NX_PARAMETER_NOT_USED(callback_data);

    switch (operation)
    {

    /* Save pointer to PTP client.  */
    case NX_PTP_CLIENT_CLOCK_INIT:
        ptp_client_ptr = client_ptr;
        break;

    /* Set clock.  */
    case NX_PTP_CLIENT_CLOCK_SET:
        ptp_timestamp = *time_ptr;
        break;

    /* Extract timestamp from packet.  */
    case NX_PTP_CLIENT_CLOCK_PACKET_TS_EXTRACT:
        *time_ptr = ptp_timestamp;
        break;

    /* Get clock.  */
    case NX_PTP_CLIENT_CLOCK_GET:
        *time_ptr = ptp_timestamp;
        break;

    /* Adjust clock.  */
    case NX_PTP_CLIENT_CLOCK_ADJUST:
        ptp_timestamp.nanosecond += time_ptr -> nanosecond;
        if (ptp_timestamp.nanosecond >= NANOSECONDS_PER_SEC)
        {
            ptp_timestamp.nanosecond -= NANOSECONDS_PER_SEC;
            if (ptp_timestamp.second_low == 0xFFFFFFFF)
            {
                ptp_timestamp.second_high++;
                ptp_timestamp.second_low = 0;
            }
            else
            {
                ptp_timestamp.second_low++;
            }
        }
        else if (ptp_timestamp.nanosecond < 0)
        {
            ptp_timestamp.nanosecond += NANOSECONDS_PER_SEC;
            if (ptp_timestamp.second_low == 0)
            {
                ptp_timestamp.second_high--;
                ptp_timestamp.second_low = 0xFFFFFFFF;
            }
            else
            {
                ptp_timestamp.second_low--;
            }
        }
        break;

    /* Prepare timestamp for current packet. */
    case NX_PTP_CLIENT_CLOCK_PACKET_TS_PREPARE:
        packet_ptr -> nx_packet_interface_capability_flag |= NX_INTERFACE_CAPABILITY_PTP_TIMESTAMP;
        break;

    /* Update soft timer.  */
    case NX_PTP_CLIENT_CLOCK_SOFT_TIMER_UPDATE:
        break;

    default:
        return(NX_PTP_PARAM_ERROR);
    }

    return(NX_SUCCESS);
}
#endif