diff --git a/doc/api/networking.rst b/doc/api/networking.rst index b21d40c49c2cc4..a659da81f412c7 100644 --- a/doc/api/networking.rst +++ b/doc/api/networking.rst @@ -108,6 +108,12 @@ Hostname Configuration Library .. doxygengroup:: net_hostname :project: Zephyr +generic Precision Time Protocol (gPTP) +************************************** + +.. doxygengroup:: gptp + :project: Zephyr + Network technologies ******************** diff --git a/include/net/ethernet.h b/include/net/ethernet.h index 0d69b5d54f8f3c..3f77740ad0abe6 100644 --- a/include/net/ethernet.h +++ b/include/net/ethernet.h @@ -44,6 +44,7 @@ struct net_eth_addr { #define NET_ETH_PTYPE_IP 0x0800 #define NET_ETH_PTYPE_IPV6 0x86dd #define NET_ETH_PTYPE_VLAN 0x8100 +#define NET_ETH_PTYPE_PTP 0x88f7 #define NET_ETH_MINIMAL_FRAME_SIZE 60 @@ -188,6 +189,14 @@ struct ethernet_context { struct net_if *iface; } carrier_mgmt; +#if defined(CONFIG_NET_GPTP) + /** The gPTP port number for this network device. We need to store the + * port number here so that we do not need to fetch it for every + * incoming gPTP packet. + */ + int port; +#endif + #if defined(CONFIG_NET_VLAN) /** Flag that tells whether how many VLAN tags are enabled for this * context. The same information can be dug from the vlan array but @@ -259,6 +268,22 @@ static inline bool net_eth_is_addr_multicast(struct net_eth_addr *addr) return false; } +static inline bool net_eth_is_addr_lldp_multicast(struct net_eth_addr *addr) +{ +#if defined(CONFIG_NET_GPTP) + if (addr->addr[0] == 0x01 && + addr->addr[1] == 0x80 && + addr->addr[2] == 0xc2 && + addr->addr[3] == 0x00 && + addr->addr[4] == 0x00 && + addr->addr[5] == 0x0e) { + return true; + } +#endif + + return false; +} + const struct net_eth_addr *net_eth_broadcast_addr(void); /** @@ -424,6 +449,32 @@ void net_eth_carrier_off(struct net_if *iface); */ struct device *net_eth_get_ptp_clock(struct net_if *iface); +#if defined(CONFIG_NET_GPTP) +/** + * @brief Return gPTP port number attached to this interface. + * + * @param iface Network interface + * + * @return Port number, no such port if < 0 + */ +int net_eth_get_ptp_port(struct net_if *iface); + +/** + * @brief Set gPTP port number attached to this interface. + * + * @param iface Network interface + * @param port Port number to set + */ +void net_eth_set_ptp_port(struct net_if *iface, int port); +#else +static inline int net_eth_get_ptp_port(struct net_if *iface) +{ + ARG_UNUSED(iface); + + return -ENODEV; +} +#endif /* CONFIG_NET_GPTP */ + #ifdef __cplusplus } #endif diff --git a/include/net/gptp.h b/include/net/gptp.h new file mode 100644 index 00000000000000..41a5e41dc3da7f --- /dev/null +++ b/include/net/gptp.h @@ -0,0 +1,296 @@ +/* + * Copyright (c) 2017 Intel Corporation. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @file + * @brief Public functions for the Precision Time Protocol Stack. + * + */ + +#ifndef __GPTP_H +#define __GPTP_H + +/** + * @brief generic Precision Time Protocol (gPTP) support + * @defgroup gptp gPTP support + * @ingroup networking + * @{ + */ + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define GPTP_CLOCK_ACCURACY_UNKNOWN 0xFE + +#define GPTP_OFFSET_SCALED_LOG_VAR_UNKNOWN 0x436A + +#define GPTP_PRIORITY1_NON_GM_CAPABLE 255 +#define GPTP_PRIORITY2_DEFAULT 248 + +/** + * @brief Scaled Nanoseconds. + */ +struct gptp_scaled_ns { + /** High half. */ + s32_t high; + + /** Low half. */ + s64_t low; +} __packed; + +/** + * @brief UScaled Nanoseconds. + */ +struct gptp_uscaled_ns { + /** High half. */ + u32_t high; + + /** Low half. */ + u64_t low; +} __packed; + +#if defined(CONFIG_NEWLIB_LIBC) +#include + +#define GPTP_POW2(exp) pow(2, exp) +#else + +static inline double _gptp_pow2(int exp) +{ + double res; + + if (exp >= 0) { + res = 1 << exp; + } else { + res = 1.0; + + while (exp++) { + res /= 2; + } + } + + return res; +} + +#define GPTP_POW2(exp) _gptp_pow2(exp) +#endif + +/* Message types. Event messages have BIT(3) set to 0, and general messages + * have that bit set to 1. IEEE 802.1AS chapter 10.5.2.2.2 + */ +#define GPTP_SYNC_MESSAGE 0x00 +#define GPTP_DELAY_REQ_MESSAGE 0x01 +#define GPTP_PATH_DELAY_REQ_MESSAGE 0x02 +#define GPTP_PATH_DELAY_RESP_MESSAGE 0x03 +#define GPTP_FOLLOWUP_MESSAGE 0x08 +#define GPTP_DELAY_RESP_MESSAGE 0x09 +#define GPTP_PATH_DELAY_FOLLOWUP_MESSAGE 0x0a +#define GPTP_ANNOUNCE_MESSAGE 0x0b +#define GPTP_SIGNALING_MESSAGE 0x0c +#define GPTP_MANAGEMENT_MESSAGE 0x0d + +#define GPTP_IS_EVENT_MSG(msg_type) (!((msg_type) & BIT(3))) + +#define GPTP_CLOCK_ID_LEN 8 + +/** + * @brief Port Identity. + */ +struct gptp_port_identity { + /** Clock identity of the port. */ + u8_t clk_id[GPTP_CLOCK_ID_LEN]; + + /** Number of the port. */ + u16_t port_number; +} __packed; + +struct gptp_flags { + union { + /** Byte access. */ + u8_t octets[2]; + + /** Whole field access. */ + u16_t all; + }; +} __packed; + +struct gptp_hdr { + /** Type of the message. */ + u8_t message_type:4; + + /** Transport specific, always 1. */ + u8_t transport_specific:4; + + /** Version of the PTP, always 2. */ + u8_t ptp_version:4; + + /** Reserved field. */ + u8_t reserved0:4; + + /** Total length of the message from the header to the last TLV. */ + u16_t message_length; + + /** Domain number, always 0. */ + u8_t domain_number; + + /** Reserved field. */ + u8_t reserved1; + + /** Message flags. */ + struct gptp_flags flags; + + /** Correction Field. The content depends of the message type. */ + s64_t correction_field; + + /** Reserved field. */ + u32_t reserved2; + + /** Port Identity of the sender. */ + struct gptp_port_identity port_id; + + /** Sequence Id. */ + u16_t sequence_id; + + /** Control value. Sync: 0, Follow-up: 2, Others: 5. */ + u8_t control; + + /** Message Interval in Log2 for Sync and Announce messages. */ + s8_t log_msg_interval; +} __packed; + +/* + * TODO: k_uptime_get need to be replaced by the MAC ptp_clock. + * The ptp_clock access infrastructure is not ready yet + * so use it for the time being. + * k_uptime time precision is in ms. + */ +#define GPTP_GET_CURRENT_TIME_NANOSECOND() (k_uptime_get() * 1000000) +#define GPTP_GET_CURRENT_TIME_USCALED_NS(uscaled_ns_ptr) \ + do { \ + (uscaled_ns_ptr)->low = \ + GPTP_GET_CURRENT_TIME_NANOSECOND() << 16; \ + (uscaled_ns_ptr)->high = 0; \ + } while (0) + +/** + * @typedef gptp_phase_dis_callback_t + * @brief Define callback that is called after a phase discontinuity has been + * sent by the grandmaster. + * @param "u8_t *gm_identity" A pointer to first element of a + * ClockIdentity array. The size of the array is GPTP_CLOCK_ID_LEN. + * @param "u16_t *gm_time_base" A pointer to the value of timeBaseIndicator + * of the current grandmaster. + * @param "struct scaled_ns *last_gm_ph_change" A pointer to the value of + * lastGmPhaseChange received from grandmaster. + * @param "double *last_gm_freq_change" A pointer to the value of + * lastGmFreqChange received from the grandmaster. + */ +typedef void (*gptp_phase_dis_callback_t)( + u8_t *gm_identity, + u16_t *time_base, + struct gptp_scaled_ns *last_gm_ph_change, + double *last_gm_freq_change); + +/** + * @brief Phase discontinuity callback structure. + * + * Stores the phase discontinuity callback information. Caller must make sure + * that the variable pointed by this is valid during the lifetime of + * registration. Typically this means that the variable cannot be + * allocated from stack. + */ +struct gptp_phase_dis_cb { + /** Node information for the slist. */ + sys_snode_t node; + + /** Phase discontinuity callback. */ + gptp_phase_dis_callback_t cb; +}; + +/** + * @brief Register a phase discontinuity callback. + * + * @param phase_dis Caller specified handler for the callback. + * @param cb Callback to register. + */ +void gptp_register_phase_dis_cb(struct gptp_phase_dis_cb *phase_dis, + gptp_phase_dis_callback_t cb); + +/** + * @brief Unregister a phase discontinuity callback. + * + * @param phase_dis Caller specified handler for the callback. + */ +void gptp_unregister_phase_dis_cb(struct gptp_phase_dis_cb *phase_dis); + +/** + * @brief Call a phase discontinuity callback function. + */ +void gptp_call_phase_dis_cb(void); + +/** + * @brief Get gPTP time. + * + * @param slave_time A pointer to structure where timestamp will be saved. + * @param gm_present A pointer to a boolean where status of the + * presence of a grand master will be saved. + * + * @return Error code. 0 if no error. + */ +int gptp_event_capture(struct net_ptp_time *slave_time, bool *gm_present); + +/** + * @brief Utility function to print clock id to a user supplied buffer. + * + * @param clk_id Clock id + * @param output Output buffer + * @param output_len Output buffer len + * + * @return Pointer to output buffer + */ +char *gptp_sprint_clock_id(const u8_t *clk_id, char *output, + size_t output_len); + +/** + * @typedef gptp_port_cb_t + * @brief Callback used while iterating over gPTP ports + * + * @param port Port number + * @param iface Pointer to network interface + * @param user_data A valid pointer to user data or NULL + */ +typedef void (*gptp_port_cb_t)(int port, struct net_if *iface, + void *user_data); + +/** + * @brief Go through all the gPTP ports and call callback for each of them. + * + * @param cb User-supplied callback function to call + * @param user_data User specified data + */ +void gptp_foreach_port(gptp_port_cb_t cb, void *user_data); + +/** + * @brief Get gPTP domain. + * @details This contains all the configuration / status of the gPTP domain. + * + * @return Pointer to domain or NULL if not found. + */ +struct gptp_domain *gptp_get_domain(void); + +#ifdef __cplusplus +} +#endif + +/** + * @} + */ + +#endif /* __GPTP_H */ diff --git a/include/net/net_ip.h b/include/net/net_ip.h index de14a9834fa146..cc972b3f5251ba 100644 --- a/include/net/net_ip.h +++ b/include/net/net_ip.h @@ -62,8 +62,10 @@ enum net_sock_type { #define ntohs(x) sys_be16_to_cpu(x) #define ntohl(x) sys_be32_to_cpu(x) +#define ntohll(x) sys_be64_to_cpu(x) #define htons(x) sys_cpu_to_be16(x) #define htonl(x) sys_cpu_to_be32(x) +#define htonll(x) sys_cpu_to_be64(x) /** IPv6 address structure */ struct in6_addr { diff --git a/subsys/net/ip/net_core.c b/subsys/net/ip/net_core.c index c850b53b422e1f..53137675453b0b 100644 --- a/subsys/net/ip/net_core.c +++ b/subsys/net/ip/net_core.c @@ -30,6 +30,7 @@ #include #include #include +#include #include "net_private.h" #include "net_shell.h" @@ -130,6 +131,9 @@ static void processing_data(struct net_pkt *pkt, bool is_loopback) /* Things to setup after we are able to RX and TX */ static void net_post_init(void) { +#if defined(CONFIG_NET_GPTP) + net_gptp_init(); +#endif } static void init_rx_queues(void) diff --git a/subsys/net/ip/net_private.h b/subsys/net/ip/net_private.h index d6d5483dc17404..c268d9ce70775c 100644 --- a/subsys/net/ip/net_private.h +++ b/subsys/net/ip/net_private.h @@ -51,6 +51,25 @@ extern void net_tc_rx_init(void); extern void net_tc_submit_to_tx_queue(u8_t tc, struct net_pkt *pkt); extern void net_tc_submit_to_rx_queue(u8_t tc, struct net_pkt *pkt); +#if defined(CONFIG_NET_GPTP) +/** + * @brief Initialize Precision Time Protocol Layer. + */ +void net_gptp_init(void); + +/** + * @brief Process a ptp message. + * + * @param buf Buffer with a valid PTP Ethernet type. + * + * @return Return the policy for network buffer. + */ +enum net_verdict net_gptp_recv(struct net_if *iface, struct net_pkt *pkt); +#else +#define net_gptp_init() +#define net_gptp_recv(iface, pkt) +#endif /* CONFIG_NET_GPTP */ + #if defined(CONFIG_NET_IPV6_FRAGMENT) int net_ipv6_send_fragmented_pkt(struct net_if *iface, struct net_pkt *pkt, u16_t pkt_len); diff --git a/subsys/net/l2/ethernet/CMakeLists.txt b/subsys/net/l2/ethernet/CMakeLists.txt index ceb3ceeba13314..0dfee8c39a61f3 100644 --- a/subsys/net/l2/ethernet/CMakeLists.txt +++ b/subsys/net/l2/ethernet/CMakeLists.txt @@ -8,3 +8,7 @@ zephyr_library_sources_ifdef(CONFIG_NET_ARP arp.c) zephyr_library_sources_ifdef(CONFIG_NET_L2_ETHERNET ethernet.c) zephyr_library_sources_ifdef(CONFIG_NET_L2_ETHERNET_MGMT ethernet_mgmt.c) zephyr_library_sources_ifdef(CONFIG_NET_STATISTICS_ETHERNET ethernet_stats.c) + +if(CONFIG_NET_GPTP) + add_subdirectory(gptp) +endif() diff --git a/subsys/net/l2/ethernet/Kconfig b/subsys/net/l2/ethernet/Kconfig index a29b506ad12410..c7761b7171cb3f 100644 --- a/subsys/net/l2/ethernet/Kconfig +++ b/subsys/net/l2/ethernet/Kconfig @@ -67,4 +67,6 @@ config NET_DEBUG_ARP help Enables core ARP code part to output debug messages +source "subsys/net/l2/ethernet/gptp/Kconfig" + endif # NET_L2_ETHERNET diff --git a/subsys/net/l2/ethernet/ethernet.c b/subsys/net/l2/ethernet/ethernet.c index e4ee8f410ad91d..5a4295b5dd28db 100644 --- a/subsys/net/l2/ethernet/ethernet.c +++ b/subsys/net/l2/ethernet/ethernet.c @@ -16,6 +16,7 @@ #include #include #include +#include #include "net_private.h" #include "ipv6.h" @@ -155,6 +156,11 @@ static enum net_verdict ethernet_recv(struct net_if *iface, net_pkt_set_family(pkt, AF_INET6); family = AF_INET6; break; +#if defined(CONFIG_NET_GPTP) + case NET_ETH_PTYPE_PTP: + family = AF_UNSPEC; + break; +#endif default: NET_DBG("Unknown hdr type 0x%04x iface %p", type, iface); return NET_DROP; @@ -185,6 +191,8 @@ static enum net_verdict ethernet_recv(struct net_if *iface, if (!net_eth_is_addr_broadcast((struct net_eth_addr *)lladdr->addr) && !net_eth_is_addr_multicast((struct net_eth_addr *)lladdr->addr) && + !net_eth_is_addr_lldp_multicast( + (struct net_eth_addr *)lladdr->addr) && !net_linkaddr_cmp(net_if_get_link_addr(iface), lladdr)) { /* The ethernet frame is not for me as the link addresses * are different. @@ -207,6 +215,13 @@ static enum net_verdict ethernet_recv(struct net_if *iface, return net_arp_input(pkt); } #endif + +#if defined(CONFIG_NET_GPTP) + if (type == NET_ETH_PTYPE_PTP) { + return net_gptp_recv(iface, pkt); + } +#endif + ethernet_update_length(iface, pkt); return NET_CONTINUE; @@ -821,6 +836,22 @@ struct device *net_eth_get_ptp_clock(struct net_if *iface) #endif } +#if defined(CONFIG_NET_GPTP) +int net_eth_get_ptp_port(struct net_if *iface) +{ + struct ethernet_context *ctx = net_if_l2_data(iface); + + return ctx->port; +} + +void net_eth_set_ptp_port(struct net_if *iface, int port) +{ + struct ethernet_context *ctx = net_if_l2_data(iface); + + ctx->port = port; +} +#endif /* CONFIG_NET_GPTP */ + void ethernet_init(struct net_if *iface) { struct ethernet_context *ctx = net_if_l2_data(iface); diff --git a/subsys/net/l2/ethernet/gptp/CMakeLists.txt b/subsys/net/l2/ethernet/gptp/CMakeLists.txt new file mode 100644 index 00000000000000..b331455b630265 --- /dev/null +++ b/subsys/net/l2/ethernet/gptp/CMakeLists.txt @@ -0,0 +1,13 @@ +zephyr_library() +zephyr_library_include_directories(. ${ZEPHYR_BASE}/subsys/net/ip) +zephyr_library_compile_definitions_ifdef( + CONFIG_NEWLIB_LIBC __LINUX_ERRNO_EXTENSIONS__ + ) + +zephyr_library_sources( + gptp.c + gptp_user_api.c + gptp_md.c + gptp_messages.c + gptp_mi.c + ) diff --git a/subsys/net/l2/ethernet/gptp/Kconfig b/subsys/net/l2/ethernet/gptp/Kconfig new file mode 100644 index 00000000000000..f0c61d000200b5 --- /dev/null +++ b/subsys/net/l2/ethernet/gptp/Kconfig @@ -0,0 +1,104 @@ +# +# Copyright (c) 2018 Intel Corporation. +# +# SPDX-License-Identifier: Apache-2.0 +# + +menuconfig NET_GPTP + bool "Enable IEEE 802.1AS (gPTP) support [EXPERIMENTAL]" + depends on NET_L2_ETHERNET + select NET_PKT_TIMESTAMP + select PTP_CLOCK + default n + help + Enable gPTP driver that send and receives gPTP packets + and handles network packet timestamps. + +if NET_GPTP + +config NET_DEBUG_GPTP + bool "Enable Debug Information for gPTP" + default n + depends on NET_LOG + help + Enable logs for the gPTP stack. + +config NET_GPTP_NUM_PORTS + int "Number of gPTP ports" + default 1 + help + Configures the gPTP stack to work with the given number of ports. + The port concept is the same thing as network interface. + +config NET_GPTP_NEIGHBOR_PROP_DELAY_THR + int "Set neighbor propagation delay threshold (ns)" + default 100000 + help + Defines the neighbor propagation delay threshold in nanoseconds. + This is the propagation time threshold, above which a port is not + considered capable of participating in the IEEE 802.1AS protocol. + See IEEE 802.1AS chapter 11.2.12.6 for details. + +config NET_GPTP_INIT_LOG_PDELAY_REQ_ITV + int "Set initial pdelay request interval in Log2 base" + default 0 + help + Defines the interval at which a Path Delay Request will be sent. + The value is the converted in nanoseconds as follow: + nanoseconds = (10^9) * 2^(16 + value) + +config NET_GPTP_INIT_LOG_SYNC_ITV + int "Set initial sync interval in Log2 base" + default -3 + help + Defines the interval at which a Sync message will be sent. + The value is the converted in nanoseconds as follow: + nanoseconds = (10^9) * 2^(16 + value) + +config NET_GPTP_INIT_LOG_ANNOUNCE_ITV + int "Set initial announce interval in Log2 base" + default 0 + help + Defines the interval at which an Announce message will be sent. + The value is the converted in nanoseconds as follow: + nanoseconds = (10^9) * 2^(16 + value) + +config NET_GPTP_SYNC_RECEIPT_TIMEOUT + int "Number of sync intervals to wait" + default 6 + help + Defines the number of sync intervals to wait without receiving + synchronization information before assuming that the master is no + longer transmitting synchronization information. + +config NET_GPTP_ANNOUNCE_RECEIPT_TIMEOUT + int "Number of announce intervals to wait" + default 3 + help + Defines the number of announce intervals to wait without receiving + an Announce message before assuming that the master is no longer + transmitting Announce messages. + +config NET_GPTP_USE_DEFAULT_CLOCK_UPDATE + bool "Use a default clock update function" + default y + help + Use a default internal function to update port local clock. + This method may not be accurate. + +config NET_GPTP_PATH_TRACE_ELEMENTS + int "How many path trace elements to track" + default 8 + help + This tells the number of time-aware systems that transmits the + Announce message. Each array element takes 8 bytes. If this value + is set to 8, then 8 * 8 = 64 bytes of memory is used. + +config NET_GPTP_STATISTICS + bool "Collect gPTP statistics" + default n + help + Enable this if you need to collect gPTP statistics. The statistics + can be seen in net-shell if needed. + +endif diff --git a/subsys/net/l2/ethernet/gptp/gptp.c b/subsys/net/l2/ethernet/gptp/gptp.c new file mode 100644 index 00000000000000..bf9fecfed95b20 --- /dev/null +++ b/subsys/net/l2/ethernet/gptp/gptp.c @@ -0,0 +1,883 @@ +/* + * Copyright (c) 2017 Intel Corporation. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#if defined(CONFIG_NET_DEBUG_GPTP) +#define SYS_LOG_DOMAIN "net/gptp" +#define NET_LOG_ENABLED 1 +#endif + +#include +#include + +#include + +#include "gptp_messages.h" +#include "gptp_mi.h" +#include "gptp_data_set.h" + +#include "gptp_private.h" + +#if !defined(CONFIG_NET_GPTP_STACK_SIZE) +#define CONFIG_NET_GPTP_STACK_SIZE 2048 +#endif + +#if CONFIG_NET_GPTP_NUM_PORTS > 32 +/* + * Boolean arrays sizes have been hardcoded. + * It has been arbitrary chosen that a system can not + * have more than 32 ports. + */ +#error Maximum number of ports exceeded. (Max is 32). +#endif + +NET_STACK_DEFINE(GPTP, gptp_stack, CONFIG_NET_GPTP_STACK_SIZE, + CONFIG_NET_GPTP_STACK_SIZE); +K_FIFO_DEFINE(gptp_rx_queue); + +static struct k_thread gptp_thread_data; +struct gptp_domain gptp_domain; + +int gptp_get_port_number(struct net_if *iface) +{ + int port = net_eth_get_ptp_port(iface); + + if (port >= GPTP_PORT_START && port < GPTP_PORT_END) { + return port; + } + + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + if (GPTP_PORT_IFACE(port) == iface) { + return port; + } + } + + return -ENODEV; +} + +bool gptp_is_slave_port(int port) +{ + return (GPTP_GLOBAL_DS()->selected_role[port] == GPTP_PORT_SLAVE); +} + +/* + * Use the given port to generate the clock identity + * for the device. + * The clock identity is unique for one time-aware system. + */ +static void gptp_compute_clock_identity(int port) +{ + struct net_if *iface = GPTP_PORT_IFACE(port); + struct gptp_default_ds *default_ds; + + default_ds = GPTP_DEFAULT_DS(); + + if (iface) { + default_ds->clk_id[0] = net_if_get_link_addr(iface)->addr[0]; + default_ds->clk_id[1] = net_if_get_link_addr(iface)->addr[1]; + default_ds->clk_id[2] = net_if_get_link_addr(iface)->addr[2]; + default_ds->clk_id[3] = 0xFF; + default_ds->clk_id[4] = 0xFE; + default_ds->clk_id[5] = net_if_get_link_addr(iface)->addr[3]; + default_ds->clk_id[6] = net_if_get_link_addr(iface)->addr[4]; + default_ds->clk_id[7] = net_if_get_link_addr(iface)->addr[5]; + } +} + +#define PRINT_INFO(msg, hdr, pkt) \ + NET_DBG("Received %s seq %d pkt %p", msg, \ + ntohs(hdr->sequence_id), pkt) \ + + +static bool gptp_handle_critical_msg(struct net_if *iface, struct net_pkt *pkt) +{ + struct gptp_hdr *hdr = GPTP_HDR(pkt); + bool handled = false; + int port; + + switch (hdr->message_type) { + case GPTP_PATH_DELAY_REQ_MESSAGE: + if (GPTP_CHECK_LEN(pkt, GPTP_PDELAY_REQ_LEN)) { + NET_WARN("Invalid length for %s packet " + "should have %zd bytes but has %lu bytes", + "PDELAY_REQ", + GPTP_PDELAY_REQ_LEN, + GPTP_PACKET_LEN(pkt)); + break; + } + + PRINT_INFO("PDELAY_REQ", hdr, pkt); + + port = gptp_get_port_number(iface); + if (port == -ENODEV) { + NET_DBG("No port found for gPTP buffer"); + return handled; + } + + if (GPTP_PORT_STATE(port)->pdelay_resp.state != + GPTP_PDELAY_RESP_NOT_ENABLED) { + gptp_handle_pdelay_req(port, pkt); + } + + handled = true; + break; + default: + /* Not a critical message, this will be handled later. */ + break; + } + + return handled; +} + +static void gptp_handle_msg(struct net_pkt *pkt) +{ + struct gptp_hdr *hdr = GPTP_HDR(pkt); + struct gptp_pdelay_req_state *pdelay_req_state; + struct gptp_sync_rcv_state *sync_rcv_state; + struct gptp_port_announce_receive_state *pa_rcv_state; + struct gptp_port_bmca_data *bmca_data; + int port; + + port = gptp_get_port_number(net_pkt_iface(pkt)); + if (port == -ENODEV) { + NET_DBG("No port found for ptp buffer"); + return; + } + + pdelay_req_state = &GPTP_PORT_STATE(port)->pdelay_req; + sync_rcv_state = &GPTP_PORT_STATE(port)->sync_rcv; + + switch (hdr->message_type) { + case GPTP_SYNC_MESSAGE: + if (GPTP_CHECK_LEN(pkt, GPTP_SYNC_LEN)) { + NET_WARN("Invalid length for %s packet " + "should have %zd bytes but has %lu bytes", + "SYNC", + GPTP_SYNC_LEN, + GPTP_PACKET_LEN(pkt)); + GPTP_STATS_INC(port, rx_ptp_packet_discard_count); + break; + } + + PRINT_INFO("SYNC", hdr, pkt); + + sync_rcv_state->rcvd_sync = true; + + /* If we already have one, drop the previous one. */ + if (sync_rcv_state->rcvd_sync_ptr) { + net_pkt_unref(sync_rcv_state->rcvd_sync_ptr); + } + + /* Keep the buffer alive until follow_up is received. */ + net_pkt_ref(pkt); + sync_rcv_state->rcvd_sync_ptr = pkt; + + GPTP_STATS_INC(port, rx_sync_count); + break; + + case GPTP_DELAY_REQ_MESSAGE: + NET_DBG("Delay Request not handled."); + break; + + case GPTP_PATH_DELAY_REQ_MESSAGE: + /* + * Path Delay Responses to Path Delay Requests need + * very low latency. These need to handled in priority + * when received as they cannot afford to be delayed + * by context switches. + */ + NET_WARN("Path Delay Request received as normal messages!"); + GPTP_STATS_INC(port, rx_ptp_packet_discard_count); + break; + + case GPTP_PATH_DELAY_RESP_MESSAGE: + if (GPTP_CHECK_LEN(pkt, GPTP_PDELAY_RESP_LEN)) { + NET_WARN("Invalid length for %s packet " + "should have %zd bytes but has %lu bytes", + "PDELAY_RESP", + GPTP_PDELAY_RESP_LEN, + GPTP_PACKET_LEN(pkt)); + GPTP_STATS_INC(port, rx_ptp_packet_discard_count); + break; + } + + PRINT_INFO("PDELAY_RESP", hdr, pkt); + + pdelay_req_state->rcvd_pdelay_resp++; + + /* If we already have one, drop the received one. */ + if (pdelay_req_state->rcvd_pdelay_resp_ptr) { + break; + } + + /* Keep the buffer alive until pdelay_rate_ratio is computed. */ + net_pkt_ref(pkt); + pdelay_req_state->rcvd_pdelay_resp_ptr = pkt; + break; + + case GPTP_FOLLOWUP_MESSAGE: + if (GPTP_CHECK_LEN(pkt, GPTP_FOLLOW_UP_LEN)) { + NET_WARN("Invalid length for %s packet " + "should have %zd bytes but has %lu bytes", + "FOLLOWUP", + GPTP_FOLLOW_UP_LEN, + GPTP_PACKET_LEN(pkt)); + GPTP_STATS_INC(port, rx_ptp_packet_discard_count); + break; + } + + PRINT_INFO("FOLLOWUP", hdr, pkt); + + sync_rcv_state->rcvd_follow_up = true; + + /* If we already have one, drop the previous one. */ + if (sync_rcv_state->rcvd_follow_up_ptr) { + net_pkt_unref(sync_rcv_state->rcvd_follow_up_ptr); + } + + /* Keep the pkt alive until info is extracted. */ + sync_rcv_state->rcvd_follow_up_ptr = net_pkt_ref(pkt); + NET_DBG("Keeping %s seq %d pkt %p", "FOLLOWUP", + ntohs(hdr->sequence_id), pkt); + break; + + case GPTP_PATH_DELAY_FOLLOWUP_MESSAGE: + if (GPTP_CHECK_LEN(pkt, GPTP_PDELAY_RESP_FUP_LEN)) { + NET_WARN("Invalid length for %s packet " + "should have %zd bytes but has %lu bytes", + "PDELAY_FOLLOWUP", + GPTP_PDELAY_RESP_FUP_LEN, + GPTP_PACKET_LEN(pkt)); + GPTP_STATS_INC(port, rx_ptp_packet_discard_count); + break; + } + + PRINT_INFO("PDELAY_FOLLOWUP", hdr, pkt); + + pdelay_req_state->rcvd_pdelay_follow_up++; + + /* If we already have one, drop the received one. */ + if (pdelay_req_state->rcvd_pdelay_follow_up_ptr) { + break; + } + + /* Keep the buffer alive until pdelay_rate_ratio is computed. */ + net_pkt_ref(pkt); + pdelay_req_state->rcvd_pdelay_follow_up_ptr = pkt; + + GPTP_STATS_INC(port, rx_pdelay_resp_fup_count); + break; + + case GPTP_ANNOUNCE_MESSAGE: + if (GPTP_ANNOUNCE_CHECK_LEN(pkt)) { + NET_WARN("Invalid length for %s packet " + "should have %zd bytes but has %lu bytes", + "ANNOUNCE", + GPTP_ANNOUNCE_LEN(pkt), + GPTP_PACKET_LEN(pkt)); + GPTP_STATS_INC(port, rx_ptp_packet_discard_count); + break; + } + + PRINT_INFO("ANNOUNCE", hdr, pkt); + + pa_rcv_state = &GPTP_PORT_STATE(port)->pa_rcv; + bmca_data = GPTP_PORT_BMCA_DATA(port); + + if (pa_rcv_state->rcvd_announce == false && + bmca_data->rcvd_announce_ptr == NULL) { + pa_rcv_state->rcvd_announce = true; + bmca_data->rcvd_announce_ptr = pkt; + net_pkt_ref(pkt); + } + + GPTP_STATS_INC(port, rx_announce_count); + break; + + case GPTP_SIGNALING_MESSAGE: + if (GPTP_CHECK_LEN(pkt, GPTP_SIGNALING_LEN)) { + NET_WARN("Invalid length for %s packet " + "should have %zd bytes but has %lu bytes", + "SIGNALING", + GPTP_SIGNALING_LEN, + GPTP_PACKET_LEN(pkt)); + GPTP_STATS_INC(port, rx_ptp_packet_discard_count); + break; + } + + PRINT_INFO("SIGNALING", hdr, pkt); + + gptp_handle_signaling(port, pkt); + break; + + case GPTP_MANAGEMENT_MESSAGE: + PRINT_INFO("MANAGEMENT", hdr, pkt); + GPTP_STATS_INC(port, rx_ptp_packet_discard_count); + break; + + default: + NET_DBG("Received unknown message %x", hdr->message_type); + GPTP_STATS_INC(port, rx_ptp_packet_discard_count); + break; + } +} + +enum net_verdict net_gptp_recv(struct net_if *iface, struct net_pkt *pkt) +{ + struct gptp_hdr *hdr = GPTP_HDR(pkt); + + if ((hdr->ptp_version != GPTP_VERSION) || + (hdr->transport_specific != GPTP_TRANSPORT_802_1_AS)) { + /* The stack only supports PTP V2 and transportSpecific set + * to 1 with IEEE802.1AS-2011. + */ + return NET_DROP; + } + + /* Handle critical messages. */ + if (!gptp_handle_critical_msg(iface, pkt)) { + k_fifo_put(&gptp_rx_queue, pkt); + + /* Returning OK here makes sure the network statistics are + * properly updated. + */ + return NET_OK; + } + + /* Message not propagated up in the stack. */ + return NET_DROP; +} + +static void gptp_init_clock_ds(void) +{ + struct gptp_global_ds *global_ds; + struct gptp_default_ds *default_ds; + struct gptp_current_ds *current_ds; + struct gptp_parent_ds *parent_ds; + struct gptp_time_prop_ds *prop_ds; + + global_ds = GPTP_GLOBAL_DS(); + default_ds = GPTP_DEFAULT_DS(); + current_ds = GPTP_CURRENT_DS(); + parent_ds = GPTP_PARENT_DS(); + prop_ds = GPTP_PROPERTIES_DS(); + + /* Initialize global data set. */ + memset(global_ds, 0, sizeof(struct gptp_global_ds)); + + /* Initialize default data set. */ + + /* Compute the clock identity from the first port MAC address. */ + gptp_compute_clock_identity(GPTP_PORT_START); + + /* XXX GrandMaster capability is not supported. */ + default_ds->gm_capable = false; + default_ds->clk_quality.clock_class = GPTP_CLASS_SLAVE_ONLY; + default_ds->clk_quality.clock_accuracy = + GPTP_CLOCK_ACCURACY_UNKNOWN; + default_ds->clk_quality.offset_scaled_log_var = + GPTP_OFFSET_SCALED_LOG_VAR_UNKNOWN; + default_ds->priority1 = GPTP_PRIORITY1_NON_GM_CAPABLE; + default_ds->priority2 = GPTP_PRIORITY2_DEFAULT; + + default_ds->cur_utc_offset = 37; /* Current leap seconds TAI - UTC */ + default_ds->flags.all = 0; + default_ds->flags.octets[1] = GPTP_FLAG_TIME_TRACEABLE; + default_ds->time_source = GPTP_TS_INTERNAL_OSCILLATOR; + + /* Initialize current data set. */ + memset(current_ds, 0, sizeof(struct gptp_current_ds)); + + /* Initialize parent data set. */ + + /* parent clock id is initialized to default_ds clock id. */ + memcpy(&parent_ds->port_id.clk_id, &default_ds->clk_id, + GPTP_CLOCK_ID_LEN); + memcpy(&parent_ds->gm_id, &default_ds->clk_id, GPTP_CLOCK_ID_LEN); + parent_ds->port_id.port_number = 0; + + /* TODO: Check correct value for below field. */ + parent_ds->cumulative_rate_ratio = 0; + + parent_ds->gm_clk_quality.clock_class = + default_ds->clk_quality.clock_class; + parent_ds->gm_clk_quality.clock_accuracy = + default_ds->clk_quality.clock_accuracy; + parent_ds->gm_clk_quality.offset_scaled_log_var = + default_ds->clk_quality.offset_scaled_log_var; + parent_ds->gm_priority1 = default_ds->priority1; + parent_ds->gm_priority2 = default_ds->priority2; + + /* Initialize properties data set. */ + + /* TODO: Get accurate values for below. From the GM. */ + prop_ds->cur_utc_offset = 37; /* Current leap seconds TAI - UTC */ + prop_ds->cur_utc_offset_valid = false; + prop_ds->leap59 = false; + prop_ds->leap61 = false; + prop_ds->time_traceable = false; + prop_ds->freq_traceable = false; + prop_ds->time_source = GPTP_TS_INTERNAL_OSCILLATOR; + + /* Set system values. */ + global_ds->sys_flags.all = default_ds->flags.all; + global_ds->sys_current_utc_offset = default_ds->cur_utc_offset; + global_ds->sys_time_source = default_ds->time_source; +} + +static void gptp_init_port_ds(int port) +{ + struct gptp_default_ds *default_ds; + struct gptp_port_ds *port_ds; + +#if defined(CONFIG_NET_GPTP_STATISTICS) + struct gptp_port_param_ds *port_param_ds; + + port_param_ds = GPTP_PORT_PARAM_DS(port); +#endif + + default_ds = GPTP_DEFAULT_DS(); + port_ds = GPTP_PORT_DS(port); + + /* Initialize port data set. */ + memcpy(port_ds->port_id.clk_id, default_ds->clk_id, GPTP_CLOCK_ID_LEN); + port_ds->port_id.port_number = port; + + port_ds->ptt_port_enabled = true; + port_ds->prev_ptt_port_enabled = true; + + port_ds->neighbor_prop_delay = 0; + port_ds->neighbor_prop_delay_thresh = GPTP_NEIGHBOR_PROP_DELAY_THR; + port_ds->delay_asymmetry = 0; + + port_ds->ini_log_announce_itv = CONFIG_NET_GPTP_INIT_LOG_ANNOUNCE_ITV; + port_ds->cur_log_announce_itv = port_ds->ini_log_announce_itv; + port_ds->announce_receipt_timeout = + CONFIG_NET_GPTP_ANNOUNCE_RECEIPT_TIMEOUT; + + /* Subtract 1 to divide by 2 the sync interval. */ + port_ds->ini_log_half_sync_itv = CONFIG_NET_GPTP_INIT_LOG_SYNC_ITV - 1; + port_ds->cur_log_half_sync_itv = port_ds->ini_log_half_sync_itv; + port_ds->sync_receipt_timeout = CONFIG_NET_GPTP_SYNC_RECEIPT_TIMEOUT; + port_ds->sync_receipt_timeout_time_itv = 10000000; /* 10ms */ + + port_ds->ini_log_pdelay_req_itv = + CONFIG_NET_GPTP_INIT_LOG_PDELAY_REQ_ITV; + port_ds->cur_log_pdelay_req_itv = port_ds->ini_log_pdelay_req_itv; + port_ds->allowed_lost_responses = GPTP_ALLOWED_LOST_RESP; + port_ds->version = GPTP_VERSION; + + gptp_set_time_itv(&port_ds->pdelay_req_itv, 1, + port_ds->cur_log_pdelay_req_itv); + + gptp_set_time_itv(&port_ds->half_sync_itv, 1, + port_ds->cur_log_half_sync_itv); + + port_ds->compute_neighbor_rate_ratio = true; + port_ds->compute_neighbor_prop_delay = true; + + /* Random Sequence Numbers. */ + port_ds->sync_seq_id = (u16_t)sys_rand32_get(); + port_ds->pdelay_req_seq_id = (u16_t)sys_rand32_get(); + port_ds->announce_seq_id = (u16_t)sys_rand32_get(); + port_ds->signaling_seq_id = (u16_t)sys_rand32_get(); + +#if defined(CONFIG_NET_GPTP_STATISTICS) + /* Initialize stats data set. */ + memset(port_param_ds, 0, sizeof(struct gptp_port_param_ds)); +#endif +} + +static void gptp_init_state_machine(void) +{ + gptp_md_init_state_machine(); + gptp_mi_init_state_machine(); +} + +static void gptp_state_machine(void) +{ + int port; + + /* Manage port states. */ + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + struct gptp_port_ds *port_ds = GPTP_PORT_DS(port); + + switch (GPTP_GLOBAL_DS()->selected_role[port]) { + case GPTP_PORT_DISABLED: + case GPTP_PORT_MASTER: + case GPTP_PORT_PASSIVE: + case GPTP_PORT_SLAVE: + gptp_md_state_machines(port); + gptp_mi_port_sync_state_machines(port); + gptp_mi_port_bmca_state_machines(port); + break; + default: + NET_DBG("%s: Unknown port state", __func__); + break; + } + + port_ds->prev_ptt_port_enabled = port_ds->ptt_port_enabled; + } + + gptp_mi_state_machines(); +} + +static void gptp_thread(void) +{ + int port; + + NET_DBG("Starting PTP thread"); + + gptp_init_clock_ds(); + + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + gptp_init_port_ds(port); + GPTP_GLOBAL_DS()->selected_role[port] = GPTP_PORT_DISABLED; + } + + while (1) { + struct net_pkt *pkt; + + pkt = k_fifo_get(&gptp_rx_queue, + K_MSEC(GPTP_THREAD_WAIT_TIMEOUT_MS)); + if (pkt) { + gptp_handle_msg(pkt); + net_pkt_unref(pkt); + } + + gptp_state_machine(); + } +} + + +static void gptp_add_port(struct net_if *iface, void *user_data) +{ + int *num_ports = user_data; + struct device *clk; + + if (*num_ports >= CONFIG_NET_GPTP_NUM_PORTS) { + return; + } + + /* Check if interface has a PTP clock. */ + clk = net_eth_get_ptp_clock(iface); + if (clk) { + gptp_domain.iface[*num_ports] = iface; + net_eth_set_ptp_port(iface, *num_ports); + (*num_ports)++; + } +} + +void gptp_set_time_itv(struct gptp_uscaled_ns *interval, + u16_t seconds, + s8_t log_msg_interval) +{ + int i; + + if (seconds == 0) { + interval->low = 0; + interval->high = 0; + return; + } else if (log_msg_interval >= 96) { + /* Overflow, set maximum. */ + interval->low = UINT64_MAX; + interval->high = UINT32_MAX; + + return; + } else if (log_msg_interval <= -64) { + /* Underflow, set to 0. */ + interval->low = 0; + interval->high = 0; + return; + } + + + /* NSEC_PER_SEC is between 2^30 and 2^31, seconds is less thant 2^16, + * thus the computation will be less than 2^63. + */ + interval->low = (seconds * (u64_t)NSEC_PER_SEC) << 16; + + if (log_msg_interval <= 0) { + interval->low >>= -log_msg_interval; + interval->high = 0; + } else { + /* Find highest bit set. */ + for (i = 63; i >= 0; i--) { + if (interval->low >> i) { + break; + } + } + + if ((i + log_msg_interval) >= 96) { + /* Overflow, set maximum. */ + interval->low = UINT64_MAX; + interval->high = UINT32_MAX; + } else { + interval->high = + interval->low >> (64 - log_msg_interval); + interval->low <<= log_msg_interval; + } + } +} + +s32_t gptp_uscaled_ns_to_timer_ms(struct gptp_uscaled_ns *usns) +{ + u64_t tmp; + + if (usns->high) { + /* Do not calculate, it reaches max value. */ + return INT32_MAX; + } + + tmp = (usns->low >> 16) / USEC_PER_SEC; + if (tmp == 0) { + /* Timer must be started with a minimum value of 1. */ + return 1; + } + + if (tmp > INT32_MAX) { + return INT32_MAX; + } + + return (tmp & INT32_MAX); + +} + +static s32_t timer_get_remaining_and_stop(struct k_timer *timer) +{ + int key; + s32_t timer_value; + + key = irq_lock(); + timer_value = k_timer_remaining_get(timer); + + /* Stop timer as the period is about to be modified. */ + k_timer_stop(timer); + irq_unlock(key); + + return timer_value; +} + +static s32_t update_itv(struct gptp_uscaled_ns *itv, + s8_t *cur_log_itv, + s8_t *ini_log_itv, + s8_t new_log_itv, + s8_t correction_log_itv) +{ + switch (new_log_itv) { + case GPTP_ITV_KEEP: + break; + case GPTP_ITV_SET_TO_INIT: + *cur_log_itv = *ini_log_itv; + gptp_set_time_itv(itv, 1, *ini_log_itv); + break; + case GPTP_ITV_STOP: + default: + *cur_log_itv = new_log_itv + correction_log_itv; + gptp_set_time_itv(itv, 1, *cur_log_itv); + break; + } + + return gptp_uscaled_ns_to_timer_ms(itv); +} + +void gptp_update_pdelay_req_interval(int port, s8_t log_val) +{ + s32_t remaining; + s32_t new_itv, old_itv; + struct gptp_pdelay_req_state *state_pdelay; + struct gptp_port_ds *port_ds; + + port_ds = GPTP_PORT_DS(port); + state_pdelay = &GPTP_PORT_STATE(port)->pdelay_req; + remaining = timer_get_remaining_and_stop(&state_pdelay->pdelay_timer); + + old_itv = gptp_uscaled_ns_to_timer_ms(&port_ds->pdelay_req_itv); + new_itv = update_itv(&port_ds->pdelay_req_itv, + &port_ds->cur_log_pdelay_req_itv, + &port_ds->ini_log_pdelay_req_itv, + log_val, + 0); + + new_itv -= (old_itv-remaining); + if (new_itv <= 0) { + new_itv = 1; + } + + k_timer_start(&state_pdelay->pdelay_timer, new_itv, 0); +} + +void gptp_update_sync_interval(int port, s8_t log_val) +{ + struct gptp_pss_send_state *state_pss_send; + struct gptp_port_ds *port_ds; + s32_t new_itv, old_itv, period; + s32_t remaining; + u32_t time_spent; + + port_ds = GPTP_PORT_DS(port); + state_pss_send = &GPTP_PORT_STATE(port)->pss_send; + remaining = + timer_get_remaining_and_stop( + &state_pss_send->half_sync_itv_timer); + old_itv = gptp_uscaled_ns_to_timer_ms(&port_ds->half_sync_itv); + new_itv = update_itv(&port_ds->half_sync_itv, + &port_ds->cur_log_half_sync_itv, + &port_ds->ini_log_half_sync_itv, + log_val, + -1); + period = new_itv; + + /* Get the time spent from the start of the timer. */ + time_spent = old_itv; + if (state_pss_send->half_sync_itv_timer_expired) { + time_spent *= 2; + } + time_spent -= remaining; + + /* Calculate remaining time and if half timer has expired. */ + if ((time_spent / 2) > new_itv) { + state_pss_send->sync_itv_timer_expired = true; + state_pss_send->half_sync_itv_timer_expired = true; + new_itv = 1; + } else if (time_spent > new_itv) { + state_pss_send->sync_itv_timer_expired = false; + state_pss_send->half_sync_itv_timer_expired = true; + new_itv -= (time_spent - new_itv); + } else { + state_pss_send->sync_itv_timer_expired = false; + state_pss_send->half_sync_itv_timer_expired = false; + new_itv -= time_spent; + } + + if (new_itv <= 0) { + new_itv = 1; + } + + k_timer_start(&state_pss_send->half_sync_itv_timer, new_itv, period); +} + +void gptp_update_announce_interval(int port, s8_t log_val) +{ + s32_t remaining; + s32_t new_itv, old_itv; + struct gptp_port_announce_transmit_state *state_ann; + struct gptp_port_bmca_data *bmca_data; + struct gptp_port_ds *port_ds; + + port_ds = GPTP_PORT_DS(port); + state_ann = &GPTP_PORT_STATE(port)->pa_transmit; + bmca_data = GPTP_PORT_BMCA_DATA(port); + remaining = timer_get_remaining_and_stop( + &state_ann->ann_send_periodic_timer); + + old_itv = gptp_uscaled_ns_to_timer_ms(&bmca_data->announce_interval); + new_itv = update_itv(&bmca_data->announce_interval, + &port_ds->cur_log_announce_itv, + &port_ds->ini_log_announce_itv, + log_val, + 0); + + new_itv -= (old_itv-remaining); + if (new_itv <= 0) { + new_itv = 1; + } + + k_timer_start(&state_ann->ann_send_periodic_timer, new_itv, 0); +} + +struct port_user_data { + gptp_port_cb_t cb; + void *user_data; +}; + +static void gptp_get_port(struct net_if *iface, void *user_data) +{ + struct port_user_data *ud = user_data; + struct device *clk; + + /* Check if interface has a PTP clock. */ + clk = net_eth_get_ptp_clock(iface); + if (clk) { + int port = gptp_get_port_number(iface); + + if (port < 0) { + return; + } + + ud->cb(port, iface, ud->user_data); + } +} + +void gptp_foreach_port(gptp_port_cb_t cb, void *user_data) +{ + struct port_user_data ud = { + .cb = cb, + .user_data = user_data + }; + + net_if_foreach(gptp_get_port, &ud); +} + +struct gptp_domain *gptp_get_domain(void) +{ + return &gptp_domain; +} + +int gptp_get_port_data(struct gptp_domain *domain, + int port, + struct gptp_port_ds **port_ds, + struct gptp_port_param_ds **port_param_ds, + struct gptp_port_states **port_state, + struct gptp_port_bmca_data **port_bmca_data, + struct net_if **iface) +{ + if (domain != &gptp_domain) { + return -ENOENT; + } + + if (port < 0 || port > CONFIG_NET_GPTP_NUM_PORTS) { + return -EINVAL; + } + + if (port_ds) { + *port_ds = GPTP_PORT_DS(port); + } + + if (port_param_ds) { +#if defined(CONFIG_NET_GPTP_STATISTICS) + *port_param_ds = GPTP_PORT_PARAM_DS(port); +#else + *port_param_ds = NULL; +#endif + } + + if (port_state) { + *port_state = GPTP_PORT_STATE(port); + } + + if (port_bmca_data) { + *port_bmca_data = GPTP_PORT_BMCA_DATA(port); + } + + if (iface) { + *iface = GPTP_PORT_IFACE(port); + } + + return 0; +} + +void net_gptp_init(void) +{ + gptp_domain.default_ds.nb_ports = 0; + net_if_foreach(gptp_add_port, &gptp_domain.default_ds.nb_ports); + + /* Only initialize the state machine once the ports are known. */ + gptp_init_state_machine(); + + k_thread_create(&gptp_thread_data, gptp_stack, sizeof(gptp_stack), + (k_thread_entry_t)gptp_thread, + NULL, NULL, NULL, K_PRIO_COOP(5), 0, 0); +} diff --git a/subsys/net/l2/ethernet/gptp/gptp_data_set.h b/subsys/net/l2/ethernet/gptp/gptp_data_set.h new file mode 100644 index 00000000000000..08d81412095e70 --- /dev/null +++ b/subsys/net/l2/ethernet/gptp/gptp_data_set.h @@ -0,0 +1,592 @@ +/* + * Copyright (c) 2018 Intel Corporation. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @file + * @brief PTP data sets + * + * This is not to be included by the application. + */ + +#ifndef __GPTP_DS_H +#define __GPTP_DS_H + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(CONFIG_NET_GPTP) + +#include +#include "gptp_state.h" + +/* Parameters for PTP data sets. */ +#define GPTP_ALLOWED_LOST_RESP 3 + +#if defined(CONFIG_NET_GPTP_NEIGHBOR_PROP_DELAY_THR) +#define GPTP_NEIGHBOR_PROP_DELAY_THR CONFIG_NET_GPTP_NEIGHBOR_PROP_DELAY_THR +#else +/* See IEEE802.1AS B.3 should be less than 800ns (cur: 100us). */ +#define GPTP_NEIGHBOR_PROP_DELAY_THR 100000 +#endif + +/* Max number of ClockIdentities in pathTrace. */ +#define GPTP_MAX_PATHTRACE_SIZE CONFIG_NET_GPTP_PATH_TRACE_ELEMENTS + +/* Helpers to access gptp_domain fields. */ +#define GPTP_PORT_START 1 +#define GPTP_PORT_END (gptp_domain.default_ds.nb_ports + GPTP_PORT_START) + +#define GPTP_PORT_INDEX (port - GPTP_PORT_START) + +#define GPTP_GLOBAL_DS() (&gptp_domain.global_ds) +#define GPTP_DEFAULT_DS() (&gptp_domain.default_ds) +#define GPTP_CURRENT_DS() (&gptp_domain.current_ds) +#define GPTP_PARENT_DS() (&gptp_domain.parent_ds) +#define GPTP_PROPERTIES_DS() (&gptp_domain.properties_ds) +#define GPTP_STATE() (&gptp_domain.state) + +#define GPTP_PORT_DS(port) \ + (&gptp_domain.port_ds[port - GPTP_PORT_START]) +#define GPTP_PORT_STATE(port) \ + (&gptp_domain.port_state[port - GPTP_PORT_START]) +#define GPTP_PORT_BMCA_DATA(port) \ + (&gptp_domain.port_bmca_data[port - GPTP_PORT_START]) +#define GPTP_PORT_IFACE(port) \ + gptp_domain.iface[port - GPTP_PORT_START] + +#if defined(CONFIG_NET_GPTP_STATISTICS) +#define GPTP_PORT_PARAM_DS(port) \ + (&gptp_domain.port_param_ds[port - GPTP_PORT_START]) +#endif + +#define CLEAR_RESELECT(global_ds, port) \ + (global_ds->reselect_array &= (~(1 << (port - 1)))) +#define SET_RESELECT(global_ds, port) \ + (global_ds->reselect_array |= (1 << (port - 1))) +#define CLEAR_SELECTED(global_ds, port) \ + (global_ds->selected_array &= (~(1 << (port - 1)))) +#define SET_SELECTED(global_ds, port) \ + (global_ds->selected_array |= (1 << (port - 1))) +#define IS_SELECTED(global_ds, port) \ + ((global_ds->selected_array >> (port - 1)) & 0x1) +#define IS_RESELECT(global_ds, port) \ + ((global_ds->reselect_array >> (port - 1)) & 0x1) + +/* + * Global definition of the gPTP domain. + * Note: Only one domain is supported for now. + */ +extern struct gptp_domain gptp_domain; + +/* + * Type of TLV message received. + */ +enum gptp_tlv_type { + GPTP_TLV_MGNT = 0x0001, + GPTP_TLV_MGNT_ERR_STATUS = 0x0002, + GPTP_TLV_ORGANIZATION_EXT = 0x0003, + GPTP_TLV_REQ_UNICAST_TX = 0x0004, + GPTP_TLV_GRANT_UNICAST_TX = 0x0005, + GPTP_TLV_CANCEL_UNICAST_TX = 0x0006, + GPTP_TLV_ACK_CANCEL_UNICAST_TX = 0x0007, + GPTP_TLV_PATH_TRACE = 0x0008, + GPTP_TLV_ALT_TIME_OFFSET_INDICATOR = 0x0009, + GPTP_TLV_AUTH = 0x2000, + GPTP_TLV_AUTH_CHALLENGE = 0x2001, + GPTP_TLV_SECURITY_ASSOC_UPDATE = 0x2002, + GPTP_TLV_CUM_FREQ_SCALE_FACTOR_OFFSET = 0x2003, +}; + +/* + * Class of the local clock used for a port. + * This is used when determining the Grand Master. + */ +enum gptp_clock_class { + GPTP_CLASS_PRIMARY = 6, + GPTP_CLASS_APP_SPECIFIC = 13, + GPTP_CLASS_APP_SPECIFIC_LOST = 14, + GPTP_CLASS_PRIMARY_DEGRADED_A = 52, + GPTP_CLASS_APP_SPECIFIC_DEGRADED_A = 58, + GPTP_CLASS_PRIMARY_DEGRADED_B = 187, + GPTP_CLASS_APP_SPECIFIC_DEGRADED_B = 193, + GPTP_CLASS_OTHER = 248, + GPTP_CLASS_SLAVE_ONLY = 255, +}; + +/* + * For gPTP, only a subset are used. + * - DisabledPort + * - MasterPort + * - PassivePort + * - SlavePort + */ +enum gptp_port_state { + GPTP_PORT_INITIALIZING, + GPTP_PORT_FAULTY, + GPTP_PORT_DISABLED, + GPTP_PORT_LISTENING, + GPTP_PORT_PRE_MASTER, + GPTP_PORT_MASTER, + GPTP_PORT_PASSIVE, + GPTP_PORT_UNCALIBRATED, + GPTP_PORT_SLAVE, +}; + +enum gptp_received_info { + GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO, + GPTP_RCVD_INFO_REPEATED_MASTER_INFO, + GPTP_RCVD_INFO_INFERIOR_MASTER_INFO, + GPTP_RCVD_INFO_OTHER_INFO, +}; + +/** + * @brief Announce path trace retaining structure. + */ +struct gptp_path_trace { + /** Length of the path trace. */ + u16_t len; + + /** Path trace of the announce message. */ + u8_t path_sequence[GPTP_MAX_PATHTRACE_SIZE][GPTP_CLOCK_ID_LEN]; +}; + +/** + * @brief Per-time-aware system global variables. + * + * Not all variables from the standard are defined yet. + * The structure is to be enhanced with missing fields when those are needed. + * + * selectedRole is not defined here as it is a duplicate of the port_state + * variable declared in gptp_port_ds. + */ +struct gptp_global_ds { + /** The synchronized time computed by the ClockSlave entity. */ + struct net_ptp_time sync_receipt_time; + + /** Last Grand Master Phase Change. */ + struct gptp_scaled_ns clk_src_last_gm_phase_change; + + /** Last Grand Master Phase Change. */ + struct gptp_scaled_ns last_gm_phase_change; + + /** Path trace to be sent in announce message. */ + struct gptp_path_trace path_trace; + + /** Grand Master priority vector. */ + struct gptp_priority_vector gm_priority; + + /** Previous Grand Master priority vector. */ + struct gptp_priority_vector last_gm_priority; + + /** Global flags. */ + struct gptp_flags global_flags; + + /** System current flags. */ + struct gptp_flags sys_flags; + + /** Mean time interval between messages providing time-sync info. */ + u64_t clk_master_sync_itv; + + /** Value if current time. */ + u64_t sync_receipt_local_time; + + /** Time provided by the ClockSource entity minus the sync time. */ + s64_t clk_src_phase_offset; + + /** Fractional frequency offset of the Clock Source entity. */ + double clk_src_freq_offset; + + /** Last Grand Master Frequency Change. */ + double clk_src_last_gm_freq_change; + + /** Ratio of the frequency of the ClockSource to the LocalClock. */ + double gm_rate_ratio; + + /** Last Grand Master Frequency Change. */ + double last_gm_freq_change; + + /** Time source. */ + enum gptp_time_source time_source; + + /** System time source. */ + enum gptp_time_source sys_time_source; + + /** Selected port Roles. */ + enum gptp_port_state selected_role[CONFIG_NET_GPTP_NUM_PORTS + 1]; + + /** Reselect port bit array. */ + u32_t reselect_array; + + /** Selected port bit array. */ + u32_t selected_array; + + /** Steps removed from selected master. */ + u16_t master_steps_removed; + + /** Current UTC offset. */ + s16_t current_utc_offset; + + /** System current UTC offset. */ + s16_t sys_current_utc_offset; + + /** Time Base Indicator. */ + u16_t clk_src_time_base_indicator; + + /** Previous Time Base Indicator. */ + u16_t clk_src_time_base_indicator_prev; + + /** Grand Master Time Base Indicator. */ + u16_t gm_time_base_indicator; + + /** A Grand Master is present in the domain. */ + bool gm_present; +}; + +/** + * @brief Default Parameter Data Set. + * + * Data Set representing capabilities of the time-aware system. + */ +struct gptp_default_ds { + /** System current flags. */ + struct gptp_flags flags; + + /** Quality of the local clock. */ + struct gptp_clock_quality clk_quality; + + /* Source of time used by the Grand Master Clock. */ + enum gptp_time_source time_source; + + /** Clock Identity of the local clock. */ + u8_t clk_id[GPTP_CLOCK_ID_LEN]; + + /** Current UTC offset. */ + u16_t cur_utc_offset; + + /** Defines if this system is Grand Master capable. */ + bool gm_capable; + + /** Number of ports of the time-aware system. */ + u8_t nb_ports; + + /** Primary priority of the time-aware system. */ + u8_t priority1; + + /** Secondary priority of the time-aware system. */ + u8_t priority2; +}; + +/** + * @brief Current Parameter Data Set. + * + * Data Set representing information relative to the Grand Master. + */ +struct gptp_current_ds { + /** Last Grand Master Phase change . */ + struct gptp_scaled_ns last_gm_phase_change; + + /** Time difference between a slave and the Grand Master. */ + s64_t offset_from_master; + + /** Last Grand Master Frequency change. */ + double last_gm_freq_change; + + /** Number of times a Grand Master has changed in the domain. */ + u32_t gm_change_count; + + /** Time when the most recent Grand Master changed. */ + u32_t last_gm_chg_evt_time; + + /** Time when the most recent Grand Master phase changed. */ + u32_t last_gm_phase_chg_evt_time; + + /** Time when the most recent Grand Master frequency changed. */ + u32_t last_gm_freq_chg_evt_time; + + /** Time Base Indicator of the current Grand Master. */ + u16_t gm_timebase_indicator; + + /** Number of steps between the local clock and the Grand Master. */ + u8_t steps_removed; +}; + +/** + * @brief Parent Parameter Data Set. + * + * Data Set representing the parent capabilities. + */ +struct gptp_parent_ds { + /** Port Identity of the Master Port attached to this system. */ + struct gptp_port_identity port_id; + + /** Ratio of the frequency of the GM with the local clock. */ + s32_t cumulative_rate_ratio; + + /** Clock Identity of the Grand Master clock. */ + u8_t gm_id[GPTP_CLOCK_ID_LEN]; + + /** Clock Class of the Grand Master clock. */ + struct gptp_clock_quality gm_clk_quality; + + /** Primary Priority of the Grand Master clock. */ + u8_t gm_priority1; + + /** Secondary Priority of the Grand Master clock. */ + u8_t gm_priority2; +}; + +/** + * @brief Time Properties Parameter Data Set. + * + * Data Set representing Grand Master capabilities from the point of view + * of this system. + */ +struct gptp_time_prop_ds { + /** The time source of the Grand Master. */ + enum gptp_time_source time_source; + + /** Current UTC offset for the Grand Master. */ + u16_t cur_utc_offset; + + /** Current UTC offset valid for the Grand Master. */ + bool cur_utc_offset_valid : 1; + + /** The Grand Master will have 59s at the end of the current UTC day. + */ + bool leap59 : 1; + + /** The Grand Master will have 61s at the end of the current UTC day. + */ + bool leap61 : 1; + + /** The current UTC offset of the GM is traceable to a primary ref. */ + bool time_traceable : 1; + + /** The frequency of the Grand Master is traceable to a primary ref. */ + bool freq_traceable : 1; +}; + +/** + * @brief Port Parameter Data Set. + * + * Data Set representing port capabilities. + */ +struct gptp_port_ds { + /** Port Identity of the port. */ + struct gptp_port_identity port_id; + + /** Sync event transmission interval for the port. */ + struct gptp_uscaled_ns half_sync_itv; + + /** Path Delay Request transmission interval for the port. */ + struct gptp_uscaled_ns pdelay_req_itv; + + /** Maximum interval between sync messages. */ + u64_t sync_receipt_timeout_time_itv; + + /** Asymmetry on the link relative to the grand master time base. */ + s64_t delay_asymmetry; + + /** One way propagation time on the link attached to this port. */ + double neighbor_prop_delay; + + /** Propagation time threshold for the link attached to this port. */ + double neighbor_prop_delay_thresh; + + /** Estimate of the ratio of the frequency with the peer. */ + double neighbor_rate_ratio; + + /** Maximum number of Path Delay Requests without a response. */ + u16_t allowed_lost_responses; + + /** Current Sync sequence id for this port. */ + u16_t sync_seq_id; + + /** Current Path Delay Request sequence id for this port. */ + u16_t pdelay_req_seq_id; + + /** Current Announce sequence id for this port. */ + u16_t announce_seq_id; + + /** Current Signaling sequence id for this port. */ + u16_t signaling_seq_id; + + /** Initial Announce Interval as a Logarithm to base 2. */ + s8_t ini_log_announce_itv; + + /** Current Announce Interval as a Logarithm to base 2. */ + s8_t cur_log_announce_itv; + + /** Time without receiving announce messages before running BMCA. */ + u8_t announce_receipt_timeout; + + /** Initial Sync Interval as a Logarithm to base 2. */ + s8_t ini_log_half_sync_itv; + + /** Current Sync Interval as a Logarithm to base 2. */ + s8_t cur_log_half_sync_itv; + + /** Time without receiving sync messages before running BMCA. */ + u8_t sync_receipt_timeout; + + /** Initial Path Delay Request Interval as a Logarithm to base 2. */ + s8_t ini_log_pdelay_req_itv; + + /** Current Path Delay Request Interval as a Logarithm to base 2. */ + s8_t cur_log_pdelay_req_itv; + + /** Version of PTP running on this port. */ + u8_t version; + + /** Time synchronization and Best Master Selection enabled. */ + bool ptt_port_enabled : 1; + + /** Previous status of ptt_port_enabled. */ + bool prev_ptt_port_enabled : 1; + + /** The port is measuring the path delay. */ + bool is_measuring_delay : 1; + + /** The port is capable of running IEEE802.1AS. */ + bool as_capable : 1; + + /** Whether neighborRateRatio needs to be computed for this port. */ + bool compute_neighbor_rate_ratio : 1; + + /** Whether neighborPropDelay needs to be computed for this port. */ + bool compute_neighbor_prop_delay : 1; +}; + +/** + * @brief Port Parameter Statistics. + * + * Data Set containing statistics associated with various events. + */ +struct gptp_port_param_ds { + /** Number of Sync messages received. */ + u32_t rx_sync_count; + + /** Number of Follow Up messages received. */ + u32_t rx_fup_count; + + /** Number of Path Delay Requests messages received. */ + u32_t rx_pdelay_req_count; + + /** Number of Path Delay Response messages received. */ + u32_t rx_pdelay_resp_count; + + /** Number of Path Delay Follow Up messages received. */ + u32_t rx_pdelay_resp_fup_count; + + /** Number of Announce messages received. */ + u32_t rx_announce_count; + + /** Number of ptp messages discarded. */ + u32_t rx_ptp_packet_discard_count; + + /** Number of Sync reception timeout. */ + u32_t sync_receipt_timeout_count; + + /** Number of Announce reception timeout. */ + u32_t announce_receipt_timeout_count; + + /** Number Path Delay Requests without a response. */ + u32_t pdelay_allowed_lost_resp_exceed_count; + + /** Number of Sync messages sent. */ + u32_t tx_sync_count; + + /** Number of Follow Up messages sent. */ + u32_t tx_fup_count; + + /** Number of Path Delay Request messages sent. */ + u32_t tx_pdelay_req_count; + + /** Number of Path Delay Response messages sent. */ + u32_t tx_pdelay_resp_count; + + /** Number of Path Delay Response messages sent. */ + u32_t tx_pdelay_resp_fup_count; + + /** Number of Announce messages sent. */ + u32_t tx_announce_count; + + /** Neighbor propagation delay threshold exceeded. */ + u32_t neighbor_prop_delay_exceeded; +}; + +/** + * @brief gPTP domain. + * + * Data Set containing all the information necessary to represent + * one time-aware system domain. + */ +struct gptp_domain { + /** Global Data Set for this gPTP domain. */ + struct gptp_global_ds global_ds; + + /** Default Data Set for this gPTP domain. */ + struct gptp_default_ds default_ds; + + /** Current Data Set for this gPTP domain. */ + struct gptp_current_ds current_ds; + + /** Parent Data Set for this gPTP domain. */ + struct gptp_parent_ds parent_ds; + + /** Time Properties Data Set for this gPTP domain. */ + struct gptp_time_prop_ds properties_ds; + + /** Current State of the MI State Machines for this gPTP domain. */ + struct gptp_states state; + + /** Port Parameter Data Sets for this gPTP domain. */ + struct gptp_port_ds port_ds[CONFIG_NET_GPTP_NUM_PORTS]; + +#if defined(CONFIG_NET_GPTP_STATISTICS) + /** Port Parameter Statistics Data Sets for this gPTP domain. */ + struct gptp_port_param_ds port_param_ds[CONFIG_NET_GPTP_NUM_PORTS]; +#endif /* CONFIG_NET_GPTP_STATISTICS */ + + /** Current States of the MD State Machines for this gPTP domain. */ + struct gptp_port_states port_state[CONFIG_NET_GPTP_NUM_PORTS]; + + /** Shared data between BMCA State Machines for this gPTP domain. */ + struct gptp_port_bmca_data port_bmca_data[CONFIG_NET_GPTP_NUM_PORTS]; + + /* Network interface linked to the PTP PORT. */ + struct net_if *iface[CONFIG_NET_GPTP_NUM_PORTS]; +}; + +/** + * @brief Get port specific data from gPTP domain. + * @details This contains all the configuration / status of the gPTP domain. + * + * @param domain gPTP domain + * @param port Port id + * @param port_ds Port specific parameter data set (returned to caller) + * @param port_param_ds Port parameter statistics data set (returned to caller) + * @param port_state Port specific states data set (returned to caller) + * @param port_bmca Port BMCA state machine specific data (returned to caller) + * @param iface Port specific parameter data set (returned to caller) + * + * @return 0 if ok, < 0 if error + */ +int gptp_get_port_data(struct gptp_domain *domain, int port, + struct gptp_port_ds **port_ds, + struct gptp_port_param_ds **port_param_ds, + struct gptp_port_states **port_state, + struct gptp_port_bmca_data **port_bmca_data, + struct net_if **iface); + +#endif /* CONFIG_NET_GPTP */ + +#ifdef __cplusplus +} +#endif + +#endif /* __GPTP_DS_H */ diff --git a/subsys/net/l2/ethernet/gptp/gptp_md.c b/subsys/net/l2/ethernet/gptp/gptp_md.c new file mode 100644 index 00000000000000..087bc3e4a28b7e --- /dev/null +++ b/subsys/net/l2/ethernet/gptp/gptp_md.c @@ -0,0 +1,862 @@ +/* + * Copyright (c) 2017 Intel Corporation. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#if defined(CONFIG_NET_DEBUG_GPTP) +#define SYS_LOG_DOMAIN "net/gptp" +#define NET_LOG_ENABLED 1 +#endif + +#include "gptp_messages.h" +#include "gptp_md.h" +#include "gptp_data_set.h" +#include "gptp_private.h" + +static void gptp_md_sync_prepare(struct net_pkt *pkt, + struct gptp_md_sync_info *sync_send) +{ + struct gptp_hdr *hdr; + + hdr = GPTP_HDR(pkt); + + memcpy(&hdr->port_id, &sync_send->src_port_id, + sizeof(struct gptp_port_identity)); + hdr->log_msg_interval = sync_send->log_msg_interval; +} + +static void gptp_md_follow_up_prepare(struct net_pkt *pkt, + struct gptp_md_sync_info *sync_send) +{ + struct gptp_hdr *hdr; + struct gptp_follow_up *fup; + + hdr = GPTP_HDR(pkt); + fup = GPTP_FOLLOW_UP(pkt); + + /* + * Compute correction field according to IEEE802.1AS 11.2.14.2.3. + * + * The correction_field already contains the timestamp of the sync + * message. + * + * TODO: if the value to be stored in correction_field is too big to + * be represented, the field should be set to all 1's except the most + * significant bit. + */ + hdr->correction_field -= sync_send->upstream_tx_time; + hdr->correction_field *= sync_send->rate_ratio; + hdr->correction_field += sync_send->follow_up_correction_field; + hdr->correction_field <<= 16; + + memcpy(&hdr->port_id, &sync_send->src_port_id, + sizeof(struct gptp_port_identity)); + hdr->log_msg_interval = sync_send->log_msg_interval; + + fup->prec_orig_ts_secs_high = + htons(sync_send->precise_orig_ts._sec.high); + fup->prec_orig_ts_secs_low = + htonl(sync_send->precise_orig_ts._sec.low); + fup->prec_orig_ts_nsecs = + htonl(sync_send->precise_orig_ts.nanosecond); + + fup->tlv.type = htons(GPTP_TLV_ORGANIZATION_EXT); + fup->tlv.len = htons(sizeof(struct gptp_follow_up_tlv)); + fup->tlv.org_id[0] = GPTP_FUP_TLV_ORG_ID_BYTE_0; + fup->tlv.org_id[1] = GPTP_FUP_TLV_ORG_ID_BYTE_1; + fup->tlv.org_id[2] = GPTP_FUP_TLV_ORG_ID_BYTE_2; + fup->tlv.org_sub_type[0] = 0; + fup->tlv.org_sub_type[1] = 0; + fup->tlv.org_sub_type[2] = GPTP_FUP_TLV_ORG_SUB_TYPE; + + fup->tlv.cumulative_scaled_rate_offset = + (sync_send->rate_ratio - 1.0) * GPTP_POW2(41); + fup->tlv.cumulative_scaled_rate_offset = + ntohl(fup->tlv.cumulative_scaled_rate_offset); + fup->tlv.gm_time_base_indicator = + ntohs(sync_send->gm_time_base_indicator); + fup->tlv.last_gm_phase_change.high = + ntohl(sync_send->last_gm_phase_change.high); + fup->tlv.last_gm_phase_change.low = + ntohll(sync_send->last_gm_phase_change.low); + fup->tlv.scaled_last_gm_freq_change = sync_send->last_gm_freq_change; + fup->tlv.scaled_last_gm_freq_change = + ntohl(fup->tlv.scaled_last_gm_freq_change); +} + +static int gptp_set_md_sync_receive(int port, + struct gptp_md_sync_info *sync_rcv) +{ + struct gptp_sync_rcv_state *state; + struct gptp_port_ds *port_ds; + struct gptp_hdr *sync_hdr, *fup_hdr; + struct gptp_follow_up *fup; + struct net_ptp_time *sync_ts; + double prop_delay_rated; + double delay_asymmetry_rated; + + state = &GPTP_PORT_STATE(port)->sync_rcv; + if (!state->rcvd_sync_ptr || !state->rcvd_follow_up_ptr) { + return -EINVAL; + } + + port_ds = GPTP_PORT_DS(port); + + sync_hdr = GPTP_HDR(state->rcvd_sync_ptr); + fup_hdr = GPTP_HDR(state->rcvd_follow_up_ptr); + fup = GPTP_FOLLOW_UP(state->rcvd_follow_up_ptr); + sync_ts = &state->rcvd_sync_ptr->timestamp; + + sync_rcv->follow_up_correction_field = + ntohll(fup_hdr->correction_field); + memcpy(&sync_rcv->src_port_id, &sync_hdr->port_id, + sizeof(struct gptp_port_identity)); + sync_rcv->log_msg_interval = fup_hdr->log_msg_interval; + sync_rcv->precise_orig_ts._sec.high = + ntohs(fup->prec_orig_ts_secs_high); + sync_rcv->precise_orig_ts._sec.low = ntohl(fup->prec_orig_ts_secs_low); + sync_rcv->precise_orig_ts.nanosecond = ntohl(fup->prec_orig_ts_nsecs); + + /* Compute time when sync was sent by the remote. */ + sync_rcv->upstream_tx_time = sync_ts->second; + sync_rcv->upstream_tx_time *= NSEC_PER_SEC; + sync_rcv->upstream_tx_time += sync_ts->nanosecond; + + prop_delay_rated = port_ds->neighbor_prop_delay; + prop_delay_rated /= port_ds->neighbor_rate_ratio; + + sync_rcv->upstream_tx_time -= prop_delay_rated; + + delay_asymmetry_rated = port_ds->delay_asymmetry; + delay_asymmetry_rated /= port_ds->neighbor_rate_ratio; + + sync_rcv->upstream_tx_time -= delay_asymmetry_rated; + + sync_rcv->rate_ratio = ntohl(fup->tlv.cumulative_scaled_rate_offset); + sync_rcv->rate_ratio *= GPTP_POW2(-41); + sync_rcv->rate_ratio += 1; + + sync_rcv->gm_time_base_indicator = + ntohs(fup->tlv.gm_time_base_indicator); + sync_rcv->last_gm_phase_change.high = + ntohl(fup->tlv.last_gm_phase_change.high); + sync_rcv->last_gm_phase_change.low = + ntohll(fup->tlv.last_gm_phase_change.low); + sync_rcv->last_gm_freq_change = + ntohl(fup->tlv.scaled_last_gm_freq_change); + + return 0; +} + +static void gptp_md_pdelay_reset(int port) +{ + struct gptp_pdelay_req_state *state; + struct gptp_port_ds *port_ds; + + NET_WARN("Reset Pdelay requests"); + + state = &GPTP_PORT_STATE(port)->pdelay_req; + port_ds = GPTP_PORT_DS(port); + + if (state->lost_responses < port_ds->allowed_lost_responses) { + state->lost_responses += 1; + } else { + port_ds->is_measuring_delay = false; + port_ds->as_capable = false; + state->init_pdelay_compute = true; + } +} + +static void gptp_md_pdelay_check_multiple_resp(int port) +{ + struct gptp_pdelay_req_state *state; + struct gptp_port_ds *port_ds; + int duration; + + state = &GPTP_PORT_STATE(port)->pdelay_req; + port_ds = GPTP_PORT_DS(port); + + if ((state->rcvd_pdelay_resp > 1) || + (state->rcvd_pdelay_follow_up > 1)) { + port_ds->as_capable = false; + NET_WARN("Too many responses (%d / %d)", + state->rcvd_pdelay_resp, + state->rcvd_pdelay_follow_up); + state->multiple_resp_count++; + } else { + state->multiple_resp_count = 0; + } + + if (state->multiple_resp_count >= 3) { + state->multiple_resp_count = 0; + k_timer_stop(&state->pdelay_timer); + state->pdelay_timer_expired = false; + + /* Subtract time spent since last pDelay request. */ + duration = GPTP_MULTIPLE_PDELAY_RESP_WAIT - + gptp_uscaled_ns_to_timer_ms(&port_ds->pdelay_req_itv); + + k_timer_start(&state->pdelay_timer, duration, 0); + } else { + state->state = GPTP_PDELAY_REQ_SEND_REQ; + } +} + +static void gptp_md_compute_pdelay_rate_ratio(int port) +{ + u64_t ingress_tstamp = 0; + u64_t resp_evt_tstamp = 0; + struct gptp_pdelay_resp_follow_up *fup; + struct gptp_pdelay_req_state *state; + struct gptp_port_ds *port_ds; + struct net_pkt *pkt; + struct gptp_hdr *hdr; + double neighbor_rate_ratio; + + state = &GPTP_PORT_STATE(port)->pdelay_req; + port_ds = GPTP_PORT_DS(port); + + /* Get ingress timestamp. */ + pkt = state->rcvd_pdelay_resp_ptr; + if (pkt) { + ingress_tstamp = + gptp_timestamp_to_nsec(net_pkt_timestamp(pkt)); + } + + /* Get peer corrected timestamp. */ + pkt = state->rcvd_pdelay_follow_up_ptr; + if (pkt) { + hdr = GPTP_HDR(pkt); + fup = GPTP_PDELAY_RESP_FOLLOWUP(pkt); + + resp_evt_tstamp = ntohs(fup->resp_orig_ts_secs_high); + resp_evt_tstamp <<= 32; + resp_evt_tstamp |= ntohl(fup->resp_orig_ts_secs_low); + resp_evt_tstamp *= NSEC_PER_SEC; + resp_evt_tstamp += ntohl(fup->resp_orig_ts_nsecs); + resp_evt_tstamp += (ntohll(hdr->correction_field) >> 16); + } + + if (state->init_pdelay_compute) { + state->init_pdelay_compute = false; + + state->ini_resp_ingress_tstamp = ingress_tstamp; + state->ini_resp_evt_tstamp = resp_evt_tstamp; + + neighbor_rate_ratio = 1.0; + + state->neighbor_rate_ratio_valid = false; + } else { + neighbor_rate_ratio = + (resp_evt_tstamp - state->ini_resp_evt_tstamp); + neighbor_rate_ratio /= + (ingress_tstamp - state->ini_resp_ingress_tstamp); + + /* Measure the ratio with the previously sent response. */ + state->ini_resp_ingress_tstamp = ingress_tstamp; + state->ini_resp_evt_tstamp = resp_evt_tstamp; + state->neighbor_rate_ratio_valid = true; + } + + port_ds->neighbor_rate_ratio = neighbor_rate_ratio; +} + +static void gptp_md_compute_prop_time(int port) +{ + u64_t t1_ns = 0, t2_ns = 0, t3_ns = 0, t4_ns = 0; + struct gptp_pdelay_resp_follow_up *fup; + struct gptp_pdelay_req_state *state; + struct gptp_pdelay_resp *resp; + struct gptp_port_ds *port_ds; + struct gptp_hdr *hdr; + struct net_pkt *pkt; + double prop_time; + + state = &GPTP_PORT_STATE(port)->pdelay_req; + port_ds = GPTP_PORT_DS(port); + + /* Get egress timestamp. */ + pkt = state->tx_pdelay_req_ptr; + if (pkt) { + t1_ns = gptp_timestamp_to_nsec(net_pkt_timestamp(pkt)); + } + + /* Get ingress timestamp. */ + pkt = state->rcvd_pdelay_resp_ptr; + if (pkt) { + t4_ns = gptp_timestamp_to_nsec(net_pkt_timestamp(pkt)); + } + + /* Get peer corrected timestamps. */ + pkt = state->rcvd_pdelay_resp_ptr; + if (pkt) { + hdr = GPTP_HDR(pkt); + resp = GPTP_PDELAY_RESP(pkt); + + t2_ns = ((u64_t)ntohs(resp->req_receipt_ts_secs_high)) << 32; + t2_ns |= ntohl(resp->req_receipt_ts_secs_low); + t2_ns *= NSEC_PER_SEC; + t2_ns += ntohl(resp->req_receipt_ts_nsecs); + t2_ns += (ntohll(hdr->correction_field) >> 16); + } + + pkt = state->rcvd_pdelay_follow_up_ptr; + if (pkt) { + hdr = GPTP_HDR(pkt); + fup = GPTP_PDELAY_RESP_FOLLOWUP(pkt); + + t3_ns = ((u64_t)ntohs(fup->resp_orig_ts_secs_high)) << 32; + t3_ns |= ntohl(fup->resp_orig_ts_secs_low); + t3_ns *= NSEC_PER_SEC; + t3_ns += ntohl(fup->resp_orig_ts_nsecs); + t3_ns += (ntohll(hdr->correction_field) >> 16); + } + + prop_time = (t4_ns - t1_ns); + prop_time *= port_ds->neighbor_rate_ratio; + prop_time -= (t3_ns - t2_ns); + prop_time /= 2; + + port_ds->neighbor_prop_delay = prop_time; +} + +static void gptp_md_pdelay_compute(int port) +{ + struct gptp_pdelay_req_state *state; + struct gptp_port_ds *port_ds; + struct gptp_hdr *hdr; + struct net_pkt *pkt; + bool local_clock; + + state = &GPTP_PORT_STATE(port)->pdelay_req; + port_ds = GPTP_PORT_DS(port); + + if (!state->tx_pdelay_req_ptr || !state->rcvd_pdelay_resp_ptr || + !state->rcvd_pdelay_follow_up_ptr) { + NET_ERR("Compute path delay called without buffer ready"); + port_ds->as_capable = false; + goto out; + } + + if (port_ds->compute_neighbor_rate_ratio) { + gptp_md_compute_pdelay_rate_ratio(port); + } + + if (port_ds->compute_neighbor_prop_delay) { + gptp_md_compute_prop_time(port); + } + + state->lost_responses = 0; + port_ds->is_measuring_delay = true; + + pkt = state->rcvd_pdelay_follow_up_ptr; + hdr = GPTP_HDR(pkt); + + local_clock = !memcmp(gptp_domain.default_ds.clk_id, + hdr->port_id.clk_id, + GPTP_CLOCK_ID_LEN); + if (local_clock) { + NET_WARN("Discard path delay response from local clock."); + goto out; + } + + if (!state->neighbor_rate_ratio_valid) { + goto out; + } + + /* + * Currently, if the computed delay is negative, this means + * that it is negligeable enough compared to other factors. + */ + if ((port_ds->neighbor_prop_delay <= + port_ds->neighbor_prop_delay_thresh)) { + port_ds->as_capable = true; + } else { + port_ds->as_capable = false; + + NET_WARN("Not AS capable: %u ns > %u ns", + (u32_t)port_ds->neighbor_prop_delay, + (u32_t)port_ds->neighbor_prop_delay_thresh); + + GPTP_STATS_INC(port, neighbor_prop_delay_exceeded); + } + +out: + /* Release buffers. */ + if (state->tx_pdelay_req_ptr) { + net_pkt_unref(state->tx_pdelay_req_ptr); + state->tx_pdelay_req_ptr = NULL; + } + + if (state->rcvd_pdelay_resp_ptr) { + net_pkt_unref(state->rcvd_pdelay_resp_ptr); + state->rcvd_pdelay_resp_ptr = NULL; + } + + if (state->rcvd_pdelay_follow_up_ptr) { + net_pkt_unref(state->rcvd_pdelay_follow_up_ptr); + state->rcvd_pdelay_follow_up_ptr = NULL; + } +} + +static void gptp_md_pdelay_req_timeout(struct k_timer *timer) +{ + struct gptp_pdelay_req_state *state; + int port; + + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + state = &GPTP_PORT_STATE(port)->pdelay_req; + if (timer == &state->pdelay_timer) { + state->pdelay_timer_expired = true; + + GPTP_STATS_INC(port, + pdelay_allowed_lost_resp_exceed_count); + } + } +} + +static void gptp_md_start_pdelay_req(int port) +{ + struct gptp_pdelay_req_state *state; + struct gptp_port_ds *port_ds; + + port_ds = GPTP_PORT_DS(port); + state = &GPTP_PORT_STATE(port)->pdelay_req; + + port_ds->neighbor_rate_ratio = 1.0; + port_ds->is_measuring_delay = false; + port_ds->as_capable = false; + state->lost_responses = 0; + state->rcvd_pdelay_resp = 0; + state->rcvd_pdelay_follow_up = 0; + state->multiple_resp_count = 0; +} + +static void gptp_md_follow_up_receipt_timeout(struct k_timer *timer) +{ + struct gptp_sync_rcv_state *state; + int port; + + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + state = &GPTP_PORT_STATE(port)->sync_rcv; + if (timer == &state->follow_up_discard_timer) { + NET_WARN("No %s received after %s message", + "FOLLOWUP", "SYNC"); + state->follow_up_timeout_expired = true; + } + } +} + +static void gptp_md_init_pdelay_req_state_machine(int port) +{ + struct gptp_pdelay_req_state *state; + + state = &GPTP_PORT_STATE(port)->pdelay_req; + + k_timer_init(&state->pdelay_timer, gptp_md_pdelay_req_timeout, NULL); + + state->state = GPTP_PDELAY_REQ_NOT_ENABLED; + + state->neighbor_rate_ratio_valid = false; + state->init_pdelay_compute = true; + state->rcvd_pdelay_resp = 0; + state->rcvd_pdelay_follow_up = 0; + state->pdelay_timer_expired = false; + + state->rcvd_pdelay_resp_ptr = NULL; + state->rcvd_pdelay_follow_up_ptr = NULL; + state->tx_pdelay_req_ptr = NULL; + + state->ini_resp_evt_tstamp = 0; + state->ini_resp_ingress_tstamp = 0; + state->lost_responses = 0; +} + +static void gptp_md_init_pdelay_resp_state_machine(int port) +{ + struct gptp_pdelay_resp_state *state; + + state = &GPTP_PORT_STATE(port)->pdelay_resp; + + state->state = GPTP_PDELAY_RESP_NOT_ENABLED; +} + +static void gptp_md_init_sync_rcv_state_machine(int port) +{ + struct gptp_sync_rcv_state *state; + + state = &GPTP_PORT_STATE(port)->sync_rcv; + + k_timer_init(&state->follow_up_discard_timer, + gptp_md_follow_up_receipt_timeout, NULL); + + state->rcvd_sync = false; + state->rcvd_follow_up = false; + state->rcvd_sync_ptr = NULL; + state->rcvd_follow_up_ptr = NULL; + + state->follow_up_timeout_expired = false; + state->follow_up_receipt_timeout = 0; + + state->state = GPTP_SYNC_RCV_DISCARD; +} + +static void gptp_md_init_sync_send_state_machine(int port) +{ + struct gptp_sync_send_state *state; + + state = &GPTP_PORT_STATE(port)->sync_send; + + state->rcvd_md_sync = false; + state->md_sync_timestamp_avail = false; + state->sync_send_ptr = NULL; + state->sync_ptr = NULL; + + state->state = GPTP_SYNC_SEND_INITIALIZING; +} + +void gptp_md_init_state_machine(void) +{ + int port; + + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + gptp_md_init_pdelay_req_state_machine(port); + gptp_md_init_pdelay_resp_state_machine(port); + gptp_md_init_sync_rcv_state_machine(port); + gptp_md_init_sync_send_state_machine(port); + } +} + +static void gptp_md_pdelay_req_state_machine(int port) +{ + struct gptp_port_ds *port_ds; + struct gptp_pdelay_req_state *state; + struct net_pkt *pkt; + + state = &GPTP_PORT_STATE(port)->pdelay_req; + port_ds = GPTP_PORT_DS(port); + + /* Unset AS-Capable if multiple responses to a pDelay request have been + * reveived. + */ + if (state->rcvd_pdelay_resp > 1 || state->rcvd_pdelay_follow_up > 1) { + port_ds->as_capable = false; + } + + if (!port_ds->ptt_port_enabled) { + /* Make sure the timer is stopped. */ + k_timer_stop(&state->pdelay_timer); + state->state = GPTP_PDELAY_REQ_NOT_ENABLED; + } + + switch (state->state) { + case GPTP_PDELAY_REQ_NOT_ENABLED: + if (port_ds->ptt_port_enabled) { + /* (Re)Init interval (as defined in + * LinkDelaySyncIntervalSetting state machine). + */ + port_ds->cur_log_pdelay_req_itv = + port_ds->ini_log_pdelay_req_itv; + + gptp_set_time_itv(&port_ds->pdelay_req_itv, 1, + port_ds->cur_log_pdelay_req_itv); + + port_ds->compute_neighbor_rate_ratio = true; + port_ds->compute_neighbor_prop_delay = true; + + state->pdelay_timer_expired = true; + state->state = GPTP_PDELAY_REQ_INITIAL_SEND_REQ; + } + + break; + + case GPTP_PDELAY_REQ_RESET: + gptp_md_pdelay_reset(port); + /* Send a request on the next timer expiry. */ + state->state = GPTP_PDELAY_REQ_WAIT_ITV_TIMER; + break; + + case GPTP_PDELAY_REQ_INITIAL_SEND_REQ: + gptp_md_start_pdelay_req(port); + + case GPTP_PDELAY_REQ_SEND_REQ: + if (state->tx_pdelay_req_ptr) { + net_pkt_unref(state->tx_pdelay_req_ptr); + state->tx_pdelay_req_ptr = NULL; + } + + if (state->rcvd_pdelay_resp_ptr) { + net_pkt_unref(state->rcvd_pdelay_resp_ptr); + state->rcvd_pdelay_resp_ptr = NULL; + } + + if (state->rcvd_pdelay_follow_up_ptr) { + net_pkt_unref(state->rcvd_pdelay_follow_up_ptr); + state->rcvd_pdelay_follow_up_ptr = NULL; + } + + gptp_send_pdelay_req(port); + + k_timer_stop(&state->pdelay_timer); + state->pdelay_timer_expired = false; + k_timer_start(&state->pdelay_timer, + gptp_uscaled_ns_to_timer_ms( + &port_ds->pdelay_req_itv), + 0); + /* + * Transition directly to GPTP_PDELAY_REQ_WAIT_RESP. + * Check for the TX timestamp will be done during + * the computation of the path delay. + */ + state->state = GPTP_PDELAY_REQ_WAIT_RESP; + break; + + case GPTP_PDELAY_REQ_WAIT_RESP: + if (state->pdelay_timer_expired) { + state->state = GPTP_PDELAY_REQ_RESET; + } else if (state->rcvd_pdelay_resp != 0) { + pkt = state->rcvd_pdelay_resp_ptr; + if (!gptp_handle_pdelay_resp(port, pkt)) { + state->state = GPTP_PDELAY_REQ_WAIT_FOLLOW_UP; + } else { + state->state = GPTP_PDELAY_REQ_RESET; + } + } + + break; + + case GPTP_PDELAY_REQ_WAIT_FOLLOW_UP: + if (state->pdelay_timer_expired) { + state->state = GPTP_PDELAY_REQ_RESET; + } else if (state->rcvd_pdelay_follow_up != 0) { + pkt = state->rcvd_pdelay_follow_up_ptr; + if (!gptp_handle_pdelay_follow_up(port, pkt)) { + gptp_md_pdelay_compute(port); + state->state = GPTP_PDELAY_REQ_WAIT_ITV_TIMER; + } else { + state->state = GPTP_PDELAY_REQ_RESET; + } + } + + break; + + case GPTP_PDELAY_REQ_WAIT_ITV_TIMER: + if (state->pdelay_timer_expired) { + gptp_md_pdelay_check_multiple_resp(port); + + state->rcvd_pdelay_resp = 0; + state->rcvd_pdelay_follow_up = 0; + } + + break; + } +} + +static void gptp_md_pdelay_resp_state_machine(int port) +{ + struct gptp_port_ds *port_ds; + struct gptp_pdelay_resp_state *state; + + state = &GPTP_PORT_STATE(port)->pdelay_resp; + port_ds = GPTP_PORT_DS(port); + + if (!port_ds->ptt_port_enabled) { + state->state = GPTP_PDELAY_RESP_NOT_ENABLED; + } + + switch (state->state) { + case GPTP_PDELAY_RESP_NOT_ENABLED: + if (port_ds->ptt_port_enabled) { + state->state = GPTP_PDELAY_RESP_INITIAL_WAIT_REQ; + } + + break; + + case GPTP_PDELAY_RESP_INITIAL_WAIT_REQ: + case GPTP_PDELAY_RESP_WAIT_REQ: + /* Handled in gptp_handle_msg for latency considerations. */ + break; + + case GPTP_PDELAY_RESP_WAIT_TSTAMP: + /* Handled in gptp_follow_up_callback. */ + break; + } + +} + +static void gptp_md_sync_receive_state_machine(int port) +{ + struct gptp_port_ds *port_ds; + struct gptp_sync_rcv_state *state; + struct gptp_pss_rcv_state *pss_state; + + state = &GPTP_PORT_STATE(port)->sync_rcv; + pss_state = &GPTP_PORT_STATE(port)->pss_rcv; + port_ds = GPTP_PORT_DS(port); + + if ((!port_ds->ptt_port_enabled) || !port_ds->as_capable) { + /* Make sure the timer is stopped. */ + k_timer_stop(&state->follow_up_discard_timer); + + /* Discard all received messages. */ + if (state->rcvd_sync_ptr) { + net_pkt_unref(state->rcvd_sync_ptr); + state->rcvd_sync_ptr = NULL; + } + + if (state->rcvd_follow_up_ptr) { + net_pkt_unref(state->rcvd_follow_up_ptr); + state->rcvd_follow_up_ptr = NULL; + } + + state->rcvd_sync = false; + state->rcvd_follow_up = false; + state->state = GPTP_SYNC_RCV_DISCARD; + return; + } + + switch (state->state) { + case GPTP_SYNC_RCV_DISCARD: + case GPTP_SYNC_RCV_WAIT_SYNC: + if (state->rcvd_sync) { + gptp_handle_sync(port, state->rcvd_sync_ptr); + state->rcvd_sync = false; + state->state = GPTP_SYNC_RCV_WAIT_FOLLOW_UP; + } else if (state->rcvd_follow_up) { + /* Delete late/early message. */ + if (state->rcvd_follow_up_ptr) { + net_pkt_unref(state->rcvd_follow_up_ptr); + state->rcvd_follow_up_ptr = NULL; + } + + state->rcvd_follow_up = false; + } + + break; + + case GPTP_SYNC_RCV_WAIT_FOLLOW_UP: + /* Never received a follow up for a sync message. */ + if (state->follow_up_timeout_expired) { + k_timer_stop(&state->follow_up_discard_timer); + state->follow_up_timeout_expired = false; + state->state = GPTP_SYNC_RCV_DISCARD; + if (state->rcvd_sync_ptr) { + net_pkt_unref(state->rcvd_sync_ptr); + state->rcvd_sync_ptr = NULL; + } + + state->rcvd_sync = false; + } else if (state->rcvd_sync) { + /* Handle received extra sync. */ + gptp_handle_sync(port, state->rcvd_sync_ptr); + state->rcvd_sync = false; + } else if (state->rcvd_follow_up) { + if (!gptp_handle_follow_up( + port, state->rcvd_follow_up_ptr)) { + + /* + * Fill the structure to be sent to + * PortSyncSyncReceive. + */ + gptp_set_md_sync_receive(port, + &pss_state->sync_rcv); + + pss_state->rcvd_md_sync = true; + + state->state = GPTP_SYNC_RCV_WAIT_SYNC; + + /* Buffers can be released now. */ + if (state->rcvd_sync_ptr) { + net_pkt_unref(state->rcvd_sync_ptr); + state->rcvd_sync_ptr = NULL; + } + + k_timer_stop(&state->follow_up_discard_timer); + state->follow_up_timeout_expired = false; + } + } + + if (state->rcvd_follow_up_ptr) { + net_pkt_unref(state->rcvd_follow_up_ptr); + state->rcvd_follow_up_ptr = NULL; + } + + state->rcvd_follow_up = false; + break; + } +} + +static void gptp_md_sync_send_state_machine(int port) +{ + struct gptp_port_ds *port_ds; + struct gptp_sync_send_state *state; + struct net_pkt *pkt; + + state = &GPTP_PORT_STATE(port)->sync_send; + port_ds = GPTP_PORT_DS(port); + + if ((!port_ds->ptt_port_enabled) || !port_ds->as_capable) { + state->rcvd_md_sync = false; + state->state = GPTP_SYNC_SEND_INITIALIZING; + + /* Sync sequence id is initialized in the port_ds init function + */ + return; + } + + switch (state->state) { + case GPTP_SYNC_SEND_INITIALIZING: + state->state = GPTP_SYNC_SEND_SEND_SYNC; + break; + + case GPTP_SYNC_SEND_SEND_SYNC: + if (state->rcvd_md_sync) { + pkt = gptp_prepare_sync(port); + if (pkt) { + /* Reference message to track timestamp info */ + state->sync_ptr = net_pkt_ref(pkt); + gptp_md_sync_prepare(pkt, + state->sync_send_ptr); + gptp_send_sync(port, pkt); + } + + state->rcvd_md_sync = false; + state->state = GPTP_SYNC_SEND_SEND_FUP; + } + + break; + + case GPTP_SYNC_SEND_SEND_FUP: + if (state->md_sync_timestamp_avail) { + state->md_sync_timestamp_avail = false; + + if (!state->sync_ptr) { + NET_ERR("Sync message not available"); + break; + } + + pkt = gptp_prepare_follow_up(port, state->sync_ptr); + if (pkt) { + gptp_md_follow_up_prepare(pkt, + state->sync_send_ptr); + gptp_send_follow_up(port, pkt); + } + + net_pkt_unref(state->sync_ptr); + state->sync_ptr = NULL; + + state->state = GPTP_SYNC_SEND_SEND_SYNC; + } + + break; + } +} + +void gptp_md_state_machines(int port) +{ + gptp_md_pdelay_req_state_machine(port); + gptp_md_pdelay_resp_state_machine(port); + gptp_md_sync_receive_state_machine(port); + gptp_md_sync_send_state_machine(port); +} diff --git a/subsys/net/l2/ethernet/gptp/gptp_md.h b/subsys/net/l2/ethernet/gptp/gptp_md.h new file mode 100644 index 00000000000000..06ff11258fdd0b --- /dev/null +++ b/subsys/net/l2/ethernet/gptp/gptp_md.h @@ -0,0 +1,77 @@ +/* + * Copyright (c) 2017 Intel Corporation. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @file + * @brief GPTP Media Dependent interface for full duplex and point to point + * + * This is not to be included by the application. + */ + +#ifndef __GPTP_MD_H +#define __GPTP_MD_H + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(CONFIG_NET_GPTP) + +#include + +/** + * @brief Media Dependent Sync Information. + * + * This structure applies for MDSyncReceive as well as MDSyncSend. + */ +struct gptp_md_sync_info { + /** PortIdentity of this port. */ + struct gptp_port_identity src_port_id; + + /* Time of the current grandmaster compared to the previous. */ + struct gptp_scaled_ns last_gm_phase_change; + + /** Most recent preciseOriginTimestamp from the PortSyncSync. */ + struct net_ptp_time precise_orig_ts; + + /** Most recent followupCorrectionField from the PortSyncSync. */ + s64_t follow_up_correction_field; + + /** Most recent upstreamTxTime from the PortSyncSync. */ + u64_t upstream_tx_time; + + /* Frequency of the current grandmaster compared to the previous. */ + double last_gm_freq_change; + + /** Most recent rateRatio from the PortSyncSync. */ + double rate_ratio; + + /* Time Base Indicator of the current Grand Master. */ + u16_t gm_time_base_indicator; + + /** Current Log Sync Interval for this port. */ + s8_t log_msg_interval; +}; + +/** + * @brief Initialize all Media Dependent State Machines. + */ +void gptp_md_init_state_machine(void); + +/** + * @brief Run all Media Dependent State Machines. + * + * @param port Number of the port the State Machines needs to be run on. + */ +void gptp_md_state_machines(int port); + +#endif /* CONFIG_NET_GPTP */ + +#ifdef __cplusplus +} +#endif + +#endif /* __GPTP_MD_H */ diff --git a/subsys/net/l2/ethernet/gptp/gptp_messages.c b/subsys/net/l2/ethernet/gptp/gptp_messages.c new file mode 100644 index 00000000000000..7c5324df82cbbb --- /dev/null +++ b/subsys/net/l2/ethernet/gptp/gptp_messages.c @@ -0,0 +1,1005 @@ +/* + * Copyright (c) 2017 Intel Corporation. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#if defined(CONFIG_NET_DEBUG_GPTP) +#define SYS_LOG_DOMAIN "net/gptp" +#define NET_LOG_ENABLED 1 +#endif + +#include + +#include "gptp_messages.h" +#include "gptp_data_set.h" +#include "gptp_md.h" +#include "gptp_private.h" + +#define NET_BUF_TIMEOUT MSEC(100) + +static struct net_if_timestamp_cb sync_timestamp_cb; +static struct net_if_timestamp_cb pdelay_response_timestamp_cb; +static bool ts_cb_registered; +static bool sync_cb_registered; + +static const struct net_eth_addr gptp_multicast_eth_addr = { + { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e } }; + +#define NET_GPTP_INFO(msg, pkt) do { \ + if (IS_ENABLED(NET_LOG_ENABLED)) { \ + struct gptp_hdr *hdr = GPTP_HDR(pkt); \ + \ + ARG_UNUSED(hdr); \ + \ + if (hdr->message_type == GPTP_ANNOUNCE_MESSAGE) { \ + struct gptp_announce *ann = GPTP_ANNOUNCE(pkt); \ + char output[sizeof("xx:xx:xx:xx:xx:xx:xx:xx")]; \ + \ + gptp_sprint_clock_id( \ + ann->root_system_id.grand_master_id, \ + output, \ + sizeof(output)); \ + \ + NET_DBG("Sending %s seq %d pkt %p GM %d/%d/0x%x/%d/%s",\ + msg, ntohs(hdr->sequence_id), pkt, \ + ann->root_system_id.grand_master_prio1, \ + ann->root_system_id.clk_quality.clock_class, \ + ann->root_system_id.clk_quality.clock_accuracy,\ + ann->root_system_id.grand_master_prio2, \ + output); \ + } else { \ + NET_DBG("Sending %s seq %d pkt %p", msg, \ + ntohs(hdr->sequence_id), pkt); \ + } \ + } \ +} while (0) + +static void gptp_sync_timestamp_callback(struct net_pkt *pkt) +{ + int port = 0; + struct gptp_sync_send_state *state; + struct gptp_hdr *hdr; + + port = gptp_get_port_number(net_pkt_iface(pkt)); + if (port == -ENODEV) { + NET_DBG("No port found for ptp buffer"); + return; + } + + state = &GPTP_PORT_STATE(port)->sync_send; + + hdr = GPTP_HDR(pkt); + + /* If this buffer is a sync, flag it to the state machine. */ + if (hdr->message_type == GPTP_SYNC_MESSAGE) { + state->md_sync_timestamp_avail = true; + + net_if_unregister_timestamp_cb(&sync_timestamp_cb); + sync_cb_registered = false; + + /* The pkt was ref'ed in gptp_send_sync() */ + net_pkt_unref(pkt); + } +} + +static void gptp_pdelay_response_timestamp_callback(struct net_pkt *pkt) +{ + int port = 0; + struct net_pkt *follow_up; + struct gptp_hdr *hdr; + + port = gptp_get_port_number(net_pkt_iface(pkt)); + if (port == -ENODEV) { + NET_DBG("No port found for ptp buffer"); + goto out; + } + + hdr = GPTP_HDR(pkt); + + /* If this buffer is a path delay response, send the follow up. */ + if (hdr->message_type == GPTP_PATH_DELAY_RESP_MESSAGE) { + follow_up = gptp_prepare_pdelay_follow_up(port, pkt); + if (!follow_up) { + /* Cannot handle the follow up, abort */ + NET_ERR("Could not get buffer"); + goto out; + } + + net_if_unregister_timestamp_cb(&pdelay_response_timestamp_cb); + ts_cb_registered = false; + + gptp_send_pdelay_follow_up(port, follow_up, + net_pkt_timestamp(pkt)); + +out: + /* The pkt was ref'ed in gptp_handle_pdelay_req() */ + net_pkt_unref(pkt); + } +} + +struct net_pkt *gptp_prepare_sync(int port) +{ + int eth_len = sizeof(struct net_eth_hdr); + struct gptp_port_ds *port_ds; + struct net_eth_hdr *eth; + struct gptp_sync *sync; + struct net_if *iface; + struct net_pkt *pkt; + struct net_buf *frag; + struct gptp_hdr *hdr; + + NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END)); + iface = GPTP_PORT_IFACE(port); + NET_ASSERT(iface); + + pkt = net_pkt_get_reserve_tx(0, NET_BUF_TIMEOUT); + if (!pkt) { + goto fail; + } + + frag = net_pkt_get_reserve_tx_data(eth_len, NET_BUF_TIMEOUT); + if (!frag) { + goto fail; + } + + net_pkt_frag_add(pkt, frag); + net_pkt_set_iface(pkt, iface); + net_pkt_set_family(pkt, AF_UNSPEC); + + net_pkt_set_ll_reserve(pkt, eth_len); + + port_ds = GPTP_PORT_DS(port); + sync = GPTP_SYNC(pkt); + hdr = GPTP_HDR(pkt); + eth = NET_ETH_HDR(pkt); + + /* + * Header configuration. + * + * Some fields are set by gptp_md_sync_send_prepare(). + */ + hdr->transport_specific = GPTP_TRANSPORT_802_1_AS; + hdr->message_type = GPTP_SYNC_MESSAGE; + hdr->ptp_version = GPTP_VERSION; + hdr->sequence_id = htons(port_ds->sync_seq_id); + hdr->domain_number = 0; + hdr->correction_field = 0; + hdr->flags.octets[0] = GPTP_FLAG_TWO_STEP; + hdr->flags.octets[1] = GPTP_FLAG_PTP_TIMESCALE; + hdr->message_length = htons(sizeof(struct gptp_hdr) + + sizeof(struct gptp_sync)); + hdr->control = GPTP_SYNC_CONTROL_VALUE; + + /* Clear reserved fields. */ + hdr->reserved0 = 0; + hdr->reserved1 = 0; + hdr->reserved2 = 0; + + /* Ethernet configuration. */ + eth->type = htons(NET_ETH_PTYPE_PTP); + + memcpy(ð->src.addr, net_if_get_link_addr(iface)->addr, + sizeof(struct net_eth_addr)); + memcpy(ð->dst.addr, &gptp_multicast_eth_addr, + sizeof(struct net_eth_addr)); + + /* PTP configuration. */ + memset(&sync->reserved, 0, sizeof(sync->reserved)); + + net_buf_add(frag, sizeof(struct gptp_hdr) + sizeof(struct gptp_sync)); + + /* Update sequence number. */ + port_ds->sync_seq_id++; + + return pkt; + +fail: + if (pkt) { + net_pkt_unref(pkt); + } + + return NULL; +} + +struct net_pkt *gptp_prepare_follow_up(int port, struct net_pkt *sync) +{ + int eth_len = sizeof(struct net_eth_hdr); + struct gptp_hdr *hdr, *sync_hdr; + struct gptp_port_ds *port_ds; + struct net_eth_hdr *eth; + struct net_if *iface; + struct net_pkt *pkt; + struct net_buf *frag; + + NET_ASSERT(sync); + NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END)); + iface = GPTP_PORT_IFACE(port); + NET_ASSERT(iface); + + pkt = net_pkt_get_reserve_tx(0, NET_BUF_TIMEOUT); + if (!pkt) { + goto fail; + } + + frag = net_pkt_get_reserve_tx_data(eth_len, NET_BUF_TIMEOUT); + if (!frag) { + goto fail; + } + + net_pkt_frag_add(pkt, frag); + net_pkt_set_iface(pkt, iface); + net_pkt_set_family(pkt, AF_UNSPEC); + net_pkt_set_ll_reserve(pkt, eth_len); + + port_ds = GPTP_PORT_DS(port); + hdr = GPTP_HDR(pkt); + sync_hdr = GPTP_HDR(sync); + eth = NET_ETH_HDR(pkt); + + /* + * Header configuration. + * + * Some fields are set by gptp_md_follow_up_prepare(). + */ + hdr->transport_specific = GPTP_TRANSPORT_802_1_AS; + hdr->message_type = GPTP_FOLLOWUP_MESSAGE; + hdr->ptp_version = GPTP_VERSION; + hdr->sequence_id = sync_hdr->sequence_id; + hdr->domain_number = 0; + /* Store timestamp value in correction field. */ + hdr->correction_field = gptp_timestamp_to_nsec(&sync->timestamp); + hdr->flags.octets[0] = 0; + hdr->flags.octets[1] = GPTP_FLAG_PTP_TIMESCALE; + hdr->message_length = htons(sizeof(struct gptp_hdr) + + sizeof(struct gptp_follow_up)); + hdr->control = GPTP_FUP_CONTROL_VALUE; + + /* Clear reserved fields. */ + hdr->reserved0 = 0; + hdr->reserved1 = 0; + hdr->reserved2 = 0; + + /* Ethernet configuration. */ + eth->type = htons(NET_ETH_PTYPE_PTP); + + memcpy(ð->src.addr, net_if_get_link_addr(iface)->addr, + sizeof(struct net_eth_addr)); + memcpy(ð->dst.addr, &gptp_multicast_eth_addr, + sizeof(struct net_eth_addr)); + + /* PTP configuration will be set by the MDSyncSend state machine. */ + + net_buf_add(frag, sizeof(struct gptp_hdr) + + sizeof(struct gptp_follow_up)); + + return pkt; + +fail: + if (pkt) { + net_pkt_unref(pkt); + } + + return NULL; +} + +struct net_pkt *gptp_prepare_pdelay_req(int port) +{ + int eth_len = sizeof(struct net_eth_hdr); + struct gptp_pdelay_req *req; + struct gptp_port_ds *port_ds; + struct net_eth_hdr *eth; + struct net_if *iface; + struct net_pkt *pkt; + struct net_buf *frag; + struct gptp_hdr *hdr; + + NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END)); + iface = GPTP_PORT_IFACE(port); + NET_ASSERT(iface); + + pkt = net_pkt_get_reserve_tx(0, NET_BUF_TIMEOUT); + if (!pkt) { + goto fail; + } + + frag = net_pkt_get_reserve_tx_data(eth_len, NET_BUF_TIMEOUT); + if (!frag) { + goto fail; + } + + net_pkt_frag_add(pkt, frag); + net_pkt_set_iface(pkt, iface); + net_pkt_set_family(pkt, AF_UNSPEC); + net_pkt_set_ll_reserve(pkt, eth_len); + + port_ds = GPTP_PORT_DS(port); + req = GPTP_PDELAY_REQ(pkt); + hdr = GPTP_HDR(pkt); + eth = NET_ETH_HDR(pkt); + + /* Header configuration. */ + hdr->transport_specific = GPTP_TRANSPORT_802_1_AS; + hdr->message_type = GPTP_PATH_DELAY_REQ_MESSAGE; + hdr->ptp_version = GPTP_VERSION; + hdr->sequence_id = htons(port_ds->pdelay_req_seq_id); + hdr->domain_number = 0; + hdr->correction_field = 0; + hdr->flags.octets[0] = 0; + hdr->flags.octets[1] = GPTP_FLAG_PTP_TIMESCALE; + + hdr->message_length = htons(sizeof(struct gptp_hdr) + + sizeof(struct gptp_pdelay_req)); + hdr->port_id.port_number = htons(port_ds->port_id.port_number); + hdr->control = GPTP_OTHER_CONTROL_VALUE; + hdr->log_msg_interval = port_ds->cur_log_pdelay_req_itv; + + /* Clear reserved fields. */ + hdr->reserved0 = 0; + hdr->reserved1 = 0; + hdr->reserved2 = 0; + + memcpy(&hdr->port_id.clk_id, + &port_ds->port_id.clk_id, GPTP_CLOCK_ID_LEN); + + /* Ethernet configuration. */ + eth->type = htons(NET_ETH_PTYPE_PTP); + + memcpy(ð->src.addr, net_if_get_link_addr(iface)->addr, + sizeof(struct net_eth_addr)); + memcpy(ð->dst.addr, &gptp_multicast_eth_addr, + sizeof(struct net_eth_addr)); + + /* PTP configuration. */ + memset(&req->reserved1, 0, sizeof(req->reserved1)); + memset(&req->reserved2, 0, sizeof(req->reserved2)); + + net_buf_add(frag, sizeof(struct gptp_hdr) + + sizeof(struct gptp_pdelay_req)); + + /* Update sequence number. */ + port_ds->pdelay_req_seq_id++; + + return pkt; + +fail: + if (pkt) { + net_pkt_unref(pkt); + } + + return NULL; +} + +struct net_pkt *gptp_prepare_pdelay_resp(int port, + struct net_pkt *req) +{ + struct net_if *iface = net_pkt_iface(req); + int eth_len = sizeof(struct net_eth_hdr); + struct gptp_pdelay_resp *pdelay_resp; + struct net_eth_hdr *eth, *eth_query; + struct gptp_pdelay_req *pdelay_req; + struct gptp_hdr *hdr, *query; + struct gptp_port_ds *port_ds; + struct net_pkt *pkt; + struct net_buf *frag; + + pkt = net_pkt_get_reserve_tx(0, NET_BUF_TIMEOUT); + if (!pkt) { + goto fail; + } + + frag = net_pkt_get_reserve_tx_data(eth_len, NET_BUF_TIMEOUT); + if (!frag) { + goto fail; + } + + net_pkt_frag_add(pkt, frag); + net_pkt_set_iface(pkt, iface); + net_pkt_set_family(pkt, AF_INET); + net_pkt_set_ll_reserve(pkt, eth_len); + + port_ds = GPTP_PORT_DS(port); + + pdelay_resp = GPTP_PDELAY_RESP(pkt); + hdr = GPTP_HDR(pkt); + eth = NET_ETH_HDR(pkt); + + pdelay_req = GPTP_PDELAY_REQ(req); + query = GPTP_HDR(req); + eth_query = NET_ETH_HDR(req); + + /* Header configuration. */ + hdr->transport_specific = GPTP_TRANSPORT_802_1_AS; + hdr->message_type = GPTP_PATH_DELAY_RESP_MESSAGE; + hdr->ptp_version = GPTP_VERSION; + hdr->sequence_id = query->sequence_id; + hdr->domain_number = query->domain_number; + hdr->correction_field = query->correction_field; + hdr->flags.octets[0] = GPTP_FLAG_TWO_STEP; + hdr->flags.octets[1] = GPTP_FLAG_PTP_TIMESCALE; + + hdr->message_length = htons(sizeof(struct gptp_hdr) + + sizeof(struct gptp_pdelay_resp)); + hdr->port_id.port_number = htons(port_ds->port_id.port_number); + hdr->control = GPTP_OTHER_CONTROL_VALUE; + hdr->log_msg_interval = GPTP_RESP_LOG_MSG_ITV; + + /* Clear reserved fields. */ + hdr->reserved0 = 0; + hdr->reserved1 = 0; + hdr->reserved2 = 0; + + memcpy(&hdr->port_id.clk_id, &port_ds->port_id.clk_id, + GPTP_CLOCK_ID_LEN); + + /* Ethernet configuration. */ + eth->type = htons(NET_ETH_PTYPE_PTP); + + memcpy(ð->dst.addr, &gptp_multicast_eth_addr, + sizeof(struct net_eth_addr)); + memcpy(ð->src.addr, net_if_get_link_addr(iface)->addr, + sizeof(struct net_eth_addr)); + + /* PTP configuration. */ + pdelay_resp->req_receipt_ts_secs_high = 0; + pdelay_resp->req_receipt_ts_secs_low = 0; + pdelay_resp->req_receipt_ts_nsecs = 0; + + memcpy(&pdelay_resp->requesting_port_id, + &query->port_id, sizeof(struct gptp_port_identity)); + + net_buf_add(frag, sizeof(struct gptp_hdr) + + sizeof(struct gptp_pdelay_resp)); + + return pkt; + +fail: + if (pkt) { + net_pkt_unref(pkt); + } + + return NULL; +} + +struct net_pkt *gptp_prepare_pdelay_follow_up(int port, + struct net_pkt *resp) +{ + struct net_if *iface = net_pkt_iface(resp); + int eth_len = sizeof(struct net_eth_hdr); + struct gptp_pdelay_resp_follow_up *follow_up; + struct net_eth_hdr *eth, *eth_query; + struct gptp_pdelay_resp *pdelay_resp; + struct gptp_hdr *hdr, *resp_hdr; + struct gptp_port_ds *port_ds; + struct net_pkt *pkt; + struct net_buf *frag; + + pkt = net_pkt_get_reserve_tx(0, NET_BUF_TIMEOUT); + if (!pkt) { + goto fail; + } + + frag = net_pkt_get_reserve_tx_data(eth_len, NET_BUF_TIMEOUT); + if (!frag) { + goto fail; + } + + net_pkt_frag_add(pkt, frag); + net_pkt_set_iface(pkt, iface); + net_pkt_set_family(pkt, AF_INET); + net_pkt_set_ll_reserve(pkt, eth_len); + + port_ds = GPTP_PORT_DS(port); + + follow_up = GPTP_PDELAY_RESP_FOLLOWUP(pkt); + hdr = GPTP_HDR(pkt); + eth = NET_ETH_HDR(pkt); + + pdelay_resp = GPTP_PDELAY_RESP(resp); + resp_hdr = GPTP_HDR(resp); + eth_query = NET_ETH_HDR(resp); + + /* Header configuration. */ + hdr->transport_specific = GPTP_TRANSPORT_802_1_AS; + hdr->ptp_version = GPTP_VERSION; + hdr->message_type = GPTP_PATH_DELAY_FOLLOWUP_MESSAGE; + hdr->sequence_id = resp_hdr->sequence_id; + hdr->domain_number = resp_hdr->domain_number; + hdr->correction_field = 0; + hdr->message_length = htons(sizeof(struct gptp_hdr) + + sizeof(struct gptp_pdelay_resp_follow_up)); + hdr->port_id.port_number = htons(port_ds->port_id.port_number); + hdr->control = GPTP_OTHER_CONTROL_VALUE; + hdr->log_msg_interval = GPTP_RESP_LOG_MSG_ITV; + + hdr->flags.octets[0] = 0; + hdr->flags.octets[1] = GPTP_FLAG_PTP_TIMESCALE; + + /* Clear reserved fields. */ + hdr->reserved0 = 0; + hdr->reserved1 = 0; + hdr->reserved2 = 0; + + memcpy(&hdr->port_id.clk_id, &port_ds->port_id.clk_id, + GPTP_CLOCK_ID_LEN); + + /* Ethernet configuration. */ + eth->type = htons(NET_ETH_PTYPE_PTP); + + memcpy(ð->dst.addr, &gptp_multicast_eth_addr, + sizeof(struct net_eth_addr)); + memcpy(ð->src.addr, net_if_get_link_addr(iface)->addr, + sizeof(struct net_eth_addr)); + + /* PTP configuration. */ + follow_up->resp_orig_ts_secs_high = 0; + follow_up->resp_orig_ts_secs_low = 0; + follow_up->resp_orig_ts_nsecs = 0; + + memcpy(&follow_up->requesting_port_id, + &pdelay_resp->requesting_port_id, + sizeof(struct gptp_port_identity)); + + net_buf_add(frag, sizeof(struct gptp_hdr) + + sizeof(struct gptp_pdelay_resp_follow_up)); + + return pkt; + +fail: + if (pkt) { + net_pkt_unref(pkt); + } + + return NULL; +} + +struct net_pkt *gptp_prepare_announce(int port) +{ + int eth_len = sizeof(struct net_eth_hdr); + struct gptp_global_ds *global_ds; + struct gptp_port_ds *port_ds; + struct gptp_announce *ann; + struct net_eth_hdr *eth; + struct net_if *iface; + struct net_pkt *pkt; + struct net_buf *frag; + struct gptp_hdr *hdr; + + NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END)); + global_ds = GPTP_GLOBAL_DS(); + iface = GPTP_PORT_IFACE(port); + NET_ASSERT(iface); + + pkt = net_pkt_get_reserve_tx(0, NET_BUF_TIMEOUT); + if (!pkt) { + goto fail; + } + + frag = net_pkt_get_reserve_tx_data(eth_len, NET_BUF_TIMEOUT); + if (!frag) { + goto fail; + } + + net_pkt_frag_add(pkt, frag); + net_pkt_set_iface(pkt, iface); + net_pkt_set_family(pkt, AF_INET); + net_pkt_set_ll_reserve(pkt, eth_len); + + eth = NET_ETH_HDR(pkt); + hdr = GPTP_HDR(pkt); + ann = GPTP_ANNOUNCE(pkt); + port_ds = GPTP_PORT_DS(port); + + /* Ethernet configuration. */ + eth->type = htons(NET_ETH_PTYPE_PTP); + + memcpy(ð->src.addr, net_if_get_link_addr(iface)->addr, + sizeof(struct net_eth_addr)); + memcpy(ð->dst.addr, &gptp_multicast_eth_addr, + sizeof(struct net_eth_addr)); + + hdr->message_type = GPTP_ANNOUNCE_MESSAGE; + hdr->transport_specific = GPTP_TRANSPORT_802_1_AS; + hdr->ptp_version = GPTP_VERSION; + + hdr->domain_number = 0; + hdr->correction_field = 0; + hdr->flags.octets[0] = 0; + + /* Copy leap61, leap59, current UTC offset valid, time traceable and + * frequency traceable flags. + */ + hdr->flags.octets[1] = + global_ds->global_flags.octets[1] | GPTP_FLAG_PTP_TIMESCALE; + + memcpy(hdr->port_id.clk_id, GPTP_DEFAULT_DS()->clk_id, + GPTP_CLOCK_ID_LEN); + + hdr->port_id.port_number = htons(port); + hdr->control = GPTP_OTHER_CONTROL_VALUE; + hdr->log_msg_interval = port_ds->cur_log_announce_itv; + + /* Clear reserved fields. */ + hdr->reserved0 = 0; + hdr->reserved1 = 0; + hdr->reserved2 = 0; + + ann->cur_utc_offset = global_ds->current_utc_offset; + ann->time_source = global_ds->time_source; + + memcpy(&ann->root_system_id, + &GPTP_PORT_BMCA_DATA(port)->master_priority.root_system_id, + sizeof(struct gptp_root_system_identity)); + ann->steps_removed = global_ds->master_steps_removed; + hdr->sequence_id = htons(port_ds->announce_seq_id); + port_ds->announce_seq_id++; + + ann->tlv.type = GPTP_ANNOUNCE_MSG_PATH_SEQ_TYPE; + + /* Clear reserved fields. */ + memset(ann->reserved1, 0, sizeof(ann->reserved1)); + ann->reserved2 = 0; + + hdr->message_length = htons(sizeof(struct gptp_hdr) + + sizeof(struct gptp_announce) - 8 + + ntohs(global_ds->path_trace.len)); + + net_buf_add(frag, sizeof(struct gptp_hdr) + + sizeof(struct gptp_announce) - 8); + + ann->tlv.len = global_ds->path_trace.len; + + if (net_pkt_append(pkt, ntohs(global_ds->path_trace.len), + &global_ds->path_trace.path_sequence[0][0], + NET_BUF_TIMEOUT) < + ntohs(global_ds->path_trace.len)) { + goto fail; + } + + return pkt; + +fail: + if (pkt) { + net_pkt_unref(pkt); + } + + return NULL; +} + +void gptp_handle_sync(int port, struct net_pkt *pkt) +{ + struct gptp_sync_rcv_state *state; + struct gptp_port_ds *port_ds; + struct gptp_hdr *hdr; + u64_t upstream_sync_itv; + s32_t duration; + + state = &GPTP_PORT_STATE(port)->sync_rcv; + port_ds = GPTP_PORT_DS(port); + hdr = GPTP_HDR(state->rcvd_sync_ptr); + + upstream_sync_itv = NSEC_PER_SEC * GPTP_POW2(hdr->log_msg_interval); + + /* Convert ns to ms. */ + duration = (upstream_sync_itv / 1000000); + + /* Start timeout timer. */ + k_timer_start(&state->follow_up_discard_timer, duration, 0); +} + +int gptp_handle_follow_up(int port, struct net_pkt *pkt) +{ + struct gptp_sync_rcv_state *state; + struct gptp_hdr *sync_hdr, *hdr; + struct gptp_port_ds *port_ds; + + state = &GPTP_PORT_STATE(port)->sync_rcv; + port_ds = GPTP_PORT_DS(port); + + sync_hdr = GPTP_HDR(state->rcvd_sync_ptr); + hdr = GPTP_HDR(pkt); + + if (sync_hdr->sequence_id != hdr->sequence_id) { + NET_WARN("%s sequence id %d does not match %s %d", + "FOLLOWUP", ntohs(hdr->sequence_id), + "SYNC", ntohs(sync_hdr->sequence_id)); + return -EINVAL; + } + + GPTP_STATS_INC(port, rx_fup_count); + + return 0; +} + +void gptp_handle_pdelay_req(int port, struct net_pkt *pkt) +{ + struct net_pkt *reply; + + GPTP_STATS_INC(port, rx_pdelay_req_count); + + /* Prepare response and send */ + reply = gptp_prepare_pdelay_resp(port, pkt); + if (reply) { + if (!ts_cb_registered) { + net_if_register_timestamp_cb( + &pdelay_response_timestamp_cb, + net_pkt_iface(pkt), + gptp_pdelay_response_timestamp_callback); + + ts_cb_registered = true; + } + + /* TS thread will send this back to us so increment ref count + * so that the packet is not removed when sending it. + * This will be unref'ed by timestamp callback in + * gptp_pdelay_response_timestamp_callback() + */ + net_pkt_ref(reply); + + gptp_send_pdelay_resp(port, reply, net_pkt_timestamp(pkt)); + } +} + +int gptp_handle_pdelay_resp(int port, struct net_pkt *pkt) +{ + struct gptp_pdelay_req_state *state; + struct gptp_default_ds *default_ds; + struct gptp_pdelay_resp *resp; + struct gptp_port_ds *port_ds; + struct net_eth_hdr *eth; + struct gptp_hdr *hdr, *req_hdr; + + eth = NET_ETH_HDR(pkt); + hdr = GPTP_HDR(pkt); + resp = GPTP_PDELAY_RESP(pkt); + state = &GPTP_PORT_STATE(port)->pdelay_req; + port_ds = GPTP_PORT_DS(port); + default_ds = GPTP_DEFAULT_DS(); + + if (!state->tx_pdelay_req_ptr) { + goto reset; + } + + req_hdr = GPTP_HDR(state->tx_pdelay_req_ptr); + + /* Check clock identity. */ + if (memcmp(default_ds->clk_id, resp->requesting_port_id.clk_id, + GPTP_CLOCK_ID_LEN)) { + NET_WARN("Requesting Clock Identity does not match"); + goto reset; + } + if (memcmp(default_ds->clk_id, hdr->port_id.clk_id, + GPTP_CLOCK_ID_LEN) == 0) { + NET_WARN("Source Clock Identity is local Clock Identity"); + goto reset; + } + + /* Check port number. */ + if (resp->requesting_port_id.port_number != htons(port)) { + NET_WARN("Requesting Port Number does not match"); + goto reset; + } + + /* Check sequence id. */ + if (hdr->sequence_id != req_hdr->sequence_id) { + NET_WARN("Sequence Id %d does not match %d", + ntohs(hdr->sequence_id), ntohs(req_hdr->sequence_id)); + goto reset; + } + + GPTP_STATS_INC(port, rx_pdelay_resp_count); + + return 0; + +reset: + return -EINVAL; +} + +int gptp_handle_pdelay_follow_up(int port, struct net_pkt *pkt) +{ + struct gptp_pdelay_resp_follow_up *follow_up; + struct gptp_hdr *hdr, *req_hdr, *resp_hdr; + struct gptp_pdelay_req_state *state; + struct gptp_default_ds *default_ds; + struct gptp_port_ds *port_ds; + struct net_eth_hdr *eth; + + eth = NET_ETH_HDR(pkt); + hdr = GPTP_HDR(pkt); + follow_up = GPTP_PDELAY_RESP_FOLLOWUP(pkt); + state = &GPTP_PORT_STATE(port)->pdelay_req; + port_ds = GPTP_PORT_DS(port); + default_ds = GPTP_DEFAULT_DS(); + + if (!state->tx_pdelay_req_ptr) { + goto reset; + } + + req_hdr = GPTP_HDR(state->tx_pdelay_req_ptr); + + if (!state->rcvd_pdelay_resp_ptr) { + goto reset; + } + + resp_hdr = GPTP_HDR(state->rcvd_pdelay_resp_ptr); + + /* Check clock identity. */ + if (memcmp(default_ds->clk_id, follow_up->requesting_port_id.clk_id, + GPTP_CLOCK_ID_LEN)) { + NET_WARN("Requesting Clock Identity does not match"); + goto reset; + } + + if (memcmp(default_ds->clk_id, hdr->port_id.clk_id, + GPTP_CLOCK_ID_LEN) == 0) { + NET_WARN("Source Clock Identity is local Clock Identity"); + goto reset; + } + + /* Check port number. */ + if (follow_up->requesting_port_id.port_number != htons(port)) { + NET_WARN("Requesting Port Number does not match"); + goto reset; + } + + /* Check sequence id. */ + if (hdr->sequence_id != req_hdr->sequence_id) { + NET_WARN("Sequence Id %d does not match %d", + ntohs(hdr->sequence_id), ntohs(req_hdr->sequence_id)); + goto reset; + } + + /* Check source port. */ + if (memcmp(&hdr->port_id, &resp_hdr->port_id, + sizeof(hdr->port_id)) != 0) { + NET_WARN("pDelay response and follow up port IDs do not match"); + goto reset; + } + + GPTP_STATS_INC(port, rx_fup_count); + + return 0; + +reset: + return -EINVAL; +} + +void gptp_handle_signaling(int port, struct net_pkt *pkt) +{ + struct gptp_port_ds *port_ds; + struct gptp_signaling *sig; + + sig = GPTP_SIGNALING(pkt); + port_ds = GPTP_PORT_DS(port); + + /* If time-synchronization not enabled, drop packet. */ + if (!port_ds->ptt_port_enabled) { + return; + } + + /* pDelay interval. */ + gptp_update_pdelay_req_interval(port, sig->tlv.link_delay_itv); + + /* Sync interval. */ + gptp_update_sync_interval(port, sig->tlv.time_sync_itv); + + /* Announce interval. */ + gptp_update_announce_interval(port, sig->tlv.announce_itv); + + port_ds->compute_neighbor_rate_ratio = + sig->tlv.compute_neighbor_rate_ratio; + port_ds->compute_neighbor_prop_delay = + sig->tlv.compute_neighbor_prop_delay; +} + +void gptp_send_sync(int port, struct net_pkt *pkt) +{ + if (sync_cb_registered) { + net_if_register_timestamp_cb(&sync_timestamp_cb, + net_pkt_iface(pkt), + gptp_sync_timestamp_callback); + sync_cb_registered = true; + } + + GPTP_STATS_INC(port, tx_sync_count); + + /* TS thread will send this back to us so increment ref count + * so that the packet is not removed when sending it. + * This will be unref'ed by timestamp callback in + * gptp_sync_timestamp_callback() + */ + net_pkt_ref(pkt); + + NET_GPTP_INFO("SYNC", pkt); + + net_if_queue_tx(net_pkt_iface(pkt), pkt); +} + +void gptp_send_follow_up(int port, struct net_pkt *pkt) +{ + GPTP_STATS_INC(port, tx_fup_count); + + NET_GPTP_INFO("FOLLOWUP", pkt); + + net_if_queue_tx(net_pkt_iface(pkt), pkt); +} + +void gptp_send_announce(int port, struct net_pkt *pkt) +{ + GPTP_STATS_INC(port, tx_announce_count); + + NET_GPTP_INFO("ANNOUNCE", pkt); + + net_if_queue_tx(net_pkt_iface(pkt), pkt); +} + +void gptp_send_pdelay_req(int port) +{ + struct gptp_pdelay_req_state *state; + struct gptp_port_ds *port_ds; + struct net_pkt *pkt; + + NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END)); + state = &GPTP_PORT_STATE(port)->pdelay_req; + port_ds = GPTP_PORT_DS(port); + + pkt = gptp_prepare_pdelay_req(port); + if (pkt) { + /* Keep the buffer alive until pdelay_rate_ratio is computed. */ + state->tx_pdelay_req_ptr = net_pkt_ref(pkt); + + GPTP_STATS_INC(port, tx_pdelay_req_count); + + NET_GPTP_INFO("PDELAY_REQ", pkt); + + net_if_queue_tx(net_pkt_iface(pkt), pkt); + } else { + NET_ERR("Failed to prepare %s", "PDELAY_REQ"); + } +} + +void gptp_send_pdelay_resp(int port, struct net_pkt *pkt, + struct net_ptp_time *treq) +{ + struct gptp_pdelay_resp *resp; + struct gptp_hdr *hdr; + + hdr = GPTP_HDR(pkt); + + /* No Fractional nsec .*/ + hdr->correction_field = 0; + + resp = GPTP_PDELAY_RESP(pkt); + resp->req_receipt_ts_secs_high = htons(treq->_sec.high); + resp->req_receipt_ts_secs_low = htonl(treq->_sec.low); + resp->req_receipt_ts_nsecs = htonl(treq->nanosecond); + + GPTP_STATS_INC(port, tx_pdelay_resp_count); + + NET_GPTP_INFO("PDELAY_RESP", pkt); + + net_if_queue_tx(net_pkt_iface(pkt), pkt); +} + +void gptp_send_pdelay_follow_up(int port, struct net_pkt *pkt, + struct net_ptp_time *tresp) +{ + struct gptp_pdelay_resp_follow_up *follow_up; + struct gptp_hdr *hdr; + + hdr = GPTP_HDR(pkt); + + /* No Fractional nsec .*/ + hdr->correction_field = 0; + + follow_up = GPTP_PDELAY_RESP_FOLLOWUP(pkt); + follow_up->resp_orig_ts_secs_high = htons(tresp->_sec.high); + follow_up->resp_orig_ts_secs_low = htonl(tresp->_sec.low); + follow_up->resp_orig_ts_nsecs = htonl(tresp->nanosecond); + + GPTP_STATS_INC(port, tx_pdelay_resp_fup_count); + + NET_GPTP_INFO("PDELAY_FOLLOWUP", pkt); + + net_if_queue_tx(net_pkt_iface(pkt), pkt); +} diff --git a/subsys/net/l2/ethernet/gptp/gptp_messages.h b/subsys/net/l2/ethernet/gptp/gptp_messages.h new file mode 100644 index 00000000000000..cb0ede9ad67987 --- /dev/null +++ b/subsys/net/l2/ethernet/gptp/gptp_messages.h @@ -0,0 +1,498 @@ +/* + * Copyright (c) 2017 Intel Corporation. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @file + * @brief gPTP message helpers. + * + * This is not to be included by the application. + */ + +#ifndef __GPTP_MESSAGES_H +#define __GPTP_MESSAGES_H + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(CONFIG_NET_GPTP) + +#include +#include +#include + +/* Helpers to access gPTP messages. */ +#define GPTP_HDR(pkt) ((struct gptp_hdr *)net_pkt_ip_data(pkt)) +#define GPTP_ANNOUNCE(pkt) ((struct gptp_announce *)gptp_data(pkt)) +#define GPTP_SIGNALING(pkt) ((struct gptp_signaling *)gptp_data(pkt)) +#define GPTP_SYNC(pkt) ((struct gptp_sync *)gptp_data(pkt)) +#define GPTP_FOLLOW_UP(pkt) ((struct gptp_follow_up *)gptp_data(pkt)) +#define GPTP_DELAY_REQ(pkt) \ + ((struct gptp_delay_req *)gptp_data(pkt)) +#define GPTP_PDELAY_REQ(pkt) \ + ((struct gptp_pdelay_req *)gptp_data(pkt)) +#define GPTP_PDELAY_RESP(pkt) \ + ((struct gptp_pdelay_resp *)gptp_data(pkt)) +#define GPTP_PDELAY_RESP_FOLLOWUP(pkt) \ + ((struct gptp_pdelay_resp_follow_up *)gptp_data(pkt)) + +/* Field values. */ +#define GPTP_TRANSPORT_802_1_AS 0x1 +#define GPTP_VERSION 0x2 + +/* Message Lengths. */ +#define GPTP_PACKET_LEN(pkt) net_pkt_get_len(pkt) +#define GPTP_VALID_LEN(pkt, len) \ + (len > (NET_ETH_MINIMAL_FRAME_SIZE - GPTP_L2_HDR_LEN(pkt))) +#define GPTP_L2_HDR_LEN(pkt) \ + ((int)GPTP_HDR(pkt) - (int)NET_ETH_HDR(pkt)) + +#define GPTP_SYNC_LEN \ + (sizeof(struct gptp_hdr) + sizeof(struct gptp_sync)) +#define GPTP_FOLLOW_UP_LEN \ + (sizeof(struct gptp_hdr) + sizeof(struct gptp_follow_up)) +#define GPTP_PDELAY_REQ_LEN \ + (sizeof(struct gptp_hdr) + sizeof(struct gptp_pdelay_req)) +#define GPTP_PDELAY_RESP_LEN \ + (sizeof(struct gptp_hdr) + sizeof(struct gptp_pdelay_resp)) +#define GPTP_PDELAY_RESP_FUP_LEN \ + (sizeof(struct gptp_hdr) + sizeof(struct gptp_pdelay_resp_follow_up)) +#define GPTP_SIGNALING_LEN \ + (sizeof(struct gptp_hdr) + sizeof(struct gptp_signaling)) + +/* For the Announce message, the TLV is variable length. The len field + * indicates the length of the TLV not accounting for tlvType and lengthField + * which are 4 bytes. + */ +#define GPTP_ANNOUNCE_LEN(pkt) \ + (sizeof(struct gptp_hdr) + sizeof(struct gptp_announce) \ + + ntohs(GPTP_ANNOUNCE(pkt)->tlv.len) \ + - sizeof(struct gptp_path_trace_tlv) + 4) + +#define GPTP_CHECK_LEN(pkt, len) \ + ((GPTP_PACKET_LEN(pkt) != len) && (GPTP_VALID_LEN(pkt, len))) +#define GPTP_ANNOUNCE_CHECK_LEN(pkt) \ + ((GPTP_PACKET_LEN(pkt) != GPTP_ANNOUNCE_LEN(pkt)) && \ + (GPTP_VALID_LEN(pkt, GPTP_ANNOUNCE_LEN(pkt)))) + +/* Header Flags. Byte 0. */ +#define GPTP_FLAG_ALT_MASTER BIT(0) +#define GPTP_FLAG_TWO_STEP BIT(1) +#define GPTP_FLAG_UNICAST BIT(2) +#define GPTP_FLAG_PROFILE_SPECIFIC1 BIT(5) +#define GPTP_FLAG_PROFILE_SPECIFIC2 BIT(6) + +/* Header Flags. Byte 1. */ +#define GPTP_FLAG_LEAP61 BIT(0) +#define GPTP_FLAG_LEAP59 BIT(1) +#define GPTP_FLAG_CUR_UTC_OFF_VALID BIT(2) +#define GPTP_FLAG_PTP_TIMESCALE BIT(3) +#define GPTP_FLAG_TIME_TRACEABLE BIT(4) +#define GPTP_FLAG_FREQ_TRACEABLE BIT(5) + +/* Signaling Interval Flags. */ +#define GPTP_FLAG_COMPUTE_NEIGHBOR_RATE_RATIO 0x1 +#define GPTP_FLAG_COMPUTE_NEIGHBOR_PROP_DELAY 0x2 + +/* Signaling Interval Values. */ +#define GPTP_ITV_KEEP -128 +#define GPTP_ITV_SET_TO_INIT 126 +#define GPTP_ITV_STOP 127 + +/* Control. Only set for header compatibility with v1. */ +#define GPTP_SYNC_CONTROL_VALUE 0x0 +#define GPTP_FUP_CONTROL_VALUE 0x2 +#define GPTP_OTHER_CONTROL_VALUE 0x5 + +/* Other default values. */ +#define GPTP_RESP_LOG_MSG_ITV 0x7F +#define GPTP_ANNOUNCE_MSG_PATH_SEQ_TYPE htons(0x8) + +/* Organization Id used for TLV. */ +#define GPTP_FUP_TLV_ORG_ID_BYTE_0 0x00 +#define GPTP_FUP_TLV_ORG_ID_BYTE_1 0x80 +#define GPTP_FUP_TLV_ORG_ID_BYTE_2 0xC2 +#define GPTP_FUP_TLV_ORG_SUB_TYPE 0x01 + +/** + * @brief gPTP Clock Quality + * + * Defines the quality of a clock. + * This is used by the Best Master Clock Algorithm. + */ +struct gptp_clock_quality { + u8_t clock_class; + u8_t clock_accuracy; + u16_t offset_scaled_log_var; +} __packed; + +/** + * @brief gPTP Root System Identity + * + * Defines the Grand Master of a clock. + * This is used by the Best Master Clock Algorithm. + */ +struct gptp_root_system_identity { + /** Grand Master priority1 component. */ + u8_t grand_master_prio1; + + /** Grand Master clock quality. */ + struct gptp_clock_quality clk_quality; + + /** Grand Master priority2 component. */ + u8_t grand_master_prio2; + + /** Grand Master clock identity. */ + u8_t grand_master_id[GPTP_CLOCK_ID_LEN]; +} __packed; + +/* Definition of all message types as defined by IEEE802.1AS. */ + +struct gptp_path_trace_tlv { + /** TLV type: 0x8. */ + u16_t type; + + /** Length. Number of TLVs * 8 bytes. */ + u16_t len; + + /** ClockIdentity array of the successive time-aware systems. */ + u8_t path_sequence[1][8]; +} __packed; + +struct gptp_announce { + /** Reserved fields. */ + u8_t reserved1[10]; + + /** Current UTC offset. */ + s16_t cur_utc_offset; + + /** Reserved field. */ + u8_t reserved2; + + /* gmPriorityVector priority 1 of the peer sending the message. */ + struct gptp_root_system_identity root_system_id; + + /** masterStepsRemoved of the peer sending the message. */ + u16_t steps_removed; + + /** timeSource of the peer sending the message. */ + u8_t time_source; + + /* Path Trace TLV. This field has a variable length. */ + struct gptp_path_trace_tlv tlv; +} __packed; + +struct gptp_sync { + /** Reserved field. This field is used for PTPv2, unused in gPTP. */ + u8_t reserved[10]; +} __packed; + +struct gptp_follow_up_tlv { + /** TLV type: 0x3. */ + u16_t type; + + /** Length: 28. */ + u16_t len; + + /** Organization Id: 00-80-C2. */ + u8_t org_id[3]; + + /** Organization Sub Type: 1. */ + u8_t org_sub_type[3]; + + /** Rate ratio relative to the grand master of the peer. */ + s32_t cumulative_scaled_rate_offset; + + /** Time Base Indicator of the current Grand Master. */ + u16_t gm_time_base_indicator; + + /** Difference of the time between the current GM and the previous. */ + struct gptp_scaled_ns last_gm_phase_change; + + /** Diff of the frequency between the current GM and the previous. */ + s32_t scaled_last_gm_freq_change; +} __packed; + +struct gptp_follow_up { + /** Higher 16 bits of the seconds at which the sync was sent. */ + u16_t prec_orig_ts_secs_high; + + /** Lower 32 bits of the seconds at which the sync was sent. */ + u32_t prec_orig_ts_secs_low; + + /** Nanoseconds at which the sync was sent. */ + u32_t prec_orig_ts_nsecs; + + /** Follow up TLV. */ + struct gptp_follow_up_tlv tlv; +} __packed; + +struct gptp_pdelay_req { + /** Reserved fields. */ + u8_t reserved1[10]; + + /** Reserved fields. */ + u8_t reserved2[10]; +} __packed; + +struct gptp_pdelay_resp { + /** Higher 16 bits of the seconds at which the request was received. */ + u16_t req_receipt_ts_secs_high; + + /** Lower 32 bits of the seconds at which the request was received. */ + u32_t req_receipt_ts_secs_low; + + /** Nanoseconds at which the pdelay request was received. */ + u32_t req_receipt_ts_nsecs; + + /** Source Port Id of the Path Delay Request. */ + struct gptp_port_identity requesting_port_id; +} __packed; + +struct gptp_pdelay_resp_follow_up { + /** Higher 16 bits of the seconds at which the response was sent. */ + u16_t resp_orig_ts_secs_high; + + /** Lower 32 bits of the seconds at which the response was sent. */ + u32_t resp_orig_ts_secs_low; + + /** Nanoseconds at which the response was received. */ + u32_t resp_orig_ts_nsecs; + + /** Source Port Id of the Path Delay Request. */ + struct gptp_port_identity requesting_port_id; +} __packed; + +struct gptp_message_itv_req_tlv { + /** TLV type: 0x3. */ + u16_t type; + + /** Length field: 12. */ + u16_t len; + + /** Organization Id: 00-80-C2. */ + u8_t org_id[3]; + + /** Organization sub type: 0x2. */ + u8_t org_sub_type[3]; + + /** Log to base 2 of the mean time interval between pdelay requests. */ + s8_t link_delay_itv; + + /** Log to base 2 of the mean time interval between syncs. */ + s8_t time_sync_itv; + + /** Log to base 2 of the mean time interval between announces. */ + s8_t announce_itv; + + /** Flags (computeNeighborRateRatio and computeNeighborPropDelay). */ + union { + struct { + u8_t compute_neighbor_rate_ratio : 1; + u8_t compute_neighbor_prop_delay : 1; + }; + u8_t flags; + }; + /** Reserved fields. */ + u8_t reserved[2]; +} __packed; + +struct gptp_signaling { + /** Target Port Identity , always 0xFF. */ + struct gptp_port_identity target_port_id; + + /** Message Interval TLV. */ + struct gptp_message_itv_req_tlv tlv; +} __packed; + +/** + * @brief Compute gPTP message location. + * + * @param pkt Network Buffer containing a gPTP message. + * + * @return Pointer to the start of the gPTP message inside the packet. + */ +static inline u8_t *gptp_data(struct net_pkt *pkt) +{ + return &pkt->frags->data[sizeof(struct gptp_hdr)]; +} + +/* Functions to prepare messages. */ + +/** + * @brief Prepare Sync message. + * + * @param port gPTP port number. + * + * @return Pointer to the prepared Network Buffer. + */ +struct net_pkt *gptp_prepare_sync(int port); + +/** + * @brief Prepare Follow Up message. + * + * @param port gPTP port number. + * + * @return Pointer to the prepared Network Buffer. + */ +struct net_pkt *gptp_prepare_follow_up(int port, struct net_pkt *sync); + +/** + * @brief Prepare Path Delay Request message. + * + * @param port gPTP port number. + * + * @return Pointer to the prepared Network Buffer. + */ +struct net_pkt *gptp_prepare_pdelay_req(int port); + +/** + * @brief Prepare Path Delay Response message. + * + * @param port gPTP port number. + * @param req Path Delay Request to reply to. + * + * @return Pointer to the prepared Network Buffer. + */ +struct net_pkt *gptp_prepare_pdelay_resp(int port, + struct net_pkt *req); + +/** + * @brief Prepare Announce message. + * + * @param port gPTP port number. + * + * @return Pointer to the prepared Network Buffer. + */ +struct net_pkt *gptp_prepare_announce(int port); + +/** + * @brief Prepare Path Delay Response message. + * + * @param port gPTP port number. + * @param resp Related Path Delay Follow Up. + * + * @return Pointer to the prepared Network Buffer. + */ +struct net_pkt *gptp_prepare_pdelay_follow_up(int port, + struct net_pkt *resp); + +/* Functions to handle received messages. */ + +/** + * @brief Handle Sync message. + * + * @param port gPTP port number. + * @param pkt Network Buffer. + */ +void gptp_handle_sync(int port, struct net_pkt *pkt); + +/** + * @brief Handle Follow Up message. + * + * @param port gPTP port number. + * @param pkt Network Buffer to parse. + * + * @return 0 if success, Error Code otherwise. + */ +int gptp_handle_follow_up(int port, struct net_pkt *pkt); + +/** + * @brief Handle Path Delay Request message. + * + * @param port gPTP port number. + * @param pkt Network Buffer. + */ +void gptp_handle_pdelay_req(int port, struct net_pkt *pkt); + +/** + * @brief Handle Path Delay Response message. + * + * @param port gPTP port number. + * @param pkt Network Buffer to parse. + * + * @return 0 if success, Error Code otherwise. + */ +int gptp_handle_pdelay_resp(int port, struct net_pkt *pkt); + +/** + * @brief Handle Path Delay Follow Up message. + * + * @param port gPTP port number. + * @param pkt Network Buffer to parse. + * + * @return 0 if success, Error Code otherwise. + */ +int gptp_handle_pdelay_follow_up(int port, struct net_pkt *pkt); + +/** + * @brief Handle Signaling message. + * + * @param port gPTP port number. + * @param pkt Network Buffer + */ +void gptp_handle_signaling(int port, struct net_pkt *pkt); + +/* Functions to send messages. */ + +/** + * @brief Send a Sync message. + * + * @param port gPTP port number. + * @param pkt Sync message. + */ +void gptp_send_sync(int port, struct net_pkt *pkt); + +/** + * @brief Send a Follow Up message. + * + * @param port gPTP port number. + * @param pkt Follow Up message. + */ +void gptp_send_follow_up(int port, struct net_pkt *pkt); + +/** + * @brief Send an Announce message. + * + * @param port gPTP port number. + * @param pkt Announce message. + */ +void gptp_send_announce(int port, struct net_pkt *pkt); + +/** + * @brief Send a Path Delay Request on the given port. + * + * @param port gPTP port number. + */ +void gptp_send_pdelay_req(int port); + +/** + * @brief Send a Path Delay Response for the given Path Delay Request. + * + * @param port gPTP port number. + * @param pkt Network Buffer containing the prepared Path Delay Response. + * @param treq Time at which the Path Delay Request was received. + */ +void gptp_send_pdelay_resp(int port, struct net_pkt *pkt, + struct net_ptp_time *treq); + +/** + * @brief Send a Path Delay Response for the given Path Delay Request. + * + * @param port gPTP port number. + * @param pkt Network Buffer containing the prepared Path Delay Follow Up. + * @param tresp Time at which the Path Delay Response was sent. + */ +void gptp_send_pdelay_follow_up(int port, struct net_pkt *pkt, + struct net_ptp_time *tresp); + +#endif /* CONFIG_NET_GPTP */ + +#ifdef __cplusplus +} +#endif + +#endif /* __GPTP_MESSAGES_H */ diff --git a/subsys/net/l2/ethernet/gptp/gptp_mi.c b/subsys/net/l2/ethernet/gptp/gptp_mi.c new file mode 100644 index 00000000000000..a00e1d210bbb08 --- /dev/null +++ b/subsys/net/l2/ethernet/gptp/gptp_mi.c @@ -0,0 +1,1520 @@ +/* + * Copyright (c) 2017 Intel Corporation. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#if defined(CONFIG_NET_DEBUG_GPTP) +#define SYS_LOG_DOMAIN "net/gptp" +#define NET_LOG_ENABLED 1 +#endif + +#include + +#include "gptp_messages.h" +#include "gptp_data_set.h" +#include "gptp_state.h" +#include "gptp_private.h" + +static void gptp_mi_half_sync_itv_timeout(struct k_timer *timer) +{ + struct gptp_pss_send_state *state; + int port; + + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + state = &GPTP_PORT_STATE(port)->pss_send; + if (&state->half_sync_itv_timer == timer) { + if (!state->half_sync_itv_timer_expired) { + state->half_sync_itv_timer_expired = true; + } else { + /* We do not need the timer anymore. */ + k_timer_stop(timer); + + state->sync_itv_timer_expired = true; + } + } + } +} + +static void gptp_mi_rcv_sync_receipt_timeout(struct k_timer *timer) +{ + struct gptp_pss_rcv_state *state; + int port; + + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + state = &GPTP_PORT_STATE(port)->pss_rcv; + if (&state->rcv_sync_receipt_timeout_timer == timer) { + state->rcv_sync_receipt_timeout_timer_expired = true; + } + + GPTP_STATS_INC(port, sync_receipt_timeout_count); + } +} + +static void gptp_mi_send_sync_receipt_timeout(struct k_timer *timer) +{ + struct gptp_pss_send_state *state; + int port; + + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + state = &GPTP_PORT_STATE(port)->pss_send; + if (&state->send_sync_receipt_timeout_timer == timer) { + state->send_sync_receipt_timeout_timer_expired = true; + } + + GPTP_STATS_INC(port, sync_receipt_timeout_count); + } +} + +static void gptp_mi_init_port_sync_sync_rcv_sm(int port) +{ + struct gptp_pss_rcv_state *pss_rcv; + + pss_rcv = &GPTP_PORT_STATE(port)->pss_rcv; + memset(pss_rcv, 0, sizeof(struct gptp_pss_rcv_state)); + + k_timer_init(&pss_rcv->rcv_sync_receipt_timeout_timer, + gptp_mi_rcv_sync_receipt_timeout, NULL); + + pss_rcv->state = GPTP_PSS_RCV_DISCARD; +} + +static void gptp_mi_init_port_sync_sync_send_sm(int port) +{ + struct gptp_pss_send_state *pss_send; + + pss_send = &GPTP_PORT_STATE(port)->pss_send; + memset(pss_send, 0, sizeof(struct gptp_pss_send_state)); + + k_timer_init(&pss_send->half_sync_itv_timer, + gptp_mi_half_sync_itv_timeout, NULL); + k_timer_init(&pss_send->send_sync_receipt_timeout_timer, + gptp_mi_send_sync_receipt_timeout, NULL); + + pss_send->state = GPTP_PSS_SEND_TRANSMIT_INIT; +} + +static void gptp_mi_init_site_sync_sync_sm(void) +{ + struct gptp_site_sync_sync_state *site_ss; + + site_ss = &GPTP_STATE()->site_ss; + memset(site_ss, 0, sizeof(struct gptp_site_sync_sync_state)); + site_ss->state = GPTP_SSS_INITIALIZING; +} + +static void gptp_mi_init_clock_slave_sync_sm(void) +{ + struct gptp_clk_slave_sync_state *clk_ss; + + clk_ss = &GPTP_STATE()->clk_slave_sync; + memset(clk_ss, 0, sizeof(struct gptp_clk_slave_sync_state)); + clk_ss->state = GPTP_CLK_SLAVE_SYNC_INITIALIZING; +} + +static void gptp_mi_init_port_announce_rcv_sm(int port) +{ + struct gptp_port_announce_receive_state *pa_rcv; + + pa_rcv = &GPTP_PORT_STATE(port)->pa_rcv; + memset(pa_rcv, 0, sizeof(struct gptp_port_announce_receive_state)); + pa_rcv->state = GPTP_PA_RCV_DISCARD; +} + +static void gptp_mi_init_clock_master_sync_rcv_sm(void) +{ + struct gptp_clk_master_sync_state *cms_rcv; + + cms_rcv = &GPTP_STATE()->clk_master_sync_receive; + memset(cms_rcv, 0, sizeof(struct gptp_clk_master_sync_state)); + cms_rcv->state = GPTP_CMS_RCV_INITIALIZING; +} + +static void announce_timer_handler(struct k_timer *timer) +{ + int port; + struct gptp_port_announce_information_state *state; + + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + state = &GPTP_PORT_STATE(port)->pa_info; + if (&state->ann_rcpt_expiry_timer == timer) { + state->ann_expired = true; + GPTP_STATS_INC(port, announce_receipt_timeout_count); + break; + } + } +} + +static void gptp_mi_init_port_announce_info_sm(int port) +{ + struct gptp_port_announce_information_state *state; + + state = &GPTP_PORT_STATE(port)->pa_info; + + k_timer_init(&state->ann_rcpt_expiry_timer, + announce_timer_handler, NULL); + + state->ann_expired = false; + state->state = GPTP_PA_INFO_DISABLED; +} + +static void gptp_mi_init_bmca_data(int port) +{ + struct gptp_port_bmca_data *bmca_data; + + bmca_data = GPTP_PORT_BMCA_DATA(port); + + memset(bmca_data, 0, sizeof(struct gptp_port_bmca_data)); + + gptp_set_time_itv(&bmca_data->announce_interval, 1, + CONFIG_NET_GPTP_INIT_LOG_ANNOUNCE_ITV); + + memset(&bmca_data->port_priority, 0xFF, + sizeof(struct gptp_priority_vector)); + memset(&bmca_data->master_priority, 0xFF, + sizeof(struct gptp_priority_vector)); +} + +static void announce_periodic_timer_handler(struct k_timer *timer) +{ + int port; + struct gptp_port_announce_transmit_state *state; + + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + state = &GPTP_PORT_STATE(port)->pa_transmit; + if (&state->ann_send_periodic_timer == timer) { + state->ann_trigger = true; + break; + } + } +} + +static void gptp_mi_init_port_announce_transmit_sm(int port) +{ + struct gptp_port_announce_transmit_state *state; + + state = &GPTP_PORT_STATE(port)->pa_transmit; + + k_timer_init(&state->ann_send_periodic_timer, + announce_periodic_timer_handler, NULL); + + state->ann_trigger = false; + state->state = GPTP_PA_TRANSMIT_INIT; +} + +static void gptp_mi_init_port_role_selection_sm(void) +{ + GPTP_STATE()->pr_sel.state = GPTP_PR_SELECTION_INIT_BRIDGE; +} + +void gptp_mi_init_state_machine(void) +{ + int port; + + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + gptp_mi_init_port_sync_sync_rcv_sm(port); + gptp_mi_init_port_sync_sync_send_sm(port); + gptp_mi_init_port_announce_rcv_sm(port); + gptp_mi_init_port_announce_info_sm(port); + gptp_mi_init_port_announce_transmit_sm(port); + gptp_mi_init_bmca_data(port); + } + + gptp_mi_init_site_sync_sync_sm(); + gptp_mi_init_clock_slave_sync_sm(); + gptp_mi_init_port_role_selection_sm(); + gptp_mi_init_clock_master_sync_rcv_sm(); +} + +static void gptp_mi_pss_rcv_compute(int port) +{ + struct gptp_pss_rcv_state *state; + struct gptp_mi_port_sync_sync *pss; + struct gptp_md_sync_info *sync_rcv; + struct gptp_port_ds *port_ds; + + state = &GPTP_PORT_STATE(port)->pss_rcv; + pss = &state->pss; + sync_rcv = &state->sync_rcv; + port_ds = GPTP_PORT_DS(port); + + state->rate_ratio = sync_rcv->rate_ratio; + state->rate_ratio += (port_ds->neighbor_rate_ratio - 1.0); + + port_ds->sync_receipt_timeout_time_itv = port_ds->sync_receipt_timeout; + port_ds->sync_receipt_timeout_time_itv *= NSEC_PER_SEC; + port_ds->sync_receipt_timeout_time_itv *= + GPTP_POW2(16 + sync_rcv->log_msg_interval); + + pss->local_port_number = port; + + memcpy(&pss->sync_info, sync_rcv, sizeof(struct gptp_md_sync_info)); + + pss->sync_receipt_timeout_time = GPTP_GET_CURRENT_TIME_NANOSECOND(); + pss->sync_receipt_timeout_time += + (port_ds->sync_receipt_timeout_time_itv >> 16); + + pss->sync_info.rate_ratio = state->rate_ratio; +} + +static void start_rcv_sync_timer(struct gptp_port_ds *port_ds, + struct gptp_pss_rcv_state *state) +{ + s32_t duration; + + duration = (port_ds->sync_receipt_timeout_time_itv >> 16) / + (NSEC_PER_SEC / MSEC_PER_SEC); + + k_timer_start(&state->rcv_sync_receipt_timeout_timer, duration, 0); +} + +static void gptp_mi_pss_rcv_state_machine(int port) +{ + struct gptp_pss_rcv_state *state; + struct gptp_site_sync_sync_state *site_ss_state; + struct gptp_port_ds *port_ds; + + state = &GPTP_PORT_STATE(port)->pss_rcv; + site_ss_state = &GPTP_STATE()->site_ss; + port_ds = GPTP_PORT_DS(port); + + if ((!port_ds->ptt_port_enabled) || !port_ds->as_capable) { + state->rcvd_md_sync = false; + state->state = GPTP_PSS_RCV_DISCARD; + return; + } + + switch (state->state) { + case GPTP_PSS_RCV_DISCARD: + k_timer_stop(&state->rcv_sync_receipt_timeout_timer); + state->rcv_sync_receipt_timeout_timer_expired = false; + + /* Fallthrough. */ + case GPTP_PSS_RCV_RECEIVED_SYNC: + if (state->rcvd_md_sync) { + state->rcvd_md_sync = false; + gptp_mi_pss_rcv_compute(port); + + state->state = GPTP_PSS_RCV_RECEIVED_SYNC; + + site_ss_state->pss_rcv_ptr = &state->pss; + site_ss_state->rcvd_pss = true; + + k_timer_stop(&state->rcv_sync_receipt_timeout_timer); + state->rcv_sync_receipt_timeout_timer_expired = false; + + if (GPTP_GLOBAL_DS()->gm_present) { + start_rcv_sync_timer(port_ds, state); + } + } + + break; + } +} + +static void gptp_mi_pss_store_last_pss(int port) +{ + struct gptp_pss_send_state *state; + struct gptp_mi_port_sync_sync *pss_ptr; + struct gptp_md_sync_info *sync_info; + struct gptp_port_ds *port_ds; + + state = &GPTP_PORT_STATE(port)->pss_send; + port_ds = GPTP_PORT_DS(port); + pss_ptr = state->pss_sync_ptr; + sync_info = &pss_ptr->sync_info; + + state->last_rcvd_port_num = pss_ptr->local_port_number; + + memcpy(&state->last_precise_orig_ts, &sync_info->precise_orig_ts, + sizeof(struct net_ptp_time)); + memcpy(&state->last_gm_phase_change, &sync_info->last_gm_phase_change, + sizeof(struct gptp_scaled_ns)); + + state->last_follow_up_correction_field = + sync_info->follow_up_correction_field; + state->last_rate_ratio = sync_info->rate_ratio; + state->last_upstream_tx_time = sync_info->upstream_tx_time; + state->last_gm_time_base_indicator = sync_info->gm_time_base_indicator; + state->last_gm_freq_change = sync_info->last_gm_freq_change; +} + +static void gptp_mi_pss_send_md_sync_send(int port) +{ + struct gptp_pss_send_state *state; + struct gptp_mi_port_sync_sync *pss_ptr; + struct gptp_port_ds *port_ds; + struct gptp_sync_send_state *sync_send; + + state = &GPTP_PORT_STATE(port)->pss_send; + port_ds = GPTP_PORT_DS(port); + pss_ptr = state->pss_sync_ptr; + sync_send = &GPTP_PORT_STATE(port)->sync_send; + + memcpy(&state->sync_send, &pss_ptr->sync_info, + sizeof(struct gptp_md_sync_info)); + + sync_send->sync_send_ptr = &state->sync_send; + sync_send->rcvd_md_sync = true; +} + +static void gptp_mi_pss_send_state_machine(int port) +{ + struct gptp_pss_send_state *state; + struct gptp_port_ds *port_ds; + struct gptp_global_ds *global_ds; + s32_t duration; + + global_ds = GPTP_GLOBAL_DS(); + state = &GPTP_PORT_STATE(port)->pss_send; + port_ds = GPTP_PORT_DS(port); + + /* Reset interval as defined in LinkDelaySyncIntervalSetting state + * machine. + */ + if (port_ds->ptt_port_enabled && !port_ds->prev_ptt_port_enabled) { + gptp_update_sync_interval(port, GPTP_ITV_SET_TO_INIT); + } + + if (state->rcvd_pss_sync && ((!port_ds->ptt_port_enabled) || + !port_ds->as_capable)) { + state->rcvd_pss_sync = false; + state->state = GPTP_PSS_SEND_TRANSMIT_INIT; + + return; + } + + switch (state->state) { + case GPTP_PSS_SEND_TRANSMIT_INIT: + case GPTP_PSS_SEND_SYNC_RECEIPT_TIMEOUT: + if (state->rcvd_pss_sync && + (state->pss_sync_ptr->local_port_number != port) && + (global_ds->selected_role[port] == GPTP_PORT_MASTER)) { + state->state = GPTP_PSS_SEND_SEND_MD_SYNC; + } else { + break; + } + + /* Fallthrough. */ + case GPTP_PSS_SEND_SEND_MD_SYNC: + if (state->rcvd_pss_sync) { + gptp_mi_pss_store_last_pss(port); + state->rcvd_pss_sync = false; + } + + /* Make sure no previous timer is still running. */ + k_timer_stop(&state->half_sync_itv_timer); + k_timer_stop(&state->send_sync_receipt_timeout_timer); + + state->half_sync_itv_timer_expired = false; + state->sync_itv_timer_expired = false; + state->send_sync_receipt_timeout_timer_expired = false; + + /* Convert ns to ms. */ + duration = gptp_uscaled_ns_to_timer_ms( + &port_ds->half_sync_itv); + + /* Start 0.5 * syncInterval timeout timer. */ + k_timer_start(&state->half_sync_itv_timer, + duration, duration); + + gptp_mi_pss_send_md_sync_send(port); + + /* Fallthrough. */ + case GPTP_PSS_SEND_SET_SYNC_RECEIPT_TIMEOUT: + /* Test conditions have been slightly rearranged compared to + * their definitions in the standard in order not to test + * AsCapable and pttPortEnabled when not needed (they are + * already tested with rcvdPSSync for the reset of this state + * machine). + */ + if ((global_ds->selected_role[port] == GPTP_PORT_MASTER) && + ((state->rcvd_pss_sync && + state->half_sync_itv_timer_expired && + state->pss_sync_ptr->local_port_number != port) || + (state->sync_itv_timer_expired && + (state->last_rcvd_port_num != port) && + port_ds->as_capable && port_ds->ptt_port_enabled))) { + + state->state = GPTP_PSS_SEND_SEND_MD_SYNC; + + } else if ((state->state == GPTP_PSS_SEND_SEND_MD_SYNC) || + (state->rcvd_pss_sync && + !state->sync_itv_timer_expired && + (global_ds->selected_role[port] == + GPTP_PORT_MASTER) && + state->pss_sync_ptr->local_port_number != port)) { + + /* Change state as it may have transitioned from + * SEND_MD_SYNC. + */ + state->state = GPTP_PSS_SEND_SET_SYNC_RECEIPT_TIMEOUT; + + /* Stop and (re)start receipt timeout timer. */ + k_timer_stop(&state->send_sync_receipt_timeout_timer); + state->send_sync_receipt_timeout_timer_expired = false; + + duration = (state->last_sync_receipt_timeout_time - + GPTP_GET_CURRENT_TIME_NANOSECOND()) / + (NSEC_PER_USEC * USEC_PER_MSEC); + + k_timer_start(&state->send_sync_receipt_timeout_timer, + duration, 0); + + } else if (state->send_sync_receipt_timeout_timer_expired) { + state->state = GPTP_PSS_SEND_SYNC_RECEIPT_TIMEOUT; + } + + break; + } +} + +static void gptp_mi_site_ss_prepare_pss_send(void) +{ + struct gptp_site_sync_sync_state *state; + + state = &GPTP_STATE()->site_ss; + + memcpy(&state->pss_send, state->pss_rcv_ptr, + sizeof(struct gptp_mi_port_sync_sync)); +} + +static void gptp_mi_site_ss_send_to_pss(void) +{ + struct gptp_site_sync_sync_state *state; + struct gptp_pss_send_state *pss_send; + int port; + + state = &GPTP_STATE()->site_ss; + + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + pss_send = &GPTP_PORT_STATE(port)->pss_send; + pss_send->pss_sync_ptr = &state->pss_send; + pss_send->rcvd_pss_sync = true; + } +} + +static void gptp_mi_site_sync_sync_state_machine(void) +{ + bool gm_present; + u16_t local_port_number; + struct gptp_site_sync_sync_state *state; + struct gptp_clk_slave_sync_state *clk_ss; + + state = &GPTP_STATE()->site_ss; + clk_ss = &GPTP_STATE()->clk_slave_sync; + gm_present = GPTP_GLOBAL_DS()->gm_present; + + if (!state->pss_rcv_ptr) { + /* We do not have connection to GM yet */ + return; + } + + local_port_number = state->pss_rcv_ptr->local_port_number; + + switch (state->state) { + case GPTP_SSS_INITIALIZING: + state->rcvd_pss = false; + state->state = GPTP_SSS_RECEIVING_SYNC; + break; + + case GPTP_SSS_RECEIVING_SYNC: + if (state->rcvd_pss) { + state->rcvd_pss = false; + if (gptp_is_slave_port(local_port_number) && + gm_present) { + gptp_mi_site_ss_prepare_pss_send(); + + /* + * Send Port Sync Sync to all + * PortSyncSyncSend State Machines. + */ + gptp_mi_site_ss_send_to_pss(); + + /* + * Send PortSyncSync to + * ClockSlaveSync State Machine. + */ + clk_ss->pss_rcv_ptr = &state->pss_send; + clk_ss->rcvd_pss = true; + } + } + + break; + } +} + +static void gptp_mi_clk_slave_sync_compute(void) +{ + struct gptp_clk_slave_sync_state *state; + struct gptp_global_ds *global_ds; + struct gptp_md_sync_info *pss; + struct gptp_port_ds *port_ds; + u64_t sync_receipt_time; + + state = &GPTP_STATE()->clk_slave_sync; + global_ds = GPTP_GLOBAL_DS(); + port_ds = GPTP_PORT_DS(state->pss_rcv_ptr->local_port_number); + + pss = &state->pss_rcv_ptr->sync_info; + + sync_receipt_time = pss->rate_ratio; + sync_receipt_time /= port_ds->neighbor_rate_ratio; + sync_receipt_time *= port_ds->neighbor_prop_delay; + sync_receipt_time += pss->follow_up_correction_field; + sync_receipt_time += port_ds->delay_asymmetry; + + global_ds->sync_receipt_time.second = sync_receipt_time / NSEC_PER_SEC; + global_ds->sync_receipt_time.nanosecond = + sync_receipt_time % NSEC_PER_SEC; + global_ds->sync_receipt_time.second += pss->precise_orig_ts.second; + global_ds->sync_receipt_time.nanosecond += + pss->precise_orig_ts.nanosecond; + + global_ds->sync_receipt_local_time = port_ds->delay_asymmetry; + global_ds->sync_receipt_local_time /= pss->rate_ratio; + global_ds->sync_receipt_local_time += + (port_ds->neighbor_prop_delay / port_ds->neighbor_rate_ratio); + global_ds->sync_receipt_local_time += pss->upstream_tx_time; + + global_ds->gm_time_base_indicator = pss->gm_time_base_indicator; + global_ds->last_gm_phase_change.high = pss->last_gm_phase_change.high; + global_ds->last_gm_phase_change.low = pss->last_gm_phase_change.low; + global_ds->last_gm_freq_change = pss->last_gm_freq_change; +} + +#if defined(CONFIG_NET_GPTP_USE_DEFAULT_CLOCK_UPDATE) +static void gptp_update_local_port_clock(void) +{ + struct gptp_clk_slave_sync_state *state; + struct gptp_global_ds *global_ds; + struct gptp_port_ds *port_ds; + int port; + s64_t nanosecond_diff; + s64_t second_diff; + struct device *clk; + struct net_ptp_time tm; + int key; + + state = &GPTP_STATE()->clk_slave_sync; + global_ds = GPTP_GLOBAL_DS(); + port = state->pss_rcv_ptr->local_port_number; + NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END)); + + port_ds = GPTP_PORT_DS(port); + + second_diff = global_ds->sync_receipt_time.second - + (global_ds->sync_receipt_local_time / NSEC_PER_SEC); + nanosecond_diff = global_ds->sync_receipt_time.nanosecond - + (global_ds->sync_receipt_local_time % NSEC_PER_SEC); + + clk = net_eth_get_ptp_clock(GPTP_PORT_IFACE(port)); + if (!clk) { + return; + } + + ptp_clock_rate_adjust(clk, port_ds->neighbor_rate_ratio); + + /* If time difference is too high, set the clock value. + * Otherwise, adjust it. + */ + if (second_diff || (second_diff == 0 && + (nanosecond_diff < -5000 || + nanosecond_diff > 5000))) { + key = irq_lock(); + ptp_clock_get(clk, &tm); + tm.second += second_diff; + tm.nanosecond += nanosecond_diff; + if (tm.nanosecond < 0) { + tm.second--; + tm.nanosecond += NSEC_PER_SEC; + } else if (tm.nanosecond >= NSEC_PER_SEC) { + tm.second++; + tm.nanosecond -= NSEC_PER_SEC; + } + + ptp_clock_set(clk, &tm); + irq_unlock(key); + } else { + if (nanosecond_diff < -200) { + nanosecond_diff = -200; + } else if (nanosecond_diff > 200) { + nanosecond_diff = 200; + } + + ptp_clock_adjust(clk, nanosecond_diff); + } +} +#endif /* CONFIG_NET_GPTP_USE_DEFAULT_CLOCK_UPDATE */ + +static void gptp_mi_clk_slave_sync_state_machine(void) +{ + struct gptp_clk_slave_sync_state *state; + + state = &GPTP_STATE()->clk_slave_sync; + + switch (state->state) { + case GPTP_CLK_SLAVE_SYNC_INITIALIZING: + state->rcvd_pss = false; + state->state = GPTP_CLK_SLAVE_SYNC_SEND_SYNC_IND; + break; + + case GPTP_CLK_SLAVE_SYNC_SEND_SYNC_IND: + if (state->rcvd_pss) { + state->rcvd_pss = false; + gptp_mi_clk_slave_sync_compute(); + +#if defined(CONFIG_NET_GPTP_USE_DEFAULT_CLOCK_UPDATE) + /* Instead of updating SlaveClock, update LocalClock */ + gptp_update_local_port_clock(); +#endif + gptp_call_phase_dis_cb(); + } + + break; + } +} + +static void gptp_mi_clk_master_sync_rcv_state_machine(void) +{ + struct gptp_clk_master_sync_state *state; + + state = &GPTP_STATE()->clk_master_sync_receive; + switch (state->state) { + case GPTP_CMS_RCV_INITIALIZING: + state->state = GPTP_CMS_RCV_WAITING; + break; + + case GPTP_CMS_RCV_WAITING: + if (state->rcvd_clock_source_req || + state->rcvd_local_clock_tick) { + state->state = GPTP_CMS_RCV_SOURCE_TIME; + } + + break; + + case GPTP_CMS_RCV_SOURCE_TIME: + /* TODO: + * updateMasterTime(); + * localTime = currentTime; + */ + if (state->rcvd_clock_source_req) { + /* TODO: + * computeGMRateRatio(); + * Update: + * clockSourceTimeBaseIndicatorOld; + * clockSourceTimeBaseIndicator + * clockSourceLastGmPhaseChange + * clockSourceLastGmFreqChange + */ + } + + state->rcvd_clock_source_req = false; + state->rcvd_local_clock_tick = false; + state->state = GPTP_CMS_RCV_WAITING; + break; + + default: + NET_ERR("Unrecognised state %d", state->state); + break; + } +} + +static void copy_path_trace(struct gptp_announce *announce) +{ + int len = ntohs(announce->tlv.len); + struct gptp_path_trace *sys_path_trace; + + if (len > GPTP_MAX_PATHTRACE_SIZE) { + NET_ERR("Too long path trace (%d vs %d)", + GPTP_MAX_PATHTRACE_SIZE, len); + return; + } + + sys_path_trace = &GPTP_GLOBAL_DS()->path_trace; + + sys_path_trace->len = htons(len + GPTP_CLOCK_ID_LEN); + + memcpy(sys_path_trace->path_sequence, announce->tlv.path_sequence, + len); + + /* Append local clockIdentity. */ + memcpy((u8_t *)sys_path_trace->path_sequence + len, + GPTP_DEFAULT_DS()->clk_id, GPTP_CLOCK_ID_LEN); +} + +static bool gptp_mi_qualify_announce(int port, struct net_pkt *announce_msg) +{ + struct gptp_announce *announce; + struct gptp_hdr *hdr; + int i; + u16_t len; + + hdr = GPTP_HDR(announce_msg); + announce = GPTP_ANNOUNCE(announce_msg); + + if (memcmp(hdr->port_id.clk_id, GPTP_DEFAULT_DS()->clk_id, + GPTP_CLOCK_ID_LEN) == 0) { + return false; + } + + len = ntohs(announce->steps_removed); + if (len >= 255) { + return false; + } + + for (i = 0; i < len + 1; i++) { + if (memcmp(announce->tlv.path_sequence[i], + GPTP_DEFAULT_DS()->clk_id, + GPTP_CLOCK_ID_LEN) == 0) { + return false; + } + } + + if (GPTP_GLOBAL_DS()->selected_role[port] == GPTP_PORT_SLAVE) { + copy_path_trace(announce); + } + + return true; +} + +static void gptp_mi_port_announce_receive_state_machine(int port) +{ + struct gptp_port_ds *port_ds; + struct gptp_port_announce_receive_state *state; + struct gptp_port_bmca_data *bmca_data; + + state = &GPTP_PORT_STATE(port)->pa_rcv; + port_ds = GPTP_PORT_DS(port); + bmca_data = GPTP_PORT_BMCA_DATA(port); + + if ((!port_ds->ptt_port_enabled) || (!port_ds->as_capable)) { + state->state = GPTP_PA_RCV_DISCARD; + } + + switch (state->state) { + case GPTP_PA_RCV_DISCARD: + state->rcvd_announce = false; + bmca_data->rcvd_msg = false; + if (bmca_data->rcvd_announce_ptr != NULL) { + net_pkt_unref(bmca_data->rcvd_announce_ptr); + bmca_data->rcvd_announce_ptr = NULL; + } + + state->state = GPTP_PA_RCV_RECEIVE; + break; + + case GPTP_PA_RCV_RECEIVE: + /* "portEnabled" is not checked: the interface is always up. */ + if (state->rcvd_announce && + port_ds->ptt_port_enabled && + port_ds->as_capable && + !bmca_data->rcvd_msg) { + state->rcvd_announce = false; + + bmca_data->rcvd_msg = gptp_mi_qualify_announce( + port, bmca_data->rcvd_announce_ptr); + if (!bmca_data->rcvd_msg) { + net_pkt_unref(bmca_data->rcvd_announce_ptr); + bmca_data->rcvd_announce_ptr = NULL; + } + } + + break; + } +} + +/* + * Compare a vector to an announce message vector. + * All must be in big endian (network) order. + */ +static enum gptp_received_info compare_priority_vectors( + struct gptp_priority_vector *vector, + struct net_pkt *pkt, int port) +{ + struct gptp_hdr *hdr; + struct gptp_announce *announce; + struct gptp_port_bmca_data *bmca_data; + int rsi_cmp, spi_cmp, port_cmp; + + bmca_data = GPTP_PORT_BMCA_DATA(port); + hdr = GPTP_HDR(pkt); + announce = GPTP_ANNOUNCE(pkt); + + /* Compare rootSystemIdentity and stepsRemoved. */ + rsi_cmp = memcmp(&announce->root_system_id, + &vector->root_system_id, + sizeof(struct gptp_root_system_identity) + + sizeof(u16_t)); + if (rsi_cmp < 0) { + /* Better rootSystemIdentity. */ + return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO; + } + + /* Compare sourcePortIdentity. */ + spi_cmp = memcmp(&hdr->port_id, &vector->src_port_id, + sizeof(struct gptp_port_identity)); + + port_cmp = (int)port - ntohs(vector->port_number); + + if (spi_cmp == 0) { + if (rsi_cmp == 0) { + if (port_cmp == 0) { + /* Same priority vector. */ + return GPTP_RCVD_INFO_REPEATED_MASTER_INFO; + } else if (port_cmp < 0) { + /* Priority vector with better reception port + * number. + */ + return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO; + } + } else { + /* Same master port but different Grand Master. */ + return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO; + } + } else if ((spi_cmp < 0) && (rsi_cmp == 0)) { + /* Same Grand Master but better masterPort. */ + return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO; + } + + return GPTP_RCVD_INFO_INFERIOR_MASTER_INFO; +} + +static enum gptp_received_info rcv_info(int port) +{ + /* TODO + * How can we define that a message does not convey the port + * role Master port ? + * It is needed to define that to be able to send + * GPTP_RCVD_INFO_OTHER_INFO. + */ + struct gptp_port_bmca_data *bmca_data; + struct gptp_announce *announce; + + bmca_data = GPTP_PORT_BMCA_DATA(port); + announce = GPTP_ANNOUNCE(bmca_data->rcvd_announce_ptr); + + bmca_data->message_steps_removed = announce->steps_removed; + + return compare_priority_vectors(&bmca_data->port_priority, + bmca_data->rcvd_announce_ptr, + port); +} + +static void record_other_announce_info(int port) +{ + struct gptp_hdr *hdr; + struct gptp_announce *announce; + struct gptp_port_bmca_data *bmca_data; + + bmca_data = GPTP_PORT_BMCA_DATA(port); + hdr = GPTP_HDR(bmca_data->rcvd_announce_ptr); + announce = GPTP_ANNOUNCE(bmca_data->rcvd_announce_ptr); + + /* Copy leap61, leap59, current UTC offset valid, time traceable and + * frequency traceable flags. + */ + bmca_data->ann_flags.octets[1] = hdr->flags.octets[1]; + + bmca_data->ann_current_utc_offset = ntohs(announce->cur_utc_offset); + bmca_data->ann_time_source = announce->time_source; +} + +static void copy_priority_vector(struct gptp_priority_vector *vector, + struct net_pkt *pkt, int port) +{ + struct gptp_hdr *hdr; + struct gptp_announce *announce; + + hdr = GPTP_HDR(pkt); + announce = GPTP_ANNOUNCE(pkt); + + memcpy(&vector->root_system_id, &announce->root_system_id, + sizeof(struct gptp_root_system_identity) + sizeof(u16_t)); + + memcpy(&vector->src_port_id, &hdr->port_id, + sizeof(struct gptp_port_identity)); + + vector->port_number = htons(port); +} + +static void gptp_mi_port_announce_information_state_machine(int port) +{ + struct gptp_port_ds *port_ds; + struct gptp_global_ds *global_ds; + struct gptp_port_announce_information_state *state; + struct gptp_announce *announce; + struct gptp_hdr *hdr; + struct gptp_port_bmca_data *bmca_data; + struct gptp_pss_rcv_state *pss_rcv; + + bmca_data = GPTP_PORT_BMCA_DATA(port); + state = &GPTP_PORT_STATE(port)->pa_info; + port_ds = GPTP_PORT_DS(port); + global_ds = GPTP_GLOBAL_DS(); + + if ((!port_ds->ptt_port_enabled || !port_ds->as_capable) && + (bmca_data->info_is != GPTP_INFO_IS_DISABLED)) { + state->state = GPTP_PA_INFO_DISABLED; + } + + switch (state->state) { + case GPTP_PA_INFO_DISABLED: + bmca_data->rcvd_msg = false; + bmca_data->info_is = GPTP_INFO_IS_DISABLED; + SET_RESELECT(global_ds, port); + CLEAR_SELECTED(global_ds, port); + state->state = GPTP_PA_INFO_POST_DISABLED; + k_timer_stop(&state->ann_rcpt_expiry_timer); + state->ann_expired = true; + /* Fallthrough. */ + + case GPTP_PA_INFO_POST_DISABLED: + if (port_ds->ptt_port_enabled && port_ds->as_capable) { + state->state = GPTP_PA_INFO_AGED; + } else if (bmca_data->rcvd_msg) { + state->state = GPTP_PA_INFO_DISABLED; + } + + break; + + case GPTP_PA_INFO_AGED: + bmca_data->info_is = GPTP_INFO_IS_AGED; + CLEAR_SELECTED(global_ds, port); + SET_RESELECT(global_ds, port); + /* Transition will be actually tested in UPDATE state. */ + state->state = GPTP_PA_INFO_UPDATE; + break; + + case GPTP_PA_INFO_UPDATE: + if (IS_SELECTED(global_ds, port) && bmca_data->updt_info) { + memcpy(&bmca_data->port_priority, + &bmca_data->master_priority, + sizeof(struct gptp_priority_vector)); + + bmca_data->port_steps_removed = + global_ds->master_steps_removed; + bmca_data->updt_info = false; + bmca_data->info_is = GPTP_INFO_IS_MINE; + bmca_data->new_info = true; + state->state = GPTP_PA_INFO_CURRENT; + } + + break; + + case GPTP_PA_INFO_CURRENT: + pss_rcv = &GPTP_PORT_STATE(port)->pss_rcv; + if (IS_SELECTED(global_ds, port) && bmca_data->updt_info) { + state->state = GPTP_PA_INFO_UPDATE; + } else if (bmca_data->rcvd_msg && !bmca_data->updt_info) { + state->state = GPTP_PA_INFO_RECEIVE; + } else if ((bmca_data->info_is == GPTP_INFO_IS_RECEIVED) && + !bmca_data->updt_info && + !bmca_data->rcvd_msg && + (state->ann_expired || + (global_ds->gm_present && + pss_rcv->rcv_sync_receipt_timeout_timer_expired))) { + state->state = GPTP_PA_INFO_AGED; + } + + break; + + case GPTP_PA_INFO_RECEIVE: + switch (rcv_info(port)) { + case GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO: + state->state = GPTP_PA_INFO_SUPERIOR_MASTER_PORT; + break; + case GPTP_RCVD_INFO_REPEATED_MASTER_INFO: + state->state = GPTP_PA_INFO_REPEATED_MASTER_PORT; + break; + case GPTP_RCVD_INFO_INFERIOR_MASTER_INFO: + /* Fallthrough. */ + case GPTP_RCVD_INFO_OTHER_INFO: + state->state = + GPTP_PA_INFO_INFERIOR_MASTER_OR_OTHER_PORT; + break; + } + + break; + + case GPTP_PA_INFO_SUPERIOR_MASTER_PORT: + /* We copy directly the content of the message to the port + * priority vector without using an intermediate + * messagePrioriry structure. + */ + + if (!bmca_data->rcvd_announce_ptr) { + /* Shouldn't be reached. Checked for safety reason. */ + bmca_data->rcvd_msg = false; + state->state = GPTP_PA_INFO_CURRENT; + break; + } + + copy_priority_vector(&bmca_data->port_priority, + bmca_data->rcvd_announce_ptr, port); + + announce = GPTP_ANNOUNCE(bmca_data->rcvd_announce_ptr); + bmca_data->port_steps_removed = ntohs(announce->steps_removed); + record_other_announce_info(port); + hdr = GPTP_HDR(bmca_data->rcvd_announce_ptr); + gptp_set_time_itv(&bmca_data->ann_rcpt_timeout_time_interval, + port_ds->announce_receipt_timeout, + hdr->log_msg_interval); + bmca_data->info_is = GPTP_INFO_IS_RECEIVED; + CLEAR_SELECTED(global_ds, port); + SET_RESELECT(global_ds, port); + /* Fallthrough. */ + + case GPTP_PA_INFO_REPEATED_MASTER_PORT: + k_timer_stop(&state->ann_rcpt_expiry_timer); + state->ann_expired = false; + k_timer_start(&state->ann_rcpt_expiry_timer, + gptp_uscaled_ns_to_timer_ms( + &bmca_data->ann_rcpt_timeout_time_interval), + 0); + /* Fallthrough. */ + + case GPTP_PA_INFO_INFERIOR_MASTER_OR_OTHER_PORT: + if (bmca_data->rcvd_announce_ptr != NULL) { + net_pkt_unref(bmca_data->rcvd_announce_ptr); + bmca_data->rcvd_announce_ptr = NULL; + } + + bmca_data->rcvd_msg = false; + state->state = GPTP_PA_INFO_CURRENT; + break; + } +} + +static void gptp_updt_role_disabled_tree(void) +{ + struct gptp_global_ds *global_ds; + int port; + + global_ds = GPTP_GLOBAL_DS(); + + /* Set all elements of the selectedRole array to DisabledPort. */ + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + global_ds->selected_role[port] = GPTP_PORT_DISABLED; + } + + /* Set lastGmPriority to all ones. */ + memset(&global_ds->last_gm_priority, 0xFF, + sizeof(struct gptp_priority_vector)); + + /* Set pathTrace array to contain the single element thisClock. */ + global_ds->path_trace.len = htons(GPTP_CLOCK_ID_LEN); + memcpy(global_ds->path_trace.path_sequence, GPTP_DEFAULT_DS()->clk_id, + GPTP_CLOCK_ID_LEN); +} + +static void gptp_clear_reselect_tree(void) +{ + /* Set all the elements of the reselect array to FALSE. */ + GPTP_GLOBAL_DS()->reselect_array = 0; +} + +static int compute_best_vector(void) +{ + struct gptp_priority_vector *gm_prio; + struct gptp_default_ds *default_ds; + struct gptp_global_ds *global_ds; + struct gptp_priority_vector *best_vector, *challenger; + int best_port, port, tmp; + struct gptp_pss_rcv_state *pss_rcv; + struct gptp_port_announce_information_state *pa_info_state; + + default_ds = GPTP_DEFAULT_DS(); + global_ds = GPTP_GLOBAL_DS(); + best_port = 0; + gm_prio = &global_ds->gm_priority; + + /* Write systemPriority into grandmaster. */ + memset(gm_prio, 0, sizeof(struct gptp_priority_vector)); + gm_prio->root_system_id.grand_master_prio1 = default_ds->priority1; + gm_prio->root_system_id.grand_master_prio2 = default_ds->priority2; + gm_prio->root_system_id.clk_quality.clock_class = + default_ds->clk_quality.clock_class; + gm_prio->root_system_id.clk_quality.clock_accuracy = + default_ds->clk_quality.clock_accuracy; + gm_prio->root_system_id.clk_quality.offset_scaled_log_var = + htons(default_ds->clk_quality.offset_scaled_log_var); + + memcpy(gm_prio->src_port_id.clk_id, default_ds->clk_id, + GPTP_CLOCK_ID_LEN); + memcpy(gm_prio->root_system_id.grand_master_id, default_ds->clk_id, + GPTP_CLOCK_ID_LEN); + + best_vector = gm_prio; + + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + challenger = &GPTP_PORT_BMCA_DATA(port)->port_priority; + pa_info_state = &GPTP_PORT_STATE(port)->pa_info; + pss_rcv = &GPTP_PORT_STATE(port)->pss_rcv; + + if (pa_info_state->ann_expired || + (global_ds->gm_present && + pss_rcv->rcv_sync_receipt_timeout_timer_expired)) { + continue; + } + + if (memcmp(challenger->src_port_id.clk_id, default_ds->clk_id, + GPTP_CLOCK_ID_LEN) == 0) { + /* Discard this challenger. */ + continue; + } + + if (best_port == 0) { + tmp = memcmp(&challenger->root_system_id, + &best_vector->root_system_id, + sizeof(struct gptp_root_system_identity)); + if (tmp < 0) { + best_vector = challenger; + best_port = port; + } else if (tmp > 0) { + continue; + } + + tmp = (int)challenger->steps_removed - + ((int)ntohs(best_vector->steps_removed) + 1); + if (tmp < 0) { + best_vector = challenger; + best_port = port; + } else if (tmp > 0) { + continue; + } + + tmp = memcmp(&challenger->src_port_id, + &best_vector->src_port_id, + sizeof(struct gptp_port_identity)); + if (tmp < 0) { + best_vector = challenger; + best_port = port; + } else if (tmp > 0) { + continue; + } + + if (ntohs(challenger->port_number) < + ntohs(best_vector->port_number)) { + best_vector = challenger; + best_port = port; + } + + } else { + /* We can compare portPriority vectors without + * calculating pathPriority vectors. + */ + if (memcmp(challenger, best_vector, + sizeof(struct gptp_priority_vector)) < 0) { + best_vector = challenger; + best_port = port; + } + } + } + + if (best_port != 0) { + memcpy(&global_ds->gm_priority.root_system_id, + &best_vector->root_system_id, + sizeof(struct gptp_root_system_identity)); + + global_ds->gm_priority.steps_removed = + htons(ntohs(best_vector->steps_removed) + 1); + + memcpy(&global_ds->gm_priority.src_port_id, + &best_vector->src_port_id, + sizeof(struct gptp_port_identity)); + + global_ds->gm_priority.port_number = best_vector->port_number; + } + + return best_port; +} + +static void update_bmca(int port, + int best_port, + struct gptp_global_ds *global_ds, + struct gptp_default_ds *default_ds, + struct gptp_priority_vector *gm_prio) +{ + struct gptp_port_bmca_data *bmca_data = GPTP_PORT_BMCA_DATA(port); + + /* Update masterPriorityVector for the port. */ + if (best_port == 0) { + memcpy(&bmca_data->master_priority, gm_prio, + sizeof(struct gptp_priority_vector)); + + bmca_data->master_priority.port_number = htons(port); + bmca_data->master_priority.src_port_id.port_number = + htons(port); + } else { + memcpy(&bmca_data->master_priority.root_system_id, + &gm_prio->root_system_id, + sizeof(struct gptp_root_system_identity)); + memcpy(bmca_data->master_priority.src_port_id.clk_id, + default_ds->clk_id, GPTP_CLOCK_ID_LEN); + bmca_data->master_priority.port_number = htons(port); + bmca_data->master_priority.src_port_id.port_number = + htons(port); + } + + switch (bmca_data->info_is) { + case GPTP_INFO_IS_DISABLED: + global_ds->selected_role[port] = GPTP_PORT_DISABLED; + break; + + case GPTP_INFO_IS_AGED: + bmca_data->updt_info = true; + global_ds->selected_role[port] = GPTP_PORT_MASTER; + break; + + case GPTP_INFO_IS_MINE: + global_ds->selected_role[port] = GPTP_PORT_MASTER; + if ((memcmp(&bmca_data->port_priority, + &bmca_data->master_priority, + sizeof(struct gptp_priority_vector)) != 0) || + (bmca_data->port_steps_removed != + global_ds->master_steps_removed)) { + bmca_data->updt_info = true; + } + + break; + + case GPTP_INFO_IS_RECEIVED: + if (best_port == port) { + /* gmPriorityVector is now derived from + * portPriorityVector. + */ + global_ds->selected_role[port] = GPTP_PORT_SLAVE; + bmca_data->updt_info = false; + } else if (memcmp(&bmca_data->port_priority, + &bmca_data->master_priority, + sizeof(struct gptp_priority_vector)) <= 0) { + /* The masterPriorityVector is not better than + * the portPriorityVector. + */ + global_ds->selected_role[port] = GPTP_PORT_PASSIVE; + + if (memcmp(bmca_data->port_priority.src_port_id.clk_id, + default_ds->clk_id, + GPTP_CLOCK_ID_LEN)) { + /* The sourcePortIdentity component of + * the portPriorityVector does not + * reflect another port on the + * time-aware system. + */ + bmca_data->updt_info = true; + } else { + bmca_data->updt_info = false; + } + } else { + global_ds->selected_role[port] = GPTP_PORT_MASTER; + bmca_data->updt_info = true; + } + + break; + } +} + +static void gptp_updt_roles_tree(void) +{ + struct gptp_global_ds *global_ds; + struct gptp_default_ds *default_ds; + struct gptp_priority_vector *gm_prio, *last_gm_prio; + struct gptp_port_bmca_data *bmca_data; + int port, best_port; + + global_ds = GPTP_GLOBAL_DS(); + default_ds = GPTP_DEFAULT_DS(); + + gm_prio = &global_ds->gm_priority; + last_gm_prio = &global_ds->last_gm_priority; + + /* Save gmPriority. */ + memcpy(last_gm_prio, gm_prio, sizeof(struct gptp_priority_vector)); + + best_port = compute_best_vector(); + + /* If the best vector was the systemPriorityVector. */ + if (best_port == 0) { + /* Copy leap61, leap59, current UTC offset valid, + * time traceable and frequency traceable flags. + */ + global_ds->global_flags.octets[1] = + global_ds->sys_flags.octets[1]; + global_ds->current_utc_offset = + global_ds->sys_current_utc_offset; + global_ds->time_source = global_ds->sys_time_source; + global_ds->master_steps_removed = 0; + } else { + bmca_data = GPTP_PORT_BMCA_DATA(best_port); + + /* Copy leap61, leap59, current UTC offset valid, + * time traceable and frequency traceable flags. + */ + global_ds->global_flags.octets[1] = + bmca_data->ann_flags.octets[1]; + global_ds->current_utc_offset = + global_ds->sys_current_utc_offset; + global_ds->time_source = bmca_data->ann_time_source; + global_ds->master_steps_removed = + htons(ntohs(bmca_data->message_steps_removed) + 1); + } + + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + update_bmca(port, best_port, global_ds, default_ds, gm_prio); + } + + /* Update gmPresent. */ + global_ds->gm_present = + (gm_prio->root_system_id.grand_master_prio1 == 255) ? + false : true; + + /* Assign the port role for port 0. */ + for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) { + if (global_ds->selected_role[port] == GPTP_PORT_SLAVE) { + global_ds->selected_role[0] = GPTP_PORT_PASSIVE; + break; + } + } + + if (port == GPTP_PORT_END) { + global_ds->selected_role[0] = GPTP_PORT_SLAVE; + } + + /* If current system is the Grand Master, set pathTrace array. */ + if (memcmp(default_ds->clk_id, gm_prio->root_system_id.grand_master_id, + GPTP_CLOCK_ID_LEN) == 0) { + global_ds->path_trace.len = htons(GPTP_CLOCK_ID_LEN); + memcpy(global_ds->path_trace.path_sequence, + default_ds->clk_id, GPTP_CLOCK_ID_LEN); + } +} + +static void gptp_set_selected_tree(void) +{ + /* Set all the elements of the selected array to TRUE. */ + GPTP_GLOBAL_DS()->selected_array = ~0; +} + +static void gptp_mi_port_role_selection_state_machine(void) +{ + struct gptp_port_role_selection_state *state; + + state = &GPTP_STATE()->pr_sel; + + switch (state->state) { + case GPTP_PR_SELECTION_INIT_BRIDGE: + gptp_updt_role_disabled_tree(); + state->state = GPTP_PR_SELECTION_ROLE_SELECTION; + + /* Be sure to enter the "if" statement immediately after. */ + GPTP_GLOBAL_DS()->reselect_array = ~0; + /* Fallthrough. */ + + case GPTP_PR_SELECTION_ROLE_SELECTION: + if (GPTP_GLOBAL_DS()->reselect_array != 0) { + gptp_clear_reselect_tree(); + gptp_updt_roles_tree(); + gptp_set_selected_tree(); + } + + break; + } +} + +static void tx_announce(int port) +{ + struct net_pkt *pkt; + + pkt = gptp_prepare_announce(port); + if (pkt) { + gptp_send_announce(port, pkt); + } +} + +static void gptp_mi_port_announce_transmit_state_machine(int port) +{ + struct gptp_port_ds *port_ds; + struct gptp_global_ds *global_ds; + struct gptp_port_announce_transmit_state *state; + struct gptp_port_bmca_data *bmca_data; + + port_ds = GPTP_PORT_DS(port); + global_ds = GPTP_GLOBAL_DS(); + bmca_data = GPTP_PORT_BMCA_DATA(port); + state = &GPTP_PORT_STATE(port)->pa_transmit; + + /* Reset interval as defined in AnnounceIntervalSetting + * state machine. + */ + if (port_ds->ptt_port_enabled && !port_ds->prev_ptt_port_enabled) { + gptp_update_announce_interval(port, GPTP_ITV_SET_TO_INIT); + } + + switch (state->state) { + case GPTP_PA_TRANSMIT_INIT: + bmca_data->new_info = true; + /* Fallthrough. */ + + case GPTP_PA_TRANSMIT_IDLE: + k_timer_stop(&state->ann_send_periodic_timer); + state->ann_trigger = false; + k_timer_start(&state->ann_send_periodic_timer, + gptp_uscaled_ns_to_timer_ms( + &bmca_data->announce_interval), + 0); + + state->state = GPTP_PA_TRANSMIT_POST_IDLE; + /* Fallthrough. */ + + case GPTP_PA_TRANSMIT_POST_IDLE: + if (IS_SELECTED(global_ds, port) && + !bmca_data->updt_info && + state->ann_trigger) { + + state->state = GPTP_PA_TRANSMIT_PERIODIC; + + } else if (IS_SELECTED(global_ds, port) && + !bmca_data->updt_info && + !state->ann_trigger && + (global_ds->selected_role[port] == + GPTP_PORT_MASTER) && + bmca_data->new_info) { + + bmca_data->new_info = false; + tx_announce(port); + state->state = GPTP_PA_TRANSMIT_IDLE; + } + + break; + + case GPTP_PA_TRANSMIT_PERIODIC: + if (global_ds->selected_role[port] == GPTP_PORT_MASTER) { + bmca_data->new_info = true; + } + state->state = GPTP_PA_TRANSMIT_IDLE; + break; + } +} + + +void gptp_mi_port_sync_state_machines(int port) +{ + gptp_mi_pss_rcv_state_machine(port); + gptp_mi_pss_send_state_machine(port); +} + +void gptp_mi_port_bmca_state_machines(int port) +{ + gptp_mi_port_announce_receive_state_machine(port); + gptp_mi_port_announce_information_state_machine(port); + gptp_mi_port_announce_transmit_state_machine(port); +} + +void gptp_mi_state_machines(void) +{ + gptp_mi_site_sync_sync_state_machine(); + gptp_mi_clk_slave_sync_state_machine(); + gptp_mi_port_role_selection_state_machine(); + gptp_mi_clk_master_sync_rcv_state_machine(); +} diff --git a/subsys/net/l2/ethernet/gptp/gptp_mi.h b/subsys/net/l2/ethernet/gptp/gptp_mi.h new file mode 100644 index 00000000000000..511621e7fbc8bc --- /dev/null +++ b/subsys/net/l2/ethernet/gptp/gptp_mi.h @@ -0,0 +1,71 @@ +/* + * Copyright (c) 2017 Intel Corporation. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @file + * @brief GPTP Media Independent interface + * + * This is not to be included by the application. + */ + +#ifndef __GPTP_MI_H +#define __GPTP_MI_H + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(CONFIG_NET_GPTP) + +#include "gptp_md.h" + +/** + * @brief Media Independent Sync Information. + * + * This structure applies for MDSyncReceive as well as MDSyncSend. + */ +struct gptp_mi_port_sync_sync { + /** Port to which the Sync Information belongs to. */ + u16_t local_port_number; + + /** Time at which the sync receipt timeout occurs. */ + u64_t sync_receipt_timeout_time; + + /** Copy of the gptp_md_sync_info to be transmitted. */ + struct gptp_md_sync_info sync_info; +}; + +/** + * @brief Initialize all Media Independent State Machines. + */ +void gptp_mi_init_state_machine(void); + +/** + * @brief Run all Media Independent Port Sync State Machines. + * + * @param port Number of the port the State Machines needs to be run on. + */ +void gptp_mi_port_sync_state_machines(int port); + +/** + * @brief Run all Media Independent Port BMCA State Machines. + * + * @param port Number of the port the State Machines needs to be run on. + */ +void gptp_mi_port_bmca_state_machines(int port); + +/** + * @brief Run all Media Independent State Machines. + */ +void gptp_mi_state_machines(void); + +#endif /* CONFIG_NET_GPTP */ + +#ifdef __cplusplus +} +#endif + +#endif /* __GPTP_MI_H */ diff --git a/subsys/net/l2/ethernet/gptp/gptp_private.h b/subsys/net/l2/ethernet/gptp/gptp_private.h new file mode 100644 index 00000000000000..382f8df42defbb --- /dev/null +++ b/subsys/net/l2/ethernet/gptp/gptp_private.h @@ -0,0 +1,130 @@ +/* + * Copyright (c) 2017 Intel Corporation. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @file + * @brief Private functions for the Precision Time Protocol Stack. + * + * This is not to be included by the application. + */ + +#ifndef __GPTP_PRIVATE_H +#define __GPTP_PRIVATE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(CONFIG_NET_GPTP) + +#include + +/* Common defines for the gPTP stack. */ +#define GPTP_THREAD_WAIT_TIMEOUT_MS 1 +#define GPTP_MULTIPLE_PDELAY_RESP_WAIT K_MINUTES(5) + +#define USCALED_NS_TO_MS(val) ((val >> 16) / 1000000) + +#if defined(CONFIG_NET_GPTP_STATISTICS) +#define GPTP_STATS_INC(port, var) (GPTP_PORT_PARAM_DS(port)->var++) +#else +#define GPTP_STATS_INC(port, var) +#endif + +/** + * @brief Is a slave acting as a slave. + * + * Utility to check if a port is configured as a slave. + * + * @param port Port to check. + * + * @return True if this is a slave port. + */ +bool gptp_is_slave_port(int port); + +/** + * @brief Convert the network interface to the correct port number. + * + * @param iface Network Interface acting as a ptp port. + * + * @return Number of the port if found, ENODEV otherwise. + */ +int gptp_get_port_number(struct net_if *iface); + +/** + * @brief Calculate a logInteral and store in Uscaled ns structure. + * + * @param interval Result of calculation. + * + * @param seconds Seconds of interval. + * + * @param log_msg_interval Logarithm 2 to apply to this interval. + */ +void gptp_set_time_itv(struct gptp_uscaled_ns *interval, + u16_t seconds, + s8_t log_msg_interval); + +/** + * @brief Convert uscaled ns to ms for timer use. + * + * @param usns Pointer to uscaled nanoseconds to convert. + * + * @return INT32_MAX if value exceed timer max value, 0 if the result of the + * conversion is less 1ms, the converted value otherwise. + */ +s32_t gptp_uscaled_ns_to_timer_ms(struct gptp_uscaled_ns *usns); + +/** + * @brief Update pDelay request interval and its timer. + * + * @param port Port number. + * + * @param log_val New logarithm 2 to apply to this interval. + */ +void gptp_update_pdelay_req_interval(int port, s8_t log_val); + +/** + * @brief Update sync interval and its timer. + * + * @param port Port number. + * + * @param log_val New logarithm 2 to apply to this interval. + */ +void gptp_update_sync_interval(int port, s8_t log_val); + +/** + * @brief Update announce interval and its timer. + * + * @param port Port number. + * + * @param log_val New logarithm 2 to apply to this interval. + */ + +void gptp_update_announce_interval(int port, s8_t log_val); + +/** + * @brief Convert a ptp timestamp to nanoseconds. + * + * @param ts A PTP timestamp. + * + * @return Number of nanoseconds. + */ +static inline u64_t gptp_timestamp_to_nsec(struct net_ptp_time *ts) +{ + if (!ts) { + return 0; + } + + return (ts->second * NSEC_PER_SEC) + ts->nanosecond; +} + +#endif /* CONFIG_NET_GPTP */ + +#ifdef __cplusplus +} +#endif + +#endif /* __GPTP_PRIVATE_H */ diff --git a/subsys/net/l2/ethernet/gptp/gptp_state.h b/subsys/net/l2/ethernet/gptp/gptp_state.h new file mode 100644 index 00000000000000..f754f79e1a8335 --- /dev/null +++ b/subsys/net/l2/ethernet/gptp/gptp_state.h @@ -0,0 +1,534 @@ +/* + * Copyright (c) 2017 Intel Corporation. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** @file + * @brief PTP state machines + * + * This is not to be included by the application. + */ + +#ifndef __GPTP_STATE_H +#define __GPTP_STATE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(CONFIG_NET_GPTP) + +#include "gptp_mi.h" + +/* PDelayRequest states. */ +enum gptp_pdelay_req_states { + GPTP_PDELAY_REQ_NOT_ENABLED, + GPTP_PDELAY_REQ_INITIAL_SEND_REQ, + GPTP_PDELAY_REQ_RESET, + GPTP_PDELAY_REQ_SEND_REQ, + GPTP_PDELAY_REQ_WAIT_RESP, + GPTP_PDELAY_REQ_WAIT_FOLLOW_UP, + GPTP_PDELAY_REQ_WAIT_ITV_TIMER, +}; + +/* Path Delay Response states. */ +enum gptp_pdelay_resp_states { + GPTP_PDELAY_RESP_NOT_ENABLED, + GPTP_PDELAY_RESP_INITIAL_WAIT_REQ, + GPTP_PDELAY_RESP_WAIT_REQ, + GPTP_PDELAY_RESP_WAIT_TSTAMP, +}; + +/* SyncReceive states. */ +enum gptp_sync_rcv_states { + GPTP_SYNC_RCV_DISCARD, + GPTP_SYNC_RCV_WAIT_SYNC, + GPTP_SYNC_RCV_WAIT_FOLLOW_UP, +}; + +/* SyncSend states. */ +enum gptp_sync_send_states { + GPTP_SYNC_SEND_INITIALIZING, + GPTP_SYNC_SEND_SEND_SYNC, + GPTP_SYNC_SEND_SEND_FUP, +}; + +/* PortSyncSyncReceive states. */ +enum gptp_pss_rcv_states { + GPTP_PSS_RCV_DISCARD, + GPTP_PSS_RCV_RECEIVED_SYNC, +}; + +/* PortSyncSyncSend states. */ +enum gptp_pss_send_states { + GPTP_PSS_SEND_TRANSMIT_INIT, + GPTP_PSS_SEND_SYNC_RECEIPT_TIMEOUT, + GPTP_PSS_SEND_SEND_MD_SYNC, + GPTP_PSS_SEND_SET_SYNC_RECEIPT_TIMEOUT, +}; + +/* SiteSyncSyncReceive states. */ +enum gptp_site_sync_sync_states { + GPTP_SSS_INITIALIZING, + GPTP_SSS_RECEIVING_SYNC, +}; + +/* ClockSlaveSync states. */ +enum gptp_clk_slave_sync_states { + GPTP_CLK_SLAVE_SYNC_INITIALIZING, + GPTP_CLK_SLAVE_SYNC_SEND_SYNC_IND, +}; + +/* PortAnnounceReceive states. */ +enum gptp_pa_rcv_states { + GPTP_PA_RCV_DISCARD, + GPTP_PA_RCV_RECEIVE, +}; + +/* PortAnnounceInformation states. */ +enum gptp_pa_info_states { + GPTP_PA_INFO_DISABLED, + /* State to handle the transition after DISABLED state. */ + GPTP_PA_INFO_POST_DISABLED, + GPTP_PA_INFO_AGED, + GPTP_PA_INFO_UPDATE, + GPTP_PA_INFO_CURRENT, + GPTP_PA_INFO_RECEIVE, + GPTP_PA_INFO_SUPERIOR_MASTER_PORT, + GPTP_PA_INFO_REPEATED_MASTER_PORT, + GPTP_PA_INFO_INFERIOR_MASTER_OR_OTHER_PORT, +}; + +/* PortRoleSelection states. */ +enum gptp_pr_selection_states { + GPTP_PR_SELECTION_INIT_BRIDGE, + GPTP_PR_SELECTION_ROLE_SELECTION, +}; + +/* PortAnnounceTransmit states. */ +enum gptp_pa_transmit_states { + GPTP_PA_TRANSMIT_INIT, + GPTP_PA_TRANSMIT_PERIODIC, + GPTP_PA_TRANSMIT_IDLE, + GPTP_PA_TRANSMIT_POST_IDLE, +}; + +/* ClockMasterSyncReceive states. */ +enum gptp_cms_rcv_states { + GPTP_CMS_RCV_INITIALIZING, + GPTP_CMS_RCV_WAITING, + GPTP_CMS_RCV_SOURCE_TIME, +}; + +/* Info_is enumeration2. */ +enum gptp_info_is { + GPTP_INFO_IS_RECEIVED, + GPTP_INFO_IS_MINE, + GPTP_INFO_IS_AGED, + GPTP_INFO_IS_DISABLED, +}; + +enum gptp_time_source { + GPTP_TS_ATOMIC_CLOCK = 0x10, + GPTP_TS_GPS = 0x20, + GPTP_TS_TERRESTRIAL_AUDIO = 0x30, + GPTS_TS_PTP = 0x40, + GPTP_TS_NTP = 0x50, + GPTP_TS_HAND_SET = 0x60, + GPTP_TS_OTHER = 0x90, + GPTP_TS_INTERNAL_OSCILLATOR = 0xA0, +}; + +/** + * @brief gPTP time-synchronization spanning tree priority vector + * + * Defines the best master selection information. + */ +struct gptp_priority_vector { + /** Identity of the source clock. */ + struct gptp_root_system_identity root_system_id; + + /** portNumber of the receiving port. */ + u16_t port_number; + + /** Port identity of the transmitting time-aware system. */ + struct gptp_port_identity src_port_id; + + /** Steps removed from the announce message transmitter and the + * master clock. + */ + u16_t steps_removed; +} __packed; + +/* Pdelay Request state machine variables. */ +struct gptp_pdelay_req_state { + /** Initial Path Delay Response Peer Timestamp. */ + u64_t ini_resp_evt_tstamp; + + /** Initial Path Delay Response Ingress Timestamp. */ + u64_t ini_resp_ingress_tstamp; + + /** Timer for the Path Delay Request. */ + struct k_timer pdelay_timer; + + /** Pointer to the received Path Delay Response. */ + struct net_pkt *rcvd_pdelay_resp_ptr; + + /** Pointer to the received Path Delay Follow Up. */ + struct net_pkt *rcvd_pdelay_follow_up_ptr; + + /** Pointer to the Path Delay Request to be transmitted. */ + struct net_pkt *tx_pdelay_req_ptr; + + /** Current state of the state machine. */ + enum gptp_pdelay_req_states state; + + /** Path Delay Response messages received. */ + u32_t rcvd_pdelay_resp; + + /** Path Delay Follow Up messages received. */ + u32_t rcvd_pdelay_follow_up; + + /** Number of lost Path Delay Responses. */ + u16_t lost_responses; + + /** Timer expired, a new Path Delay Request needs to be sent. */ + bool pdelay_timer_expired; + + /** NeighborRateRatio has been computed successfully. */ + bool neighbor_rate_ratio_valid; + + /** Path Delay has already been computed after initialization. */ + bool init_pdelay_compute; + + /** Count consecutive Pdelay_req with multiple responses. */ + u8_t multiple_resp_count; +}; + +/** + * @brief Pdelay Response state machine variables. + */ +struct gptp_pdelay_resp_state { + /** Current state of the state machine. */ + enum gptp_pdelay_resp_states state; +}; + +/* SyncReceive state machine variables. */ +struct gptp_sync_rcv_state { + /** Time at which a Sync Message without Follow Up will be discarded. */ + u64_t follow_up_receipt_timeout; + + /** Timer for the Follow Up discard. */ + struct k_timer follow_up_discard_timer; + + /** Pointer to the received Sync message. */ + struct net_pkt *rcvd_sync_ptr; + + /** Pointer to the received Follow Up message. */ + struct net_pkt *rcvd_follow_up_ptr; + + /** Current state of the state machine. */ + enum gptp_sync_rcv_states state; + + /** A Sync Message has been received. */ + bool rcvd_sync; + + /** A Follow Up Message has been received. */ + bool rcvd_follow_up; + + /** A Follow Up Message has been received. */ + bool follow_up_timeout_expired; +}; + +/* SyncSend state machine variables. */ +struct gptp_sync_send_state { + /** Pointer to the received MDSyncSend structure. */ + struct gptp_md_sync_info *sync_send_ptr; + + /** Pointer to the sync message to be sent. */ + struct net_pkt *sync_ptr; + + /** Current state of the state machine. */ + enum gptp_sync_send_states state; + + /** A MDSyncSend structure has been received. */ + bool rcvd_md_sync; + + /** The timestamp for the sync message has been received. */ + bool md_sync_timestamp_avail; +}; + +/* PortSyncSyncReceive state machine variables. */ +struct gptp_pss_rcv_state { + /** Sync receive provided by the MD Sync Receive State Machine. */ + struct gptp_md_sync_info sync_rcv; + + /** PortSyncSync structure to be transmitted to the Site Sync Sync. */ + struct gptp_mi_port_sync_sync pss; + + /** SyncReceiptTimeoutTimer for PortAnnounce state machines. */ + struct k_timer rcv_sync_receipt_timeout_timer; + + /** Ratio of the Grand Master frequency with the Local Clock. */ + double rate_ratio; + + /** Current state of the state machine. */ + enum gptp_pss_rcv_states state; + + /** A MDSyncReceive structure is ready to be processed. */ + bool rcvd_md_sync; + + /** Expiry of SyncReceiptTimeoutTimer. */ + bool rcv_sync_receipt_timeout_timer_expired; +}; + +/* PortSyncSyncSend state machine variables. */ +struct gptp_pss_send_state { + /** Sync send to be transmitted to the MD Sync Send State Machine. */ + struct gptp_md_sync_info sync_send; + + /** Source Port Identity of the last received PortSyncSync. */ + struct gptp_port_identity last_src_port_id; + + /** Precise Origin Timestamp of the last received PortSyncSync. */ + struct net_ptp_time last_precise_orig_ts; + + /** Half Sync Interval Timer. */ + struct k_timer half_sync_itv_timer; + + /** syncReceiptTimeout Timer. */ + struct k_timer send_sync_receipt_timeout_timer; + + /** GM Phase Change of the last received PortSyncSync. */ + struct gptp_scaled_ns last_gm_phase_change; + + /** Follow Up Correction Field of the last received PortSyncSync. */ + s64_t last_follow_up_correction_field; + + /** Upstream Tx Time of the last received PortSyncSync. */ + u64_t last_upstream_tx_time; + + /** Sync Receipt Timeout Time of the last received PortSyncSync. */ + u64_t last_sync_receipt_timeout_time; + + /** PortSyncSync structure received from the SiteSyncSync. */ + struct gptp_mi_port_sync_sync *pss_sync_ptr; + + /** Rate Ratio of the last received PortSyncSync. */ + double last_rate_ratio; + + /** GM Freq Change of the last received PortSyncSync. */ + double last_gm_freq_change; + + /** Current state of the state machine. */ + enum gptp_pss_send_states state; + + /** GM Time Base Indicator of the last received PortSyncSync. */ + u16_t last_gm_time_base_indicator; + + /** Received Port Number of the last received PortSyncSync. */ + u16_t last_rcvd_port_num; + + /** A PortSyncSync structure is ready to be processed. */ + bool rcvd_pss_sync; + + /** Flag when the half_sync_itv_timer has expired. */ + bool half_sync_itv_timer_expired; + + /** Flag when the half_sync_itv_timer has expired twice. */ + bool sync_itv_timer_expired; + + /** Flag when the syncReceiptTimeoutTime has expired. */ + bool send_sync_receipt_timeout_timer_expired; +}; + +/* SiteSyncSync state machine variables. */ +struct gptp_site_sync_sync_state { + /** PortSyncSync structure to be sent to other ports and to the Slave. + */ + struct gptp_mi_port_sync_sync pss_send; + + /** Pointer to the PortSyncSync structure received. */ + struct gptp_mi_port_sync_sync *pss_rcv_ptr; + + /** Current state of the state machine. */ + enum gptp_site_sync_sync_states state; + + /** A PortSyncSync structure is ready to be processed. */ + bool rcvd_pss; +}; + +/* ClockSlaveSync state machine variables. */ +struct gptp_clk_slave_sync_state { + /** Pointer to the PortSyncSync structure received. */ + struct gptp_mi_port_sync_sync *pss_rcv_ptr; + + /** Current state of the state machine. */ + enum gptp_clk_slave_sync_states state; + + /** A PortSyncSync structure is ready to be processed. */ + bool rcvd_pss; + + /** The local clock has expired. */ + bool rcvd_local_clk_tick; +}; + +/* ClockMasterSync state machine variables. */ +struct gptp_clk_master_sync_state { + /** Current state of the state machine */ + enum gptp_cms_rcv_states state; + + /** A ClockSourceTime.invoke function is received from the + * Clock source entity + */ + bool rcvd_clock_source_req; + + /** The local clock has expired */ + bool rcvd_local_clock_tick; +}; + +/* PortAnnounceReceive state machine variables. */ +struct gptp_port_announce_receive_state { + /** Current state of the state machine. */ + enum gptp_pa_rcv_states state; + + /** An announce message is ready to be processed. */ + bool rcvd_announce; +}; + +struct gptp_port_announce_information_state { + /** Timer for the announce expiry. */ + struct k_timer ann_rcpt_expiry_timer; + + /** PortRoleInformation state machine variables. */ + enum gptp_pa_info_states state; + + /* Expired announce information. */ + bool ann_expired; +}; + +/* PortRoleSelection state machine variables. */ +struct gptp_port_role_selection_state { + enum gptp_pr_selection_states state; +}; + +/** + * @brief PortAnnounceTransmit state machine variables. + */ +struct gptp_port_announce_transmit_state { + /** Timer for the announce expiry. */ + struct k_timer ann_send_periodic_timer; + + /** PortRoleTransmit state machine variables. */ + enum gptp_pa_transmit_states state; + + /** Trigger announce information. */ + bool ann_trigger; +}; + +/** + * @brief Structure maintaining per Time-Aware States. + */ +struct gptp_states { + /** SiteSyncSync state machine variables. */ + struct gptp_site_sync_sync_state site_ss; + + /** ClockSlaveSync state machine variables. */ + struct gptp_clk_slave_sync_state clk_slave_sync; + + /** PortRoleSelection state machine variables. */ + struct gptp_port_role_selection_state pr_sel; + + /** ClockMasterSyncReceive state machine variables. */ + struct gptp_clk_master_sync_state clk_master_sync_receive; +}; + +/** + * @brief Structure maintaining per Port States. + */ +struct gptp_port_states { + /** PathDelayRequest state machine variables. */ + struct gptp_pdelay_req_state pdelay_req; + + /** PathDelayResponse state machine variables. */ + struct gptp_pdelay_resp_state pdelay_resp; + + /** SyncReceive state machine variables. */ + struct gptp_sync_rcv_state sync_rcv; + + /** SyncSend state machine variables. */ + struct gptp_sync_send_state sync_send; + + /** PortSyncSyncReceive state machine variables. */ + struct gptp_pss_rcv_state pss_rcv; + + /** PortSyncSync Send state machine variables. */ + struct gptp_pss_send_state pss_send; + + /** PortAnnounceReceive state machine variables. */ + struct gptp_port_announce_receive_state pa_rcv; + + /** PortAnnounceInformation state machine variables. */ + struct gptp_port_announce_information_state pa_info; + + /** PortAnnounceTransmit state machine variables. */ + struct gptp_port_announce_transmit_state pa_transmit; +}; + +/** + * @brief Structure maintaining per port BMCA state machines variables. + */ +struct gptp_port_bmca_data { + /** Pointer to announce message. */ + struct net_pkt *rcvd_announce_ptr; + + /** The masterPriorityVector for the port. */ + struct gptp_priority_vector master_priority; + + /** The portPriorityVector for the port. */ + struct gptp_priority_vector port_priority; + + /** Announce interval. */ + struct gptp_uscaled_ns announce_interval; + + /** Announce receipt timeout time interval. */ + struct gptp_uscaled_ns ann_rcpt_timeout_time_interval; + + /** Last announce message flags. */ + struct gptp_flags ann_flags; + + /** Origin and state of the port's spanning tree information. */ + enum gptp_info_is info_is; + + /** Last announce message time source. */ + enum gptp_time_source ann_time_source; + + /** The value of steps removed for the port. */ + u16_t port_steps_removed; + + /** The value of steps removed for the port. */ + u16_t message_steps_removed; + + /** Last announce message current UTC offset value. */ + s16_t ann_current_utc_offset; + + /** A qualified announce message has been received. */ + bool rcvd_msg; + + /** Indicate if PortAnnounceInformation should copy the newly determined + * master_prioriry and master_steps_removed. + */ + bool updt_info; + + /** Cause a port to transmit Announce Information. */ + bool new_info; +}; + +#endif /* CONFIG_NET_GPTP */ + +#ifdef __cplusplus +} +#endif + +#endif /* __GPTP_STATE_H */ diff --git a/subsys/net/l2/ethernet/gptp/gptp_user_api.c b/subsys/net/l2/ethernet/gptp/gptp_user_api.c new file mode 100644 index 00000000000000..66c44feefe686f --- /dev/null +++ b/subsys/net/l2/ethernet/gptp/gptp_user_api.c @@ -0,0 +1,83 @@ +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#if defined(CONFIG_NET_DEBUG_GPTP) +#define SYS_LOG_DOMAIN "net/gptp" +#endif + +#include +#include + +#include "gptp_messages.h" +#include "gptp_data_set.h" + +#include "net_private.h" + +static sys_slist_t phase_dis_callbacks; + +void gptp_register_phase_dis_cb(struct gptp_phase_dis_cb *phase_dis, + gptp_phase_dis_callback_t cb) +{ + sys_slist_find_and_remove(&phase_dis_callbacks, &phase_dis->node); + sys_slist_prepend(&phase_dis_callbacks, &phase_dis->node); + + phase_dis->cb = cb; +} + +void gptp_unregister_phase_dis_cb(struct gptp_phase_dis_cb *phase_dis) +{ + sys_slist_find_and_remove(&phase_dis_callbacks, &phase_dis->node); +} + +void gptp_call_phase_dis_cb(void) +{ + struct gptp_global_ds *global_ds; + sys_snode_t *sn, *sns; + u8_t *gm_id; + + global_ds = GPTP_GLOBAL_DS(); + gm_id = &global_ds->gm_priority.root_system_id.grand_master_id[0]; + + SYS_SLIST_FOR_EACH_NODE_SAFE(&phase_dis_callbacks, sn, sns) { + struct gptp_phase_dis_cb *phase_dis = + CONTAINER_OF(sn, struct gptp_phase_dis_cb, node); + + phase_dis->cb(gm_id, + &global_ds->gm_time_base_indicator, + &global_ds->clk_src_last_gm_phase_change, + &global_ds->clk_src_last_gm_freq_change); + } +} + +int gptp_event_capture(struct net_ptp_time *slave_time, bool *gm_present) +{ + int port, key; + struct device *clk; + + key = irq_lock(); + *gm_present = GPTP_GLOBAL_DS()->gm_present; + + for (port = GPTP_PORT_START; port <= GPTP_PORT_END; port++) { + /* Get first available clock, or slave clock if GM present. */ + if (!*gm_present || (GPTP_GLOBAL_DS()->selected_role[port] == + GPTP_PORT_SLAVE)) { + clk = net_eth_get_ptp_clock(GPTP_PORT_IFACE(port)); + if (clk) { + ptp_clock_get(clk, slave_time); + irq_unlock(key); + return 0; + } + } + } + + irq_unlock(key); + return -EAGAIN; +} + +char *gptp_sprint_clock_id(const u8_t *clk_id, char *output, size_t output_len) +{ + return net_sprint_ll_addr_buf(clk_id, 8, output, output_len); +} diff --git a/tests/net/all/prj.conf b/tests/net/all/prj.conf index ae7b5afaf32777..33c40d7a05237a 100644 --- a/tests/net/all/prj.conf +++ b/tests/net/all/prj.conf @@ -228,3 +228,8 @@ CONFIG_HTTP_PARSER_STRICT=y # VLAN CONFIG_NET_VLAN=y + +# gPTP +CONFIG_NET_GPTP=y +CONFIG_NET_GPTP_STATISTICS=y +CONFIG_NET_DEBUG_GPTP=y