flexPTP basic excerpt

2023-04-16 21:20:57 +02:00 · 2023-04-16 21:20:57 +02:00 · ac63947fe1
commit ac63947fe1
55 changed files with 3485 additions and 0 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -0,0 +1,45 @@
+set(FLEXPTP_SRC
+        # core files
+        cli_cmds.c
+        cli_cmds.h
+        clock_utils.c
+        clock_utils.h
+        format_utils.c
+        format_utils.h
+        logging.c
+        logging.h
+        msg_utils.c
+        msg_utils.h
+        ptp_core.c
+        ptp_core.h
+        sbmc.c
+        sbmc.h
+        stats.c
+        stats.h
+        timeutils.c
+        timeutils.h
+        ptp_defs.h
+        ptp_types.h
+
+        # servo files
+        servo/pd_controller.c
+        servo/pd_controller.h
+
+        # hw-port files
+        hw_port/flexptp_options_simulation.h
+        hw_port/ptp_port_simulation.cpp
+        hw_port/ptp_port_simulation.h
+
+        # simulation objects
+        hw_port/simsrc/lwip_simulation.h
+        hw_port/simsrc/lwip_simulation.cpp
+        hw_port/simsrc/FreeRTOS_simulation.h
+        hw_port/simsrc/FreeRTOS_simulation.cpp
+        hw_port/simsrc/PtpClock.h
+        hw_port/simsrc/PtpClock.cpp
+        hw_port/simsrc/PtpMasterClock.h
+        hw_port/simsrc/PtpMasterClock.cpp
+        hw_port/simsrc/PtpSlaveClock.h
+        hw_port/simsrc/PtpSlaveClock.cpp
+
+        PARENT_SCOPE)
--- a/cli_cmds.c
+++ b/cli_cmds.c
@ -0,0 +1,171 @@
+#include "cli_cmds.h"
+
+#include "ptp_core.h"
+#include "msg_utils.h"
+#include "format_utils.h"
+#include "logging.h"
+#include "clock_utils.h"
+#include "ptp_profile_presets.h"
+
+#include "settings_interface.h"
+
+#include <ctype.h>
+
+//#define S (gPtpCoreState)
+
+// ---- COMPILE ONLY IF CLI_REG_CMD is provided ----
+
+#ifdef CLI_REG_CMD
+
+static int CB_reset(const CliToken_Type * ppArgs, uint8_t argc)
+{
+    ptp_reset();
+    MSG("> PTP reset!\n");
+    return 0;
+}
+
+static int CB_offset(const CliToken_Type * ppArgs, uint8_t argc)
+{
+    if (argc > 0) {
+        ptp_set_clock_offset(atoi(ppArgs[0]));
+    }
+
+    MSG("> PTP clock offset: %d ns\n", ptp_get_clock_offset());
+    return 0;
+}
+
+static int CB_log(const CliToken_Type * ppArgs, uint8_t argc)
+{
+    bool logEn = false;
+
+    if (argc > 1) {
+        if (!strcmp(ppArgs[1], "on")) {
+            logEn = true;
+        } else if (!strcmp(ppArgs[1], "off")) {
+            logEn = false;
+        } else {
+            return -1;
+        }
+
+        if (!strcmp(ppArgs[0], "def")) {
+            ptp_enable_logging(PTP_LOG_DEF, logEn);
+        } else if (!strcmp(ppArgs[0], "corr")) {
+            ptp_enable_logging(PTP_LOG_CORR_FIELD, logEn);
+        } else if (!strcmp(ppArgs[0], "ts")) {
+            ptp_enable_logging(PTP_LOG_TIMESTAMPS, logEn);
+        } else if (!strcmp(ppArgs[0], "info")) {
+            ptp_enable_logging(PTP_LOG_INFO, logEn);
+        } else if (!strcmp(ppArgs[0], "locked")) {
+            ptp_enable_logging(PTP_LOG_LOCKED_STATE, logEn);
+        } else {
+            return -1;
+        }
+
+    } else {
+        return -1;
+    }
+
+    return 0;
+}
+
+static int CB_time(const CliToken_Type * ppArgs, uint8_t argc)
+{
+    char datetime[24];
+    TimestampU t;
+    ptp_time(&t);
+
+    if (argc > 0 && !strcmp(ppArgs[0], "ns")) {
+        MSG("%lu %u\n", t.sec, t.nanosec);
+    } else {
+        tsPrint(datetime, (const TimestampI *)&t);
+        MSG("%s\n", datetime);
+    }
+
+    return 0;
+}
+
+static int CB_master(const CliToken_Type * ppArgs, uint8_t argc)
+{
+    if (argc > 0) {
+        if (!strcmp(ppArgs[0], "prefer")) {
+            uint64_t idM = ptp_get_current_master_clock_identity();
+            if (argc > 1) {
+                idM = hextoclkid(ppArgs[1]);
+            }
+            ptp_prefer_master_clock(idM);
+        } else if (!strcmp(ppArgs[0], "unprefer")) {
+            ptp_unprefer_master_clock();
+        }
+    }
+
+    MSG("Master clock ID: ");
+    ptp_print_clock_identity(ptp_get_current_master_clock_identity());
+    MSG("\n");
+    return 0;
+}
+
+static int CB_ptpinfo(const CliToken_Type * ppArgs, uint8_t argc)
+{
+    MSG("Own clock ID: ");
+    ptp_print_clock_identity(ptp_get_own_clock_identity());
+    MSG("\nMaster clock ID: ");
+    ptp_print_clock_identity(ptp_get_current_master_clock_identity());
+    MSG("\n");
+    return 0;
+}
+
+static int CB_ptpdomain(const CliToken_Type * ppArgs, uint8_t argc)
+{
+    if (argc > 0) {
+        ptp_set_domain(atoi(ppArgs[0]));
+    }
+    MSG("PTP domain: %d\n", (int)ptp_get_domain());
+    return 0;
+}
+
+static int CB_addend(const CliToken_Type * ppArgs, uint8_t argc)
+{
+    if (argc > 0) {
+        ptp_set_addend((uint32_t) atol(ppArgs[0]));
+    }
+    MSG("Addend: %u\n", ptp_get_addend());
+    return 0;
+}
+
+static char *sTransportTypeHint[] = { "IPv4", "802.3" };
+
+// command assignments
+enum PTP_CMD_IDS {
+    CMD_RESET, CMD_OFFSET, CMD_LOG, CMD_TIME, CMD_MASTER, CMD_PTPINFO, CMD_PTPDOMAIN, CMD_ADDEND,
+
+    CMD_N
+};
+
+// command descriptors
+static int sCmds[CMD_N + 1];
+
+#endif                          // CLI_REG_CMD
+
+// register cli commands
+void ptp_register_cli_commands()
+{
+#ifdef CLI_REG_CMD
+    sCmds[CMD_RESET] = cli_register_command("ptp reset \t\t\tReset PTP subsystem", 2, 0, CB_reset);
+    sCmds[CMD_OFFSET] = cli_register_command("ptp servo offset [offset_ns] \t\t\tSet or query clock offset", 3, 0, CB_offset);
+    sCmds[CMD_LOG] = cli_register_command("ptp log {def|corr|ts|info|locked} {on|off} \t\t\tTurn on or off logging", 2, 2, CB_log);
+    sCmds[CMD_TIME] = cli_register_command("time [ns] \t\t\tPrint time", 1, 0, CB_time);
+    sCmds[CMD_MASTER] = cli_register_command("ptp master [[un]prefer] [clockid] \t\t\tMaster clock settings", 2, 0, CB_master);
+    sCmds[CMD_PTPINFO] = cli_register_command("ptp info \t\t\tPrint PTP info", 2, 0, CB_ptpinfo);
+    sCmds[CMD_PTPDOMAIN] = cli_register_command("ptp domain [domain]\t\t\tPrint or get PTP domain", 2, 0, CB_ptpdomain);
+    sCmds[CMD_ADDEND] = cli_register_command("ptp addend [addend]\t\t\tPrint or set addend", 2, 0, CB_addend);
+
+    sCmds[CMD_N] = -1;
+#endif                          // CLI_REG_CMD
+}
+
+void ptp_remove_cli_commands()
+{
+#ifdef CLI_REG_CMD
+    cli_remove_command_array(sCmds);
+#endif                          // CLI_REG_CMD
+}
--- a/cli_cmds.h
+++ b/cli_cmds.h
@ -0,0 +1,7 @@
+#ifndef FLEXPTP_CLI_CMDS_H_
+#define FLEXPTP_CLI_CMDS_H_
+
+void ptp_register_cli_commands();
+void ptp_remove_cli_commands();
+
+#endif                          /* FLEXPTP_CLI_CMDS_H_ */
--- a/clock_utils.c
+++ b/clock_utils.c
@ -0,0 +1,34 @@
+#include <flexptp/clock_utils.h>
+#include <stdint.h>
+
+#include <flexptp_options.h>
+
+#include "ptp_core.h"
+
+#define S (gPtpCoreState)
+
+// print clock identity
+void ptp_print_clock_identity(uint64_t clockID)
+{
+    uint8_t *p = (uint8_t *) & clockID;
+    uint8_t i;
+    for (i = 0; i < 8; i++) {   // reverse byte order due to Network->Host byte order conversion
+        MSG("%02x", p[7 - i]);
+    }
+}
+
+// create clock identity based on MAC address
+void ptp_create_clock_identity()
+{
+    uint8_t *p = (uint8_t *) & S.hwoptions.clockIdentity;
+    // construct clockIdentity
+    memcpy(p, netif_default->hwaddr, 3);        // first 3 octets of MAC address
+    p[3] = 0xff;
+    p[4] = 0xfe;
+    memcpy(&p[5], &netif_default->hwaddr[3], 3);        // last 3 octets of MAC address
+
+    // display ID
+    MSG("Own clock ID: ");
+    ptp_print_clock_identity(S.hwoptions.clockIdentity);
+    MSG("\n");
+}
--- a/clock_utils.h
+++ b/clock_utils.h
@ -0,0 +1,13 @@
+#ifndef FLEXPTP_CLOCK_UTILS_H_
+#define FLEXPTP_CLOCK_UTILS_H_
+
+#include <stdint.h>
+
+void ptp_print_clock_identity(uint64_t clockID);        // print clock identity
+
+void ptp_create_clock_identity();       // create clock identity based on MAC address
+
+void ptp_set_clock_offset(int32_t offset);      // set PPS offset
+int32_t ptp_get_clock_offset(); // get PPS offset
+
+#endif                          /* FLEXPTP_CLOCK_UTILS_H_ */
--- a/format_utils.c
+++ b/format_utils.c
@ -0,0 +1,39 @@
+/* (C) András Wiesner, 2020-2022 */
+
+#include "format_utils.h"
+
+#include <stdint.h>
+
+// lookup table for log msg. period to ms conversion (ms values are floor-rounded)
+#define LOG_INTERVAL_LOOKUP_OFFSET (6)
+static uint16_t sLogIntervalMs[] = { 15, 31, 62, 125, 250, 500, 1000, 2000, 4000, 8000, 0 };    // terminating zero as last element
+
+// log interval to milliseconds
+uint16_t ptp_logi2ms(int8_t logi)
+{
+    return sLogIntervalMs[logi + LOG_INTERVAL_LOOKUP_OFFSET];
+}
+
+// milliseconds to log interval
+int8_t ptp_ms2logi(uint16_t ms)
+{
+    uint16_t *pIter = sLogIntervalMs;
+    while (*pIter != 0) {
+        if (*pIter == ms) {
+            break;
+        }
+    }
+    return (int8_t) (pIter - sLogIntervalMs) + LOG_INTERVAL_LOOKUP_OFFSET;
+}
+
+// Network->Host byte order conversion for 64-bit values
+uint64_t ntohll(uint64_t in)
+{
+    unsigned char out[8] = { in >> 56, in >> 48, in >> 40, in >> 32, in >> 24, in >> 16, in >> 8, in };
+    return *(uint64_t *) out;
+}
+
+uint64_t htonll(uint64_t in)
+{
+    return ntohll(in);
+}
--- a/format_utils.h
+++ b/format_utils.h
@ -0,0 +1,13 @@
+/* (C) András Wiesner, 2020-2022 */
+
+#ifndef FLEXPTP_FORMAT_UTILS_H_
+#define FLEXPTP_FORMAT_UTILS_H_
+
+#include <stdint.h>
+
+uint16_t ptp_logi2ms(int8_t logi);      // log interval to milliseconds
+int8_t ptp_ms2logi(uint16_t ms);        // milliseconds to log interval
+uint64_t ntohll(uint64_t in);   // network->host byte order change
+uint64_t htonll(uint64_t in);   // host->network byte order change
+
+#endif                          /* FLEXPTP_FORMAT_UTILS_H_ */
--- a/hw_port/flexptp_options_simulation.h
+++ b/hw_port/flexptp_options_simulation.h
@ -0,0 +1,70 @@
+#ifndef FLEXPTP_OPTIONS_STM32H743_H_
+#define FLEXPTP_OPTIONS_STM32H743_H_
+
+// -------------------------------------------
+// --- DEFINES FOR PORTING IMPLEMENTATION ----
+// -------------------------------------------
+
+// Include LwIP headers here
+
+//#include "lwip/netif.h"
+//#include "lwip/tcpip.h"
+//#include "lwip/udp.h"
+//#include "lwip/igmp.h"
+
+#include <arpa/inet.h>
+
+#include "simsrc/FreeRTOS_simulation.h"
+#include "simsrc/lwip_simulation.h"
+
+// Give a printf-like printing implementation MSG(...)
+// Give a maskable printing implementation CLILOG(en,...)
+
+#include <env/utils.h>
+
+// Include hardware port files and fill the defines below to port the PTP stack to a physical hardware:
+// - PTP_HW_INIT(increment, addend): function initializing timestamping hardware
+// - PTP_MAIN_OSCILLATOR_FREQ_HZ: clock frequency fed into the timestamp unit [Hz]
+// - PTP_INCREMENT_NSEC: hardware clock increment [ns]
+// - PTP_UPDATE_CLOCK(s,ns): function jumping clock by defined value (negative time value means jumping backward)
+// - PTP_SET_ADDEND(addend): function writing hardware clock addend register
+
+#include "ptp_port_simulation.h"
+
+#define PTP_MAIN_OSCILLATOR_FREQ_HZ (200000000)
+#define PTP_INCREMENT_NSEC (5)
+
+#define PTP_HW_INIT(increment, addend) ptphw_init(increment, addend)
+#define PTP_UPDATE_CLOCK(s,ns) 	ptphw_update_clock(labs(s), abs(ns), (s * NANO_PREFIX + ns) < 0)
+#define PTP_SET_ADDEND(addend) ptphw_set_addend(addend)
+#define PTP_HW_GET_TIME(pt) ptphw_gettime(pt)
+
+// Include the clock servo (controller) and define the following:
+// - PTP_SERVO_INIT(): function initializing clock servo
+// - PTP_SERVO_DEINIT(): function deinitializing clock servo
+// - PTP_SERVO_RESET(): function reseting clock servo
+// - PTP_SERVO_RUN(d): function running the servo, input: master-slave time difference (error), return: clock tuning value in PPB
+//
+
+#include "../servo/pd_controller.h"
+
+#define PTP_SERVO_INIT() pd_ctrl_init()
+#define PTP_SERVO_DEINIT() pd_ctrl_deinit()
+#define PTP_SERVO_RESET() pd_ctrl_reset()
+#define PTP_SERVO_RUN(d,pscd) pd_ctrl_run(d,pscd)
+
+// Optionally add interactive, tokenizing CLI-support
+// - CLI_REG_CMD(cmd_hintline,n_cmd,n_min_arg,cb): function for registering CLI-commands
+//      cmd_hintline: text line printed in the help beginning with the actual command, separated from help text by \t charaters
+//      n_cmd: number of tokens (words) the command consists of
+//      n_arg: minimal number of arguments must be passed with the command
+//      cb: callback function cb(const CliToken_Type *ppArgs, uint8_t argc)
+//  return: cmd id (can be null, if discarded)
+
+#include <cli/cli.h>
+
+#define CLI_REG_CMD(cmd_hintline,n_cmd,n_min_arg,cb) cli_register_command(cmd_hintline, n_cmd, n_min_arg, cb)
+
+// -------------------------------------------
+
+#endif                          // FLEXPTP_OPTIONS_STM32H743_H_
--- a/hw_port/flexptp_options_stm32h743.h
+++ b/hw_port/flexptp_options_stm32h743.h
@ -0,0 +1,71 @@
+#ifndef FLEXPTP_OPTIONS_STM32H743_H_
+#define FLEXPTP_OPTIONS_STM32H743_H_
+
+// -------------------------------------------
+// --- DEFINES FOR PORTING IMPLEMENTATION ----
+// -------------------------------------------
+
+// Include LwIP headers here
+
+#include "lwip/netif.h"
+#include "lwip/tcpip.h"
+#include "lwip/udp.h"
+#include "lwip/igmp.h"
+#include "lwip/pbuf.h"
+
+// Give a printf-like printing implementation MSG(...)
+// Give a maskable printing implementation CLILOG(en,...)
+// Provide an SPRINTF-implementation SPRINTF(str,n,fmt,...)
+
+#include "utils.h"
+
+// Include hardware port files and fill the defines below to port the PTP stack to a physical hardware:
+// - PTP_HW_INIT(increment, addend): function initializing timestamping hardware
+// - PTP_MAIN_OSCILLATOR_FREQ_HZ: clock frequency fed into the timestamp unit [Hz]
+// - PTP_INCREMENT_NSEC: hardware clock increment [ns]
+// - PTP_UPDATE_CLOCK(s,ns): function jumping clock by defined value (negative time value means jumping backward)
+// - PTP_SET_ADDEND(addend): function writing hardware clock addend register
+
+#include "ptp_port_stm32h743.h"
+
+#include "stm32h7xx_hal.h"
+extern ETH_HandleTypeDef EthHandle;
+
+#define PTP_MAIN_OSCILLATOR_FREQ_HZ (200000000)
+#define PTP_INCREMENT_NSEC (5)
+
+#define PTP_HW_INIT(increment, addend) ptphw_init(increment, addend)
+#define PTP_UPDATE_CLOCK(s,ns) 	ETH_UpdatePTPTime(&EthHandle, labs(s), abs(ns), (s * NANO_PREFIX + ns) < 0)
+#define PTP_SET_CLOCK(s,ns) ETH_InitPTPTime(&EthHandle, labs(s), abs(ns))
+#define PTP_SET_ADDEND(addend) ETH_SetPTPAddend(&EthHandle, addend)
+#define PTP_HW_GET_TIME(pt) ptphw_gettime(pt)
+
+// Include the clock servo (controller) and define the following:
+// - PTP_SERVO_INIT(): function initializing clock servo
+// - PTP_SERVO_DEINIT(): function deinitializing clock servo
+// - PTP_SERVO_RESET(): function reseting clock servo
+// - PTP_SERVO_RUN(d): function running the servo, input: master-slave time difference (error), return: clock tuning value in PPB
+//
+
+#include "../servo/pd_controller.h"
+
+#define PTP_SERVO_INIT() pd_ctrl_init()
+#define PTP_SERVO_DEINIT() pd_ctrl_deinit()
+#define PTP_SERVO_RESET() pd_ctrl_reset()
+#define PTP_SERVO_RUN(d,pscd) pd_ctrl_run(d,pscd)
+
+// Optionally add interactive, tokenizing CLI-support
+// - CLI_REG_CMD(cmd_hintline,n_cmd,n_min_arg,cb): function for registering CLI-commands
+//      cmd_hintline: text line printed in the help beginning with the actual command, separated from help text by \t charaters
+//      n_cmd: number of tokens (words) the command consists of
+//      n_arg: minimal number of arguments must be passed with the command
+//      cb: callback function cb(const CliToken_Type *ppArgs, uint8_t argc)
+//  return: cmd id (can be null, if discarded)
+
+#include "cli.h"
+
+#define CLI_REG_CMD(cmd_hintline,n_cmd,n_min_arg,cb) cli_register_command(cmd_hintline, n_cmd, n_min_arg, cb)
+
+// -------------------------------------------
+
+#endif                          // FLEXPTP_OPTIONS_STM32H743_H_
--- a/hw_port/flexptp_options_tm4c1294.h
+++ b/hw_port/flexptp_options_tm4c1294.h
@ -0,0 +1,68 @@
+#ifndef FLEXPTP_OPTIONS_STM32H743_H_
+#define FLEXPTP_OPTIONS_STM32H743_H_
+
+// -------------------------------------------
+// --- DEFINES FOR PORTING IMPLEMENTATION ----
+// -------------------------------------------
+
+// Include LwIP headers here
+
+#include "utils/lwiplib.h"
+#include "utils/uartstdio.h"
+
+#include "driverlib/emac.h"
+#include "driverlib/gpio.h"
+#include "inc/hw_memmap.h"
+#include "driverlib/pin_map.h"
+
+// Give a printf-like printing implementation MSG(...)
+// Give a maskable printing implementation CLILOG(en,...)
+
+#include "utils.h"
+
+// Include hardware port files and fill the defines below to port the PTP stack to a physical hardware:
+// - PTP_HW_INIT(increment, addend): function initializing timestamping hardware
+// - PTP_MAIN_OSCILLATOR_FREQ_HZ: clock frequency fed into the timestamp unit [Hz]
+// - PTP_INCREMENT_NSEC: hardware clock increment [ns]
+// - PTP_UPDATE_CLOCK(s,ns): function jumping clock by defined value (negative time value means jumping backward)
+// - PTP_SET_ADDEND(addend): function writing hardware clock addend register
+
+#include "ptp_port_tiva_tm4c1295.h"
+
+#define PTP_MAIN_OSCILLATOR_FREQ_HZ (25000000)
+#define PTP_INCREMENT_NSEC (50)
+
+#define PTP_HW_INIT(increment, addend) ptphw_init(increment, addend)
+#define PTP_UPDATE_CLOCK(s,ns) EMACTimestampSysTimeUpdate(EMAC0_BASE, labs(s), abs(ns), (s * NANO_PREFIX + ns) < 0)
+#define PTP_SET_ADDEND(addend) EMACTimestampAddendSet(EMAC0_BASE, addend)
+#define PTP_HW_GET_TIME(pt) ptphw_gettime(pt)
+
+// Include the clock servo (controller) and define the following:
+// - PTP_SERVO_INIT(): function initializing clock servo
+// - PTP_SERVO_DEINIT(): function deinitializing clock servo
+// - PTP_SERVO_RESET(): function reseting clock servo
+// - PTP_SERVO_RUN(d): function running the servo, input: master-slave time difference (error), return: clock tuning value in PPB
+//
+
+#include "../servo/pd_controller.h"
+
+#define PTP_SERVO_INIT() pd_ctrl_init()
+#define PTP_SERVO_DEINIT() pd_ctrl_deinit()
+#define PTP_SERVO_RESET() pd_ctrl_reset()
+#define PTP_SERVO_RUN(d,pscd) pd_ctrl_run(d,pscd)
+
+// Optionally add interactive, tokenizing CLI-support
+// - CLI_REG_CMD(cmd_hintline,n_cmd,n_min_arg,cb): function for registering CLI-commands
+//      cmd_hintline: text line printed in the help beginning with the actual command, separated from help text by \t charaters
+//      n_cmd: number of tokens (words) the command consists of
+//      n_arg: minimal number of arguments must be passed with the command
+//      cb: callback function cb(const CliToken_Type *ppArgs, uint8_t argc)
+//  return: cmd id (can be null, if discarded)
+
+#include "cli.h"
+
+#define CLI_REG_CMD(cmd_hintline,n_cmd,n_min_arg,cb) cli_register_command(cmd_hintline, n_cmd, n_min_arg, cb)
+
+// -------------------------------------------
+
+#endif                          // FLEXPTP_OPTIONS_STM32H743_H_
--- a/hw_port/ptp_port_simulation.cpp
+++ b/hw_port/ptp_port_simulation.cpp
@ -0,0 +1,42 @@
+#include "ptp_port_simulation.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <memory>
+
+#include "simsrc/PtpClock.h"
+
+std::shared_ptr<PtpClock> pSlClock;
+
+extern "C" {
+
+void ptphw_init(uint32_t increment, uint32_t addend) {
+    // create clock
+    PtpClock::ClockProperties ckProps = {
+            0xFFFFFFFF,
+            0xFFFF5DCC,
+            0.0
+    };
+    pSlClock = std::make_shared<PtpClock>(ckProps);
+
+    // set default addend
+    pSlClock->setAddend(addend);
+}
+
+void ptphw_gettime(TimestampU *pTime) {
+    PtpClock::Timestamp ts = pSlClock->getTime();
+    pTime->sec = ts.sec;
+    pTime->nanosec = ts.ns;
+}
+
+void ptphw_set_addend(uint32_t addend) {
+    pSlClock->setAddend(addend);
+}
+
+void ptphw_update_clock(uint32_t s, uint32_t ns, int dir) {
+    pSlClock->updateTime(s, ns, dir);
+}
+
+}
+
--- a/hw_port/ptp_port_simulation.h
+++ b/hw_port/ptp_port_simulation.h
@ -0,0 +1,18 @@
+#ifndef HW_PORT_PTP_PORT_SIMULATION_
+#define HW_PORT_PTP_PORT_SIMULATION_
+
+#include "../timeutils.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    void ptphw_init(uint32_t increment, uint32_t addend);       // initialize PTP hardware
+    void ptphw_gettime(TimestampU * pTime);     // get time
+    void ptphw_set_addend(uint32_t addend);     // set addend
+    void ptphw_update_clock(uint32_t s, uint32_t ns, int dir);  // update clock
+
+#ifdef __cplusplus
+}
+#endif
+#endif                          /* HW_PORT_PTP_PORT_SIMULATION_ */
--- a/hw_port/ptp_port_stm32h743.c
+++ b/hw_port/ptp_port_stm32h743.c
@ -0,0 +1,146 @@
+#include "ptp_port_stm32h743.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "stm32h7xx_hal.h"
+
+#include "flexptp_options_stm32h743.h"
+
+#include "utils.h"
+#include "cli.h"
+
+extern ETH_HandleTypeDef EthHandle;
+
+static unsigned sFreq = 1;
+
+#ifdef CLI_REG_CMD
+
+static int CB_pps(const CliToken_Type * ppArgs, uint8_t argc)
+{
+    if (argc >= 1) {
+        float freq = atof(ppArgs[0]);
+
+        int fc_exp;
+
+        if (freq > 0) {
+            fc_exp = round(log2f(freq)) + 1;
+
+            // lower limit
+            if (fc_exp < ETH_PTP_PPS_1Hz) {
+                fc_exp = ETH_PTP_PPS_1Hz;
+            }
+            // upper limit
+            if (fc_exp > ETH_PTP_PPS_16384Hz) {
+                fc_exp = ETH_PTP_PPS_16384Hz;
+            }
+
+            sFreq = exp2(fc_exp - 1);
+        } else {
+            sFreq = 0;
+            fc_exp = 0;
+        }
+
+        ETH_SetPTPPPSFreq(&EthHandle, fc_exp);
+
+//          // parse frequency [Hz] (integer only!)
+//          uint32_t freq_Hz = atoi(ppArgs[0]);
+//
+//          if (freq_Hz == 0) {
+//              // stop pulse train generation
+//              ETH_StopPTPPPSPulseTrain(&EthHandle);
+//          } else {
+//            // compute period [ns]
+//            uint32_t period_ns = NANO_PREFIX / freq_Hz;
+//
+//            // display warning if frequency is not integer divisor of 1E+09
+//            if ((NANO_PREFIX % freq_Hz) != 0) {
+//                MSG("Warning! PPS frequency is not totally accurate, "
+//                    "choose frequency values corresponding to periods "
+//                    "being integer divisors of 1E+09!\n");
+//            }
+//
+//            // set duty cycle (try 50%) by specifying positive pulse length
+//            uint32_t high_ns = period_ns / 2;
+//
+//            // start pulse train generation
+//            ETH_StartPTPPPSPulseTrain(&EthHandle, high_ns, period_ns);
+//          }
+//
+//          // store frequency setting
+//          sFreq = freq_Hz;
+    }
+
+    if (sFreq > 0) {
+        MSG("PPS frequency: %u Hz\n", sFreq);
+    } else {
+        MSG("PPS output is turned off.\n");
+    }
+
+    return 0;
+}
+
+static void ptphw_register_cli_commands()
+{                               // TODO....
+    cli_register_command("ptp pps {freq} \t\t\tSet or query PPS signal frequency [Hz]", 2, 0, CB_pps);
+}
+
+#endif                          // CLI_REG_CMD
+
+void ptphw_init(uint32_t increment, uint32_t addend)
+{
+    // multicast fogadás engedélyezése
+    ETH_MACFilterConfigTypeDef filterConf;
+    HAL_ETH_GetMACFilterConfig(&EthHandle, &filterConf);
+    filterConf.PassAllMulticast = ENABLE;
+    HAL_ETH_SetMACFilterConfig(&EthHandle, &filterConf);
+
+    // timestamp engedélyezése
+    ETH_EnablePTPTimeStamping(&EthHandle);
+
+    vTaskDelay(pdMS_TO_TICKS(10));
+
+    //ETH_EnablePTPTimeStamping(&EthHandle);
+
+    // idő beállítása
+    ETH_InitPTPTime(&EthHandle, 0, 0);
+
+    // fine correction engedélyezése
+    ETH_EnablePTPFineCorr(&EthHandle, true);
+
+    // addend beállítása
+    ETH_SetPTPAddend(&EthHandle, addend);
+
+    // increment beállítása
+    ETH_SetPTPSubsecondIncrement(&EthHandle, increment);
+
+    //ETH_StartPTPPPSPulseTrain(&EthHandle, 500E+06, 1E+09);
+    ETH_SetPTPPPSFreq(&EthHandle, ETH_PTP_PPS_1Hz);
+    sFreq = 1;
+
+    __HAL_RCC_GPIOG_CLK_ENABLE();
+
+    // setup PPS-pin
+    GPIO_InitTypeDef GPIO_InitStructure;
+    GPIO_InitStructure.Pin = GPIO_PIN_8;
+    GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStructure.Alternate = GPIO_AF11_ETH;
+    GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
+    GPIO_InitStructure.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOG, &GPIO_InitStructure);
+
+#ifdef CLI_REG_CMD
+    // register cli commands
+    ptphw_register_cli_commands();
+#endif                          // CLI_REG_CMD
+}
+
+void ptphw_gettime(TimestampU * pTime)
+{
+    pTime->sec = EthHandle.Instance->MACSTSR;
+    pTime->nanosec = EthHandle.Instance->MACSTNR & ETH_MACSTNR_TSSS;
+}
--- a/hw_port/ptp_port_stm32h743.h
+++ b/hw_port/ptp_port_stm32h743.h
@ -0,0 +1,16 @@
+/*
+ * ptp_port_tiva_tm4c1294.c
+ *
+ *  Created on: 2021. szept. 17.
+ *      Author: epagris
+ */
+
+#ifndef HW_PORT_PTP_PORT_TIVA_TM4C1294_C_
+#define HW_PORT_PTP_PORT_TIVA_TM4C1294_C_
+
+#include "../timeutils.h"
+
+void ptphw_init(uint32_t increment, uint32_t addend);   // initialize PTP hardware
+void ptphw_gettime(TimestampU * pTime); // get time
+
+#endif                          /* HW_PORT_PTP_PORT_TIVA_TM4C1294_C_ */
--- a/hw_port/ptp_port_tiva_tm4c1294.c
+++ b/hw_port/ptp_port_tiva_tm4c1294.c
@ -0,0 +1,30 @@
+#include "ptp_port_tiva_tm4c1295.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "driverlib/emac.h"
+#include "driverlib/gpio.h"
+#include "inc/hw_memmap.h"
+#include "driverlib/pin_map.h"
+
+void ptphw_init(uint32_t increment, uint32_t addend)
+{
+    // init clock
+    EMACTimestampConfigSet(EMAC0_BASE, (EMAC_TS_ALL_RX_FRAMES | EMAC_TS_DIGITAL_ROLLOVER | EMAC_TS_PROCESS_IPV4_UDP | EMAC_TS_ALL | EMAC_TS_PTP_VERSION_2 | EMAC_TS_UPDATE_FINE),       // PTPv2 processing
+                           increment);
+    EMACTimestampAddendSet(EMAC0_BASE, addend);
+    EMACTimestampEnable(EMAC0_BASE);
+
+    // init PPS output
+    GPIOPinTypePWM(GPIO_PORTG_AHB_BASE, GPIO_PIN_0);
+    GPIOPinConfigure(GPIO_PG0_EN0PPS);
+    EMACTimestampPPSSimpleModeSet(EMAC0_BASE, EMAC_PPS_1HZ);
+}
+
+void ptphw_gettime(TimestampU * pTime)
+{
+    pTime->sec = 0;
+    pTime->nanosec = 0;
+    EMACTimestampSysTimeGet(EMAC0_BASE, &(pTime->sec), &(pTime->nanosec));
+}
--- a/hw_port/ptp_port_tiva_tm4c1295.h
+++ b/hw_port/ptp_port_tiva_tm4c1295.h
@ -0,0 +1,16 @@
+/*
+ * ptp_port_tiva_tm4c1294.c
+ *
+ *  Created on: 2021. szept. 17.
+ *      Author: epagris
+ */
+
+#ifndef HW_PORT_PTP_PORT_TIVA_TM4C1294_C_
+#define HW_PORT_PTP_PORT_TIVA_TM4C1294_C_
+
+#include "../timeutils.h"
+
+void ptphw_init(uint32_t increment, uint32_t addend);   // initialize PTP hardware
+void ptphw_gettime(TimestampU * pTime); // get time
+
+#endif                          /* HW_PORT_PTP_PORT_TIVA_TM4C1294_C_ */
--- a/hw_port/simsrc/FreeRTOS_simulation.cpp
+++ b/hw_port/simsrc/FreeRTOS_simulation.cpp
@ -0,0 +1,100 @@
+#include "FreeRTOS_simulation.h"
+
+#include <csignal>
+#include <map>
+#include <string>
+#include <iostream>
+
+struct TmrInfo {
+    std::string name;
+    uint32_t timeout;
+    bool singleShot;
+    void *timerID;
+    tmrcb cb;
+};
+
+static std::map<timer_t, TmrInfo> tmrLut;
+
+extern "C" {
+
+TimerHandle_t xTimerCreate(const char *name, TickType_t timeout, bool singleShot, void *timerID, tmrcb cb) {
+    // create timer
+    timer_t *pTmr = new timer_t;
+    sigevent sev;
+    //sev.sigev_notify = SIGEV_SIGNAL;
+    sev.sigev_notify = SIGEV_THREAD;
+    sev.sigev_signo = SIGRTMIN;
+    sev.sigev_value.sival_ptr = pTmr;
+    sev.sigev_notify_function = (void(*)(union sigval))cb;
+    sev.sigev_notify_attributes = nullptr;
+
+    if (timer_create(CLOCK_REALTIME, &sev, pTmr) != 0) {
+        delete pTmr;
+        return nullptr;
+    }
+
+    // put callback function into the lookup-table
+    TmrInfo info = {name, timeout, singleShot, timerID, cb};
+
+    tmrLut[*pTmr] = info;
+
+    return pTmr;
+}
+
+BaseType_t xTimerDelete(TimerHandle_t xTimer, TickType_t xBlockTime) {
+    // delete timer
+    timer_delete(*xTimer);
+
+    // remove callback from lut
+    tmrLut.erase(*xTimer);
+
+    // release timer ID resource
+    delete xTimer;
+
+    return 0;
+}
+
+BaseType_t xTimerStart(TimerHandle_t xTimer, TickType_t xBlockTime) {
+    const TmrInfo &info = tmrLut.at(*xTimer);
+
+    itimerspec its;
+    its.it_value.tv_sec = info.timeout / 1000;
+    its.it_value.tv_nsec = info.timeout % 1000;
+
+    if (!info.singleShot) {
+        its.it_interval = its.it_value;
+    } else {
+        its.it_interval.tv_sec = 0;
+        its.it_interval.tv_nsec = 0;
+    }
+
+    timer_settime(*xTimer, 0, &its, nullptr);
+
+    return 0;
+}
+
+BaseType_t xTimerStop(TimerHandle_t xTimer, TickType_t xBlockTime) {
+    itimerspec its;
+    its.it_interval.tv_sec = 0;
+    its.it_interval.tv_nsec = 0;
+    its.it_value = its.it_interval;
+
+    timer_settime(*xTimer, 0, &its, nullptr);
+
+    return 0;
+}
+
+static void tmr_handler(int sig, siginfo_t *si, void *uc) {
+    std::cout << "Timer!" << std::endl;
+    signal(sig, SIG_IGN);
+}
+
+//void FreeRTOS_simulation_init() {
+//    struct sigaction sa;
+//    sa.sa_flags = SA_SIGINFO;
+//    sa.sa_sigaction = tmr_handler;
+//    sigemptyset(&sa.sa_mask);
+//    sigaction(SIGRTMIN, &sa, nullptr);
+//}
+
+}
--- a/hw_port/simsrc/FreeRTOS_simulation.h
+++ b/hw_port/simsrc/FreeRTOS_simulation.h
@ -0,0 +1,32 @@
+#ifndef FLEXPTP_SIM_FREERTOS_SIMULATION_H
+#define FLEXPTP_SIM_FREERTOS_SIMULATION_H
+
+#include <time.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    typedef timer_t *TimerHandle_t;
+    typedef void (*tmrcb)(TimerHandle_t tmr);
+
+    typedef uint32_t TickType_t;
+    typedef uint32_t BaseType_t;
+#define pdMS_TO_TICKS(x) (x)
+
+    TimerHandle_t xTimerCreate(const char *name, TickType_t timeout, bool singleShot, void *timerID, tmrcb cb);
+    BaseType_t xTimerDelete(TimerHandle_t xTimer, TickType_t xBlockTime);
+    BaseType_t xTimerStart(TimerHandle_t xTimer, TickType_t xBlockTime);
+    BaseType_t xTimerStop(TimerHandle_t xTimer, TickType_t xBlockTime);
+
+//typedef Timer * TimerHandle_t;
+
+//void FreeRTOS_simulation_init();
+
+#ifdef __cplusplus
+}
+#endif
+#endif                          //FLEXPTP_SIM_FREERTOS_SIMULATION_H
--- a/hw_port/simsrc/PtpClock.cpp
+++ b/hw_port/simsrc/PtpClock.cpp
@ -0,0 +1,29 @@
+#include "PtpClock.h"
+
+PtpClock::PtpClock(const PtpClock::ClockProperties &ckProps) : mCkProps(ckProps) {}
+
+void PtpClock::setTime(uint32_t s, uint32_t ns) {
+    mTime = s * C_NANO_PREFIX + ns * C_NANO_PREFIX;
+}
+
+void PtpClock::updateTime(uint32_t s, uint32_t ns, bool dir) {
+    int64_t diff = s * C_NANO_PREFIX + ns;
+    diff *= dir ? +1 : -1;
+    mTime += diff;
+}
+
+PtpClock::Timestamp PtpClock::getTime() const {
+    return { mTime / C_NANO_PREFIX, (uint32_t)(mTime % C_NANO_PREFIX) };
+}
+
+void PtpClock::setAddend(uint64_t addend) {
+    mAddend = addend;
+}
+
+uint64_t PtpClock::getAddend() const {
+    return mAddend;
+}
+
+void PtpClock::advanceClock(uint32_t s, uint32_t ns) {
+    mTime += (s * C_NANO_PREFIX + ns) * (mAddend / mCkProps.ckNomAddend);
+}
--- a/hw_port/simsrc/PtpClock.h
+++ b/hw_port/simsrc/PtpClock.h
@ -0,0 +1,36 @@
+#ifndef FLEXPTP_SIM_PTPCLOCK_H
+#define FLEXPTP_SIM_PTPCLOCK_H
+
+#include <chrono>
+
+class PtpClock {
+ public:
+    using ClockProperties = struct {
+        uint64_t ckDivOF;       // clock division overflow
+        uint64_t ckNomAddend;   // nominal addend value
+        double ckVariance_nsns; // clock variance in ns square
+    };
+    using Timestamp = struct {
+        uint64_t sec;
+        uint32_t ns;
+    };
+ public:
+    static constexpr uint32_t C_NANO_PREFIX = 1000000000;
+ private:
+     uint64_t mTime {
+    };                          // slave clock time
+    uint64_t mAddend {
+    };                          // addend value
+
+    const ClockProperties mCkProps;     // slave clock overflow
+ public:
+    explicit PtpClock(const ClockProperties & ckProps); // slave clock overflow
+    void setTime(uint32_t s, uint32_t ns);      // set time
+    void updateTime(uint32_t s, uint32_t ns, bool dir); // update time by value
+    Timestamp getTime() const;  // get time
+    void setAddend(uint64_t addend);    // set addend
+    uint64_t getAddend() const; // get addend
+    void advanceClock(uint32_t s, uint32_t ns); // advance clock with given time difference
+};
+
+#endif                          //FLEXPTP_SIM_PTPCLOCK_H
--- a/hw_port/simsrc/PtpMasterClock.cpp
+++ b/hw_port/simsrc/PtpMasterClock.cpp
@ -0,0 +1,21 @@
+//
+// Created by epagris on 2022.10.22..
+//
+
+#include "PtpMasterClock.h"
+
+#include <memory>
+
+void PtpMasterClock::setup(const PtpMasterClock::PtpMasterClockSettings &settings) {
+    mSettings = settings;
+}
+
+void PtpMasterClock::start() {
+    mBeaconThread = std::make_shared<std::thread>(&PtpMasterClock::mBeaconThread, this);
+}
+
+void PtpMasterClock::mBeaconThread_CB() {
+    while (mRunning) {
+
+    }
+}
--- a/hw_port/simsrc/PtpMasterClock.h
+++ b/hw_port/simsrc/PtpMasterClock.h
@ -0,0 +1,35 @@
+#ifndef FLEXPTP_SIM_PTPMASTERCLOCK_H
+#define FLEXPTP_SIM_PTPMASTERCLOCK_H
+
+#include <memory>
+#include <list>
+#include <thread>
+#include "PtpSlaveClock.h"
+
+class PtpMasterClock {
+ public:
+    using PtpMasterClockSettings = struct {
+        uint16_t originCurrentUTCOffset;
+        uint8_t priority1;
+        uint8_t grandmasterClockClass;
+        uint8_t grandmasterClockAccuracy;
+        uint16_t grandmasterClockVariance;
+        uint8_t priority2;
+        uint64_t grandmasterClockIdentity;
+        uint16_t localStepsRemoved;
+        uint8_t timeSource;
+    };
+ private:
+     std::list < std::shared_ptr < PtpSlaveClock >> mSlvs;      // slave clocks connected to this master clock
+    PtpMasterClockSettings mSettings;   // master clock settings
+     std::shared_ptr < std::thread > mBeaconThread;     // thread handling multicasted message transmission
+    bool mRunning {
+    };
+    void mBeaconThread_CB();    // function running in the thread
+ public:
+     PtpMasterClock() = default;
+    void setup(const PtpMasterClockSettings & settings);        // setup master clock
+    void start();               // start master clock
+};
+
+#endif                          //FLEXPTP_SIM_PTPMASTERCLOCK_H
--- a/hw_port/simsrc/PtpSlaveClock.cpp
+++ b/hw_port/simsrc/PtpSlaveClock.cpp
@ -0,0 +1,5 @@
+//
+// Created by epagris on 2022.10.22..
+//
+
+#include "PtpSlaveClock.h"
--- a/hw_port/simsrc/PtpSlaveClock.h
+++ b/hw_port/simsrc/PtpSlaveClock.h
@ -0,0 +1,8 @@
+#ifndef FLEXPTP_SIM_PTPSLAVECLOCK_H
+#define FLEXPTP_SIM_PTPSLAVECLOCK_H
+
+class PtpSlaveClock {
+
+};
+
+#endif                          //FLEXPTP_SIM_PTPSLAVECLOCK_H
--- a/hw_port/simsrc/lwip_simulation.cpp
+++ b/hw_port/simsrc/lwip_simulation.cpp
@ -0,0 +1,19 @@
+#include "lwip_simulation.h"
+
+extern "C" {
+
+struct netif netif_default_obj = {{0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF}};
+struct netif *netif_default = &netif_default_obj;
+
+pbuf *pbuf_alloc(enum pbuf_layer layer, uint16_t length, enum pbuf_type type) {
+    pbuf *p = new pbuf;
+    p->payload = new uint8_t[length];
+    return p;
+}
+
+void pbuf_free(pbuf *p) {
+    delete[] (uint8_t *) p->payload;
+    delete p;
+}
+
+}
--- a/hw_port/simsrc/lwip_simulation.h
+++ b/hw_port/simsrc/lwip_simulation.h
@ -0,0 +1,48 @@
+#ifndef FLEXPTP_SIM_LWIP_SIMULATION_H
+#define FLEXPTP_SIM_LWIP_SIMULATION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <arpa/inet.h>
+
+    typedef struct ip4_addr {
+        uint32_t addr;
+    } ip4_addr_t;
+
+    typedef ip4_addr_t ip_addr_t;
+
+#define ipaddr_addr(x) inet_addr(x)
+
+    enum pbuf_layer {
+        PBUF_TRANSPORT, PBUF_IP, PBUF_LINK, PBUF_RAW_TX,
+        PBUF_RAW
+    };
+
+    enum pbuf_type {
+        PBUF_RAM, PBUF_ROM, PBUF_REF, PBUF_POOL
+    };
+
+/** Main packet buffer struct */
+    struct pbuf {
+        void *payload;
+        /*uint16_t tot_len;
+           uint16_t len; */
+        uint32_t time_s, time_ns;
+    };
+
+    struct pbuf *pbuf_alloc(enum pbuf_layer layer, uint16_t length, enum pbuf_type type);
+    void pbuf_free(struct pbuf *p);
+
+    struct netif {
+        uint8_t hwaddr[6];
+    };
+
+    extern struct netif *netif_default;
+
+#ifdef __cplusplus
+}
+#endif
+#endif                          //FLEXPTP_SIM_LWIP_SIMULATION_H
--- a/logging.c
+++ b/logging.c
@ -0,0 +1,60 @@
+#include "logging.h"
+
+#include <flexptp_options.h>
+
+#include "ptp_core.h"
+
+#define S (gPtpCoreState)
+
+// enable/disable general logging
+static void ptp_log_def_en(bool en)
+{
+    if (en) {                   // on turning on
+        MSG("\n\nT1 [s] | T1 [ns] | T4 [s] | T4 [ns] | Dt [s] | Dt [ns] | Dt [tick] | Addend\n\n");
+    }
+}
+
+// ------------------------
+
+// PTP log pair
+typedef struct {
+    int id;                     // ID of log type
+    void (*logEnFn)(bool);      // callback function on turning on/off logging
+    bool *en;                   // variable storing log state
+} PtpLogPair;
+
+static PtpLogPair sLogTable[PTP_LOG_N + 1] = {
+    {PTP_LOG_DEF, ptp_log_def_en, &(S.logging.def)},
+    {PTP_LOG_CORR_FIELD, NULL, &(S.logging.corr)},
+    {PTP_LOG_TIMESTAMPS, NULL, &(S.logging.timestamps)},
+    {PTP_LOG_INFO, NULL, &(S.logging.info)},
+    {PTP_LOG_LOCKED_STATE, NULL, &(S.logging.locked)},
+    {-1, NULL, NULL}
+};
+
+void ptp_enable_logging(int logId, bool en)
+{
+    PtpLogPair *pIter = sLogTable;
+    while (pIter->id != -1) {
+        if (pIter->id == logId && *(pIter->en) != en) { // if callback is found and changing state indeed
+            if (pIter->logEnFn != NULL) {       // callback function is not necessary
+                pIter->logEnFn(en);
+            }
+            *(pIter->en) = en;
+            break;
+        }
+        pIter++;
+    }
+}
+
+void ptp_disable_all_logging()
+{
+    PtpLogPair *pIter = sLogTable;
+    while (pIter->logEnFn != NULL) {
+        if (pIter->logEnFn != NULL) {
+            pIter->logEnFn(false);
+        }
+        *(pIter->en) = false;
+        pIter++;
+    }
+}
--- a/logging.h
+++ b/logging.h
@ -0,0 +1,18 @@
+#ifndef FLEXPTP_LOGGING_H_
+#define FLEXPTP_LOGGING_H_
+
+#include "ptp_core.h"
+
+enum {
+    PTP_LOG_DEF,
+    PTP_LOG_CORR_FIELD,
+    PTP_LOG_TIMESTAMPS,
+    PTP_LOG_INFO,
+    PTP_LOG_LOCKED_STATE,
+    PTP_LOG_N
+};
+
+void ptp_enable_logging(int logId, bool en);
+void ptp_disable_all_logging();
+
+#endif                          /* FLEXPTP_LOGGING_H_ */
--- a/msg_utils.c
+++ b/msg_utils.c
@ -0,0 +1,243 @@
+/* (C) András Wiesner, 2020-2022 */
+
+#include "msg_utils.h"
+
+#include <string.h>
+
+#include "ptp_defs.h"
+#include "format_utils.h"
+
+// load ptp flags from bitfield
+void ptp_load_flags(PTPFlags * pFlags, uint16_t bitfield)
+{
+#define GET_FLAG_FROM_BITFIELD(flag,n) (pFlags->flag) = (bitfield >> (n)) & 1
+
+    GET_FLAG_FROM_BITFIELD(PTP_SECURITY, 15);
+    GET_FLAG_FROM_BITFIELD(PTP_ProfileSpecific_2, 14);
+    GET_FLAG_FROM_BITFIELD(PTP_ProfileSpecific_1, 13);
+    GET_FLAG_FROM_BITFIELD(PTP_UNICAST, 10);
+    GET_FLAG_FROM_BITFIELD(PTP_TWO_STEP, 9);
+    GET_FLAG_FROM_BITFIELD(PTP_ALTERNATE_MASTER, 8);
+    GET_FLAG_FROM_BITFIELD(FREQUENCY_TRACEABLE, 7);
+    GET_FLAG_FROM_BITFIELD(TIME_TRACEABLE, 4);
+    GET_FLAG_FROM_BITFIELD(PTP_TIMESCALE, 3);
+    GET_FLAG_FROM_BITFIELD(PTP_UTC_REASONABLE, 2);
+    GET_FLAG_FROM_BITFIELD(PTP_LI_59, 1);
+    GET_FLAG_FROM_BITFIELD(PTP_LI_61, 0);
+
+/*	pFlags->PTP_SECURITY = (bitfield >> 15) & 1;
+	pFlags->PTP_ProfileSpecific_2 = (bitfield >> 14) & 1;
+	pFlags->PTP_ProfileSpecific_1 = (bitfield >> 13) & 1;
+
+	pFlags->PTP_UNICAST = (bitfield >> 10) & 1;
+	pFlags->PTP_TWO_STEP = (bitfield >> 9) & 1;
+	pFlags->PTP_ALTERNATE_MASTER = (bitfield >> 8) & 1;
+
+	pFlags->FREQUENCY_TRACEABLE = (bitfield >> 5) & 1;
+	pFlags->TIME_TRACEABLE = (bitfield >> 4) & 1;
+
+	pFlags->PTP_TIMESCALE = (bitfield >> 3) & 1;
+	pFlags->PTP_UTC_REASONABLE = (bitfield >> 2) & 1;
+	pFlags->PTP_LI_59 = (bitfield >> 1) & 1;
+	pFlags->PTP_LI_61 = (bitfield >> 0) & 1;*/
+}
+
+// write flags to bitfield
+uint16_t ptp_write_flags(PTPFlags * pFlags)
+{
+#define SET_BIT_IN_FLAG_BITFIELD(flag,n) bitfield |= (pFlags->flag) ? (1 << (n)) : 0
+
+    uint16_t bitfield = 0;
+    SET_BIT_IN_FLAG_BITFIELD(PTP_SECURITY, 15);
+    SET_BIT_IN_FLAG_BITFIELD(PTP_ProfileSpecific_2, 14);
+    SET_BIT_IN_FLAG_BITFIELD(PTP_ProfileSpecific_1, 13);
+    SET_BIT_IN_FLAG_BITFIELD(PTP_UNICAST, 10);
+    SET_BIT_IN_FLAG_BITFIELD(PTP_TWO_STEP, 9);
+    SET_BIT_IN_FLAG_BITFIELD(PTP_ALTERNATE_MASTER, 8);
+    SET_BIT_IN_FLAG_BITFIELD(FREQUENCY_TRACEABLE, 7);
+    SET_BIT_IN_FLAG_BITFIELD(TIME_TRACEABLE, 4);
+    SET_BIT_IN_FLAG_BITFIELD(PTP_TIMESCALE, 3);
+    SET_BIT_IN_FLAG_BITFIELD(PTP_UTC_REASONABLE, 2);
+    SET_BIT_IN_FLAG_BITFIELD(PTP_LI_59, 1);
+    SET_BIT_IN_FLAG_BITFIELD(PTP_LI_61, 0);
+
+    return bitfield;
+}
+
+// extract fields from a PTP header
+void ptp_extract_header(PtpHeader * pHeader, const void *pPayload)
+{
+    // cast header to byte accessible form
+    uint8_t *p = (uint8_t *) pPayload;
+
+    uint16_t flags;
+
+    // copy header fields
+    memcpy(&pHeader->messageType, p + 0, 1);
+    memcpy(&pHeader->versionPTP, p + 1, 1);
+    memcpy(&pHeader->messageLength, p + 2, 2);
+    memcpy(&pHeader->domainNumber, p + 4, 1);
+    memcpy(&flags, p + 6, 2);
+    memcpy(&pHeader->correction_ns, p + 8, 8);
+    memcpy(&pHeader->clockIdentity, p + 20, 8);
+    memcpy(&pHeader->sourcePortID, p + 28, 2);
+    memcpy(&pHeader->sequenceID, p + 30, 2);
+    memcpy(&pHeader->control, p + 32, 1);
+    memcpy(&pHeader->logMessagePeriod, p + 33, 1);
+
+    pHeader->transportSpecific = (0xf0 & pHeader->messageType) >> 4;
+    pHeader->messageType &= 0x0f;
+
+    // read flags
+    ptp_load_flags(&pHeader->flags, ntohs(flags));
+
+    // read correction field
+    pHeader->correction_subns = ntohll(pHeader->correction_ns) & 0xffff;
+    pHeader->correction_ns = ntohll(pHeader->correction_ns) >> 16;
+
+    pHeader->messageLength = ntohs(pHeader->messageLength);
+    pHeader->sourcePortID = ntohs(pHeader->sourcePortID);
+    pHeader->sequenceID = ntohs(pHeader->sequenceID);
+}
+
+// extract announce message
+void ptp_extract_announce_message(PtpAnnounceBody * pAnnounce, void *pPayload)
+{
+    // cast header to byte accessible form
+    uint8_t *p = (uint8_t *) pPayload + (PTP_HEADER_LENGTH + PTP_TIMESTAMP_LENGTH);
+
+    // copy header fields
+    memcpy(&pAnnounce->originCurrentUTCOffset, p + 0, 2);
+    memcpy(&pAnnounce->priority1, p + 3, 1);
+    memcpy(&pAnnounce->grandmasterClockClass, p + 4, 1);
+    memcpy(&pAnnounce->grandmasterClockAccuracy, p + 5, 1);
+    memcpy(&pAnnounce->grandmasterClockVariance, p + 6, 2);
+    memcpy(&pAnnounce->priority2, p + 8, 1);
+    memcpy(&pAnnounce->grandmasterClockIdentity, p + 9, 8);
+    memcpy(&pAnnounce->localStepsRemoved, p + 17, 2);
+    memcpy(&pAnnounce->timeSource, p + 19, 1);
+
+    pAnnounce->originCurrentUTCOffset = ntohs(pAnnounce->originCurrentUTCOffset);
+    pAnnounce->grandmasterClockVariance = ntohs(pAnnounce->grandmasterClockVariance);
+    pAnnounce->grandmasterClockIdentity = ntohll(pAnnounce->grandmasterClockIdentity);
+    pAnnounce->localStepsRemoved = ntohs(pAnnounce->localStepsRemoved);
+}
+
+// construct binary header from header structure
+void ptp_construct_binary_header(void *pData, const PtpHeader * pHeader)
+{
+    uint8_t *p = (uint8_t *) pData;
+    uint8_t firstByte;
+
+    // host->network
+    uint16_t messageLength = htons(pHeader->messageLength);
+    uint16_t sourcePortID = htons(pHeader->sourcePortID);
+    uint16_t sequenceID = htons(pHeader->sequenceID);
+
+    // fill in flags FIXME
+    uint16_t flags = htons(ptp_write_flags(&pHeader->flags));   // convert from header fields
+
+    // fill in correction value
+    uint64_t correction = htonll((pHeader->correction_ns << 16) | (pHeader->correction_subns)); // TODO: ...
+
+    // copy fields
+    firstByte = (pHeader->transportSpecific << 4) | (pHeader->messageType & 0x0f);
+    memcpy(p, &firstByte, 1);
+    memcpy(p + 1, &pHeader->versionPTP, 1);
+    memcpy(p + 2, &messageLength, 2);
+    memcpy(p + 4, &pHeader->domainNumber, 1);
+    memcpy(p + 6, &flags, 2);
+    memcpy(p + 8, &correction, 8);
+    memcpy(p + 20, &pHeader->clockIdentity, 8);
+    memcpy(p + 28, &sourcePortID, 2);
+    memcpy(p + 30, &sequenceID, 2);
+    memcpy(p + 32, &pHeader->control, 1);
+    memcpy(p + 33, &pHeader->logMessagePeriod, 1);
+}
+
+// write n timestamps following the header in to packet
+void ptp_write_binary_timestamps(void *pPayload, TimestampI * ts, uint8_t n)
+{
+    uint8_t *p = ((uint8_t *) pPayload) + PTP_HEADER_LENGTH;
+
+    // write n times
+    uint8_t i;
+    for (i = 0; i < n; i++) {
+        // get timestamp data
+        uint64_t sec = htonll(ts->sec << 16);
+        uint64_t nanosec = htonl(ts->nanosec);
+
+        // fill in time data
+        memcpy(p, &sec, 6);     // 48-bit
+        p += 6;
+
+        memcpy(p, &nanosec, 4);
+        p += 4;
+
+        // step onto next element
+        ts++;
+    }
+}
+
+// extract n timestamps from a message
+void ptp_extract_timestamps(TimestampI * ts, void *pPayload, uint8_t n)
+{
+    uint8_t *p = ((uint8_t *) pPayload) + PTP_HEADER_LENGTH;    // pointer at the beginning of first timestamp
+
+    // read n times
+    uint8_t i;
+    for (i = 0; i < n; i++) {
+        // seconds
+        ts->sec = 0;
+        memcpy(&ts->sec, p, 6); // 48-bit
+        p += 6;
+
+        // nanoseconds
+        memcpy(&ts->nanosec, p, 4);
+        p += 4;
+
+        // network->host
+        ts->sec = ntohll(ts->sec << 16);
+        ts->nanosec = ntohl(ts->nanosec);
+
+        // step to next timestamp
+        ts++;
+    }
+}
+
+// extract Delay_Resp ID data
+void ptp_read_delay_resp_id_data(Delay_RespIdentification * pDRData, void *pPayload)
+{
+    uint8_t *p = (uint8_t *) pPayload;
+    memcpy(&pDRData->requestingSourceClockIdentity, p + 44, 8);
+    memcpy(&pDRData->requestingSourcePortIdentity, p + 52, 2);
+
+    // network->host
+    pDRData->requestingSourcePortIdentity = ntohs(pDRData->requestingSourcePortIdentity);
+}
+
+// insert Delay_Resp ID data
+void ptp_write_delay_resp_id_data(void *pPayload, const Delay_RespIdentification * pDRData)
+{
+    uint8_t *p = (uint8_t *) pPayload;
+    uint16_t reqSrcPortId = htons(pDRData->requestingSourcePortIdentity);       // host->network
+    memcpy(p + 44, &pDRData->requestingSourceClockIdentity, 8);
+    memcpy(p + 52, &reqSrcPortId, 2);
+}
+
+// clear flag structure
+void ptp_clear_flags(PTPFlags * pFlags)
+{
+    memset(pFlags, 0, sizeof(PTPFlags));
+}
+
+// construct Sync message (TWO_STEP-mode only!)
+void ptp_construct_binary_sync(void *pData, const PtpHeader * pHeader)
+{
+    // insert header
+    ptp_construct_binary_header(pData, pHeader);
+
+    // insert empty timestamps
+    TimestampI zeroTs = { 0, 0 };
+    ptp_write_binary_timestamps(pData, &zeroTs, 1);
+}
--- a/msg_utils.h
+++ b/msg_utils.h
@ -0,0 +1,22 @@
+/* (C) András Wiesner, 2020-2022 */
+
+#ifndef FLEXPTP_MSG_UTILS_H_
+#define FLEXPTP_MSG_UTILS_H_
+
+#include <stdint.h>
+
+#include "ptp_types.h"
+#include "timeutils.h"
+
+void ptp_load_flags(PTPFlags * pFlags, uint16_t bitfield);      // load ptp flags from bitfield
+void ptp_extract_header(PtpHeader * pHeader, const void *pPayload);     // extract fields from a PTP header
+void ptp_extract_announce_message(PtpAnnounceBody * pAnnounce, void *pPayload); // extract announce message
+void ptp_construct_binary_header(void *pData, const PtpHeader * pHeader);       // construct binary header from header structure
+void ptp_write_binary_timestamps(void *pPayload, TimestampI * ts, uint8_t n);   // write n timestamps following the header in to packet
+void ptp_extract_timestamps(TimestampI * ts, void *pPayload, uint8_t n);        // extract n timestamps from a message
+void ptp_read_delay_resp_id_data(Delay_RespIdentification * pDRData, void *pPayload);   // extract Delay_Resp ID data
+void ptp_write_delay_resp_id_data(void *pPayload, const Delay_RespIdentification * pDRData);    // insert Delay_Resp ID data
+void ptp_clear_flags(PTPFlags * pFlags);        // clear flag structure
+void ptp_construct_binary_sync(void *pData, const PtpHeader * pHeader); // create Sync message
+
+#endif                          /* FLEXPTP_MSG_UTILS_H_ */
--- a/ptp_core.c
+++ b/ptp_core.c
@ -0,0 +1,610 @@
+/* (C) András Wiesner, 2020-2022 */
+
+#include <math.h>
+
+#include <flexptp_options.h>
+
+#include "ptp_core.h"
+
+#ifndef SIMULATION
+#include "FreeRTOS.h"
+#include "timers.h"
+#endif
+
+#include "ptp_defs.h"
+#include "ptp_types.h"
+#include "clock_utils.h"
+#include "format_utils.h"
+#include "msg_utils.h"
+#include "sbmc.h"
+#include "stats.h"
+#include "timeutils.h"
+#include "cli_cmds.h"
+#include "logging.h"
+
+#ifndef SIMULATION
+#include "ptp_msg_tx.h"
+#endif
+
+// --------------
+
+// --------------
+
+void ptp_init();
+
+// global state
+PtpCoreState gPtpCoreState;
+#define S (gPtpCoreState)
+
+static PtpHeader sDelayReqHeader;       // header for sending Delay_Reg messages
+
+static SyncCallback sSyncCallback = NULL;       // callback function on synchronization
+
+// --------------------------
+
+#define T1 (0)
+#define T2 (1)
+#define T3 (2)
+#define T4 (3)
+
+// --------------------------
+
+void ptp_reset();
+
+// --------------------------
+
+void ptp_set_sync_callback(SyncCallback syncCB)
+{
+    sSyncCallback = syncCB;
+}
+
+static void ptp_sbmc_tmr_tick(TimerHandle_t timer);
+static void ptp_delreq_tmr_tick(TimerHandle_t xTimer);
+
+//#define RESP_TIMEOUT (2000) // allowed maximal Delay_Resp response time
+#define SBMC_TICKRATE (1000)    // 1000ms period for SBMC-ticking
+
+void ptp_create_timers()
+{
+    // create smbc timer
+    S.timers.sbmc = xTimerCreate("sbmctimer", pdMS_TO_TICKS(SBMC_TICKRATE),     // timeout
+                                 true,  // timer operates in repeat mode
+                                 (void *)2,     // ID
+                                 ptp_sbmc_tmr_tick);    // callback-function
+
+    // create delreq timer
+    S.timers.delreq = xTimerCreate("delreq", pdMS_TO_TICKS(ptp_logi2ms(0)), true, (void *)3, ptp_delreq_tmr_tick);
+}
+
+void ptp_delete_timers()
+{
+    xTimerDelete(S.timers.sbmc, 0);
+    xTimerDelete(S.timers.delreq, 0);
+}
+
+// initialize Delay_Req header
+void ptp_init_delay_req_header()
+{
+    sDelayReqHeader.messageType = S.profile.delayMechanism == PTP_DM_E2E ? PTP_MT_Delay_Req : PTP_MT_PDelay_Req;
+    sDelayReqHeader.transportSpecific = (uint8_t) S.profile.transportSpecific;
+    sDelayReqHeader.versionPTP = 2;     // PTPv2
+    sDelayReqHeader.messageLength = PTP_HEADER_LENGTH + PTP_TIMESTAMP_LENGTH + (S.profile.delayMechanism == PTP_DM_P2P ? PTP_TIMESTAMP_LENGTH : 0);
+    sDelayReqHeader.domainNumber = S.profile.domainNumber;
+    ptp_clear_flags(&(sDelayReqHeader.flags));  // no flags
+    sDelayReqHeader.correction_ns = 0;
+    sDelayReqHeader.correction_subns = 0;
+
+    memcpy(&sDelayReqHeader.clockIdentity, &S.hwoptions.clockIdentity, 8);
+
+    sDelayReqHeader.sourcePortID = 1;   // TODO? No more ports...
+    sDelayReqHeader.sequenceID = 0;     // will change in every sync cycle
+    sDelayReqHeader.control = S.profile.delayMechanism == PTP_DM_E2E ? PTP_CON_Delay_Req : PTP_CON_Other;
+    sDelayReqHeader.logMessagePeriod = 0;
+}
+
+// --------------------------------------
+
+#define DEFAULT_SERVO_OFFSET (2800)
+
+// initialize PTP module
+void ptp_init()
+{
+    // create clock identity
+    ptp_create_clock_identity();
+
+    // seed the randomizer
+    srand(S.hwoptions.clockIdentity);
+
+    // reset options
+    nsToTsI(&S.hwoptions.offset, DEFAULT_SERVO_OFFSET);
+
+    // initialize hardware
+    PTP_HW_INIT(PTP_INCREMENT_NSEC, PTP_ADDEND_INIT);
+
+    // initialize controller
+    PTP_SERVO_INIT();
+
+    // create timers
+    ptp_create_timers();
+    xTimerStart(S.timers.sbmc, 0);      // TODO!!
+
+    // reset PTP subsystem
+    ptp_reset();
+
+#ifdef CLI_REG_CMD
+    // register cli commands
+    ptp_register_cli_commands();
+#endif                          // CLI_REG_CMD
+}
+
+// deinit PTP module
+void ptp_deinit()
+{
+#ifdef CLI_REG_CMD
+    // remove cli commands
+    ptp_remove_cli_commands();
+#endif                          // CLI_REG_CMD
+
+    // deinitialize controller
+    PTP_SERVO_DEINIT();
+
+    // delete timers
+    ptp_delete_timers();
+}
+
+// construct and send Delay_Req message (NON-REENTRANT!)
+void ptp_send_delay_req_message()
+{
+    static TimestampI zeroTs = { 0, 0 };        // timestamp appended at the end of packet
+
+    // PTP message
+    static RawPtpMessage delReqMsg = { 0 };
+    delReqMsg.size = sDelayReqHeader.messageLength;
+    delReqMsg.pTs = &(S.scd.t[T3]);
+    delReqMsg.tx_dm = S.profile.delayMechanism;
+    delReqMsg.tx_mc = PTP_MC_EVENT;
+
+    // increment sequenceID
+    sDelayReqHeader.sequenceID = ++S.messaging.delay_reqSequenceID;
+    sDelayReqHeader.domainNumber = S.profile.domainNumber;
+
+    // fill in header
+    ptp_construct_binary_header(delReqMsg.data, &sDelayReqHeader);
+
+    // fill in timestamp
+    ptp_write_binary_timestamps(delReqMsg.data, &zeroTs, 1);
+
+    // send message
+    ptp_transmit_enqueue(&delReqMsg);
+}
+
+// perform clock correction based on gathered timestamps (NON-REENTRANT!)
+void ptp_perform_correction()
+{
+    // don't do any processing if no delay_request data is present
+    if (!nonZeroI(&S.network.meanPathDelay)) {
+        return;
+    }
+
+    static TimestampI syncMa_prev = { 0 };
+
+    // timestamps and time intervals
+    TimestampI d, syncMa, syncSl, delReqSl, delReqMa;
+
+    // copy timestamps to assign them with meaningful names
+    syncMa = S.scd.t[T1];
+    syncSl = S.scd.t[T2];
+    delReqSl = S.scd.t[T3];
+    delReqMa = S.scd.t[T4];
+
+    // log timestamps (if enabled)
+
+    CLILOG(S.logging.timestamps,
+           "seqID: %u\n"
+           "T1: %d.%09d <- Sync TX (master)\n"
+           "T2: %d.%09d <- Sync RX (slave) \n"
+           "T3: %d.%09d <- Del_Req TX (slave) \n"
+           "T4: %d.%09d <- Del_Req RX (master)\n\n",
+           (uint32_t) S.messaging.sequenceID,
+           (int32_t) syncMa.sec, syncMa.nanosec, (int32_t) syncSl.sec, syncSl.nanosec, (int32_t) delReqSl.sec, delReqSl.nanosec, (int32_t) delReqMa.sec, delReqMa.nanosec);
+
+    // compute difference between master and slave clock
+    /*subTime(&d, &syncSl, &syncMa); // t2 - t1 ...
+       subTime(&d, &d, &delReqMa); // - t4 ...
+       addTime(&d, &d, &delReqSl); // + t3
+       divTime(&d, &d, 2); // division by 2 */
+
+    subTime(&d, &syncSl, &syncMa);      // t2 - t1 ...
+    subTime(&d, &d, &S.network.meanPathDelay);  // - MPD
+
+    // substract offset
+    subTime(&d, &d, &S.hwoptions.offset);
+
+    // normalize time difference (eliminate malformed time value issues)
+    normTime(&d);
+
+    // translate time difference into clock tick unit
+    int32_t d_ticks = tsToTick(&d, PTP_CLOCK_TICK_FREQ_HZ);
+
+    // if time difference is at least one second, then jump the clock
+    int64_t d_ns = nsI(&d);
+    if (llabs(d_ns) > 20000000) {
+        PTP_SET_CLOCK(syncMa.sec, syncMa.nanosec);
+
+        CLILOG(S.logging.info, "Time difference is over 20ms [%ldns], performing coarse correction!\n", d_ns);
+
+        syncMa_prev = syncMa;
+
+        return;
+    }
+
+    // prepare data to pass to the controller
+    double measSyncPeriod_ns;
+
+    TimestampI measSyncPeriod;
+    subTime(&measSyncPeriod, &syncMa, &syncMa_prev);
+    measSyncPeriod_ns = nsI(&measSyncPeriod);
+
+    PtpServoAuxInput saux = { S.scd, S.messaging.logSyncPeriod, S.messaging.syncPeriodMs, measSyncPeriod_ns };
+
+    // run controller
+    float corr_ppb = PTP_SERVO_RUN(nsI(&d), &saux);
+
+    // compute addend value
+    int64_t compAddend = (int64_t) S.hwclock.addend + (int64_t) (corr_ppb * PTP_ADDEND_CORR_PER_PPB_F); // compute addend value
+    S.hwclock.addend = MIN(compAddend, 0xFFFFFFFF);     // limit to 32-bit range
+
+    // write addend into hardware
+    PTP_SET_ADDEND(S.hwclock.addend);
+
+    // collect statistics
+    ptp_collect_stats(nsI(&d));
+
+    // log on cli (if enabled)
+    CLILOG(S.logging.def, "%d %09d %d %09d %d % 9d %d %u %f %ld %09lu\n",
+           (int32_t) syncMa.sec, syncMa.nanosec, (int32_t) delReqMa.sec, delReqMa.nanosec,
+           (int32_t) d.sec, d.nanosec, d_ticks, S.hwclock.addend, corr_ppb, nsI(&S.network.meanPathDelay), (uint64_t) measSyncPeriod_ns);
+
+    // call sync callback if defined
+    //if (sSyncCallback != NULL) {
+    //  sSyncCallback(nsI(&d), &scd, S.addend);
+    //}
+
+    syncMa_prev = syncMa;
+}
+
+static void ptp_delreq_tmr_tick(TimerHandle_t xTimer)
+{
+    // check that our last Delay_Req has been responded
+    CLILOG(S.messaging.delay_reqSequenceID != S.messaging.lastRespondedDelReqId, "(P)Del_Req #%d was unresponded!\n", S.messaging.delay_reqSequenceID);
+    ptp_send_delay_req_message();
+}
+
+static void ptp_start_delreq_timer()
+{
+    // start DelReq timer if not running yet
+    if (S.profile.logDelayReqPeriod != PTP_LOGPER_SYNCMATCHED) {
+        xTimerReset(S.timers.delreq, 0);
+        xTimerChangePeriod(S.timers.delreq, pdMS_TO_TICKS(ptp_logi2ms(S.profile.logDelayReqPeriod)), 0);
+        xTimerStart(S.timers.delreq, 0);
+    }
+}
+
+static void ptp_stop_delreq_timer()
+{
+    // stop DelReq timer (since there's no master to talk to)
+    xTimerStop(S.timers.delreq, portMAX_DELAY);
+}
+
+static void ptp_sbmc_tmr_tick(TimerHandle_t timer)
+{
+    PtpSBmcState *s = &(S.sbmc);
+
+    // main state machine dropout
+    if (s->mstState == SBMC_MASTER_OK) {
+        s->masterTOCntr += SBMC_TICKRATE;
+        if (s->masterTOCntr > 2 * s->masterAnnPer_ms + SBMC_TICKRATE) {
+            s->mstState = SBMC_NO_MASTER;
+            s->masterProps.priority1 = 255;
+            s->masterProps.grandmasterClockIdentity = 0;
+            CLILOG(S.logging.info, "Master lost!\n");
+
+            ptp_stop_delreq_timer();
+        }
+    }
+
+    // candidate switchover machine dropout
+    if (s->candState == SBMC_CANDIDATE_COLLECTION) {
+        s->candTOCntr += SBMC_TICKRATE;
+        if (s->candTOCntr > 2 * s->candAnnPer_ms + SBMC_TICKRATE) {
+            s->candState = SBMC_NO_CANDIDATE;
+        }
+    }
+}
+
+// handle announce messages
+void ptp_handle_announce_msg(PtpAnnounceBody * pAnn, PtpHeader * pHeader)
+{
+    PtpSBmcState *s = &(S.sbmc);
+    bool masterChanged = false;
+
+    switch (s->mstState) {
+    case SBMC_NO_MASTER:       // no master, accept the first one announcing itself
+        s->masterProps = *pAnn; // save master settings
+        s->mstState = SBMC_MASTER_OK;   // master found
+        s->candState = SBMC_NO_CANDIDATE;       // stop candidate processing, since master is selected
+        s->masterAnnPer_ms = ptp_logi2ms(pHeader->logMessagePeriod);
+        masterChanged = true;   // indicate that master has changed
+        // no break here!
+    case SBMC_MASTER_OK:       // already bound to master
+        if (pAnn->grandmasterClockIdentity == s->masterProps.grandmasterClockIdentity) {        // only clear when receiving from elected master
+            s->masterTOCntr = 0;        // clear counter
+        }
+    }
+
+    // run only candidate state evaluation if no main state changed took place
+    if (!masterChanged) {
+        switch (s->candState) {
+        case SBMC_NO_CANDIDATE:{
+                if (ptp_select_better_master(pAnn, &s->masterProps) == 0) {
+                    s->candProps = *pAnn;
+                    s->candAnnPer_ms = ptp_logi2ms(pHeader->logMessagePeriod);
+                    s->candState = SBMC_CANDIDATE_COLLECTION;   // switch to next syncState
+                    s->candCntr = 1;
+                }
+                break;
+            }
+
+        case SBMC_CANDIDATE_COLLECTION:{
+                // determine that the received master clock dataset is not better than the previously found one
+                if (ptp_select_better_master(pAnn, &s->candProps) == 0) {       // if better, reset counter and stay in current syncState
+                    s->candProps = *pAnn;
+                    s->candCntr = 1;
+                } else {
+                    // verify, that announce message comes from the same source
+                    if (pAnn->grandmasterClockIdentity == s->candProps.grandmasterClockIdentity) {
+                        s->candCntr++;  // advance counter
+                        s->candTOCntr = 0;      // clear counter
+                    }
+
+                    // if counter expired...
+                    if (s->candCntr == ANNOUNCE_COLLECTION_WINDOW) {
+                        s->masterProps = s->candProps;  // change master
+                        s->candProps.priority1 = 255;   // set to worst value, i.e. make other values meaningless
+                        s->candProps.grandmasterClockIdentity = 0;      // also clear ID
+
+                        masterChanged = true;
+                        s->candState = SBMC_NO_CANDIDATE;       // switch back to NO_CANDIDATE syncState
+                    }
+                }
+                break;
+            }
+        }
+    }
+
+    if (masterChanged) {
+        if (S.logging.info) {
+            MSG("Switched to new master: ");
+            ptp_print_clock_identity(s->masterProps.grandmasterClockIdentity);
+            MSG("\n");
+        }
+
+        ptp_start_delreq_timer();
+    }
+}
+
+void ptp_handle_correction_field(TimestampI * ts, const PtpHeader * pHeader)
+{
+    TimestampI correctionField;
+    correctionField.sec = 0;
+    correctionField.nanosec = pHeader->correction_ns;
+    subTime(ts, ts, &correctionField);
+    normTime(ts);
+}
+
+// TODO TODO TODO ...
+void ptp_compute_mean_path_delay(const TimestampI * pTs, TimestampI * pMPD)
+{
+    //static double a = 0.533488091091103; // fc = 10Hz
+    static double a = 0.00186744273170799;      // fc = 1Hz
+
+    double mpd_prev_ns = nsI(pMPD);
+
+    //MSG("%f\n", mpd_prev_ns);
+
+    // compute difference between master and slave clock
+    subTime(pMPD, &pTs[T2], &pTs[T1]);  // t2 - t1 ...
+    subTime(pMPD, pMPD, &pTs[T3]);      // - t3 ...
+    addTime(pMPD, pMPD, &pTs[T4]);      // + t4
+    divTime(pMPD, pMPD, 2);     // division by 2
+
+    // performing time error filtering
+    double mpd_new_ns = nsI(pMPD);
+    mpd_new_ns = a * mpd_prev_ns + (1 - a) * mpd_new_ns;        // filtering equation
+    nsToTsI(pMPD, (int64_t) mpd_new_ns);
+}
+
+// reset PTP subsystem
+void ptp_reset()
+{
+    // reset subsystem states...
+    memset(&S.messaging, 0, sizeof(PtpMessagingState)); // messaging state
+    S.hwclock.addend = PTP_ADDEND_INIT; // HW clock state
+    PTP_SET_ADDEND(PTP_ADDEND_INIT);
+    memset(&S.network, 0, sizeof(PtpNetworkState));     // network state
+    memset(&S.sbmc, 0, sizeof(PtpSBmcState));   // SBMC state
+    memset(&S.scd, 0, sizeof(PtpSyncCycleData));        // Sync cycle data
+
+    // remove delreq time
+    ptp_stop_delreq_timer();
+
+    // reset controller
+    PTP_SERVO_RESET();
+
+    // (re)init header for sending (P)Delay_Req messages
+    ptp_init_delay_req_header();
+
+    // reset statistics
+    ptp_clear_stats();
+}
+
+// packet processing (NON-REENTRANT!!)
+void ptp_process_packet(RawPtpMessage * pRawMsg)
+{
+    static PtpHeader header;    // PTP header
+    static Delay_RespIdentification delay_respID;       // identification received in every Delay_Resp packet
+    //TimestampI correctionField;
+
+    // header readout
+    ptp_extract_header(&header, pRawMsg->data);
+
+    // if other than Announce received
+    //MSG("%d\n", header.messageType);
+
+    // consider only messages in our domain
+    if (header.domainNumber != S.profile.domainNumber || header.transportSpecific != S.profile.transportSpecific) {
+        return;
+    }
+
+    if (header.messageType == PTP_MT_Announce) {
+        PtpMasterProperties newMstProp;
+        ptp_extract_announce_message(&newMstProp, pRawMsg->data);
+        ptp_handle_announce_msg(&newMstProp, &header);
+        return;
+    }
+
+    // process non-Announce messages
+    if (header.messageType == PTP_MT_Sync || header.messageType == PTP_MT_Follow_Up) {
+        switch (S.messaging.m2sState) {
+            // wait for Sync message
+        case SIdle:{
+                // switch into next state if Sync packet has arrived
+                if (header.messageType == PTP_MT_Sync) {
+                    // save sync interval
+                    S.messaging.logSyncPeriod = header.logMessagePeriod;
+                    S.messaging.syncPeriodMs = ptp_logi2ms(header.logMessagePeriod);
+
+                    //MSG("%d\n", header.logMessagePeriod);
+
+                    // save reception time
+                    S.scd.t[T2] = pRawMsg->ts;
+
+                    // switch to next syncState
+                    S.messaging.sequenceID = header.sequenceID;
+
+                    // handle two step/one step messaging
+                    //if (header.flags.PTP_TWO_STEP) {
+                    S.messaging.m2sState = SWaitFollowUp;
+                    //} else {
+                    /*ptp_extract_timestamps(&sSyncData.t1, pRawMsg->data, 1); // extract t1
+                       ptp_handle_correction_field(&sSyncData.t2, &header); // process correction field
+                       ptp_perform_correction(); // run clock correction */
+                    //}
+                }
+
+                break;
+            }
+            // wait for Follow_Up message
+        case SWaitFollowUp:
+            if (header.messageType == PTP_MT_Follow_Up) {
+                // check sequence ID if the response is ours
+                if (header.sequenceID == S.messaging.sequenceID) {
+                    ptp_extract_timestamps(&S.scd.t[T1], pRawMsg->data, 1);     // read t1
+                    ptp_handle_correction_field(&S.scd.t[T2], &header); // process correction field
+
+                    // log correction field (if enabled)
+                    CLILOG(S.logging.corr, "C [Follow_Up]: %09lu\n", header.correction_ns);
+
+                    // delay Delay_Req transmission with a random amount of time
+                    //vTaskDelay(pdMS_TO_TICKS(rand() % (S.syncIntervalMs / 2)));
+
+                    // send Delay_Req message
+                    if (S.profile.logDelayReqPeriod == PTP_LOGPER_SYNCMATCHED) {
+                        ptp_send_delay_req_message();
+                    }
+
+                    // jump clock if error is way too big...
+                    TimestampI d;
+                    subTime(&d, &S.scd.t[T2], &S.scd.t[T1]);
+                    if (d.sec != 0) {
+                        PTP_SET_CLOCK(S.scd.t[T1].sec, S.scd.t[T1].nanosec);
+                    }
+
+                    // run servo only if not syncmatched
+                    if (S.profile.logDelayReqPeriod != PTP_LOGPER_SYNCMATCHED) {
+                        ptp_perform_correction();
+                    }
+
+                    // switch to next syncState
+                    S.messaging.m2sState = SIdle;
+                }
+            }
+
+            break;
+
+        }
+
+    }
+
+    // ------ (P)DELAY_RESPONSE PROCESSING --------
+
+    // wait for (P)Delay_Resp message
+    if ((header.messageType == PTP_MT_Delay_Resp && S.profile.delayMechanism == PTP_DM_E2E)) {
+        if (header.sequenceID == S.messaging.delay_reqSequenceID) {     // read clock ID of requester
+
+            ptp_read_delay_resp_id_data(&delay_respID, pRawMsg->data);
+
+            // if the response was sent to us as a response to our Delay_Req then continue processing
+            if (delay_respID.requestingSourceClockIdentity == S.hwoptions.clockIdentity && delay_respID.requestingSourcePortIdentity == sDelayReqHeader.sourcePortID) {
+
+                ptp_extract_timestamps(&S.scd.t[T4], pRawMsg->data, 1); // store t4
+                ptp_handle_correction_field(&S.scd.t[T4], &header);     // substract correction field from t4
+
+                // compute mean path delay
+                ptp_compute_mean_path_delay(S.scd.t, &S.network.meanPathDelay);
+
+                // store last response ID
+                S.messaging.lastRespondedDelReqId = header.sequenceID;
+
+                // perform correction if operating on syncmatched mode
+                if (S.profile.logDelayReqPeriod == PTP_LOGPER_SYNCMATCHED) {
+                    ptp_perform_correction();
+                }
+
+                // log correction field (if enabled)
+                CLILOG(S.logging.corr, "C [Del_Resp]: %09lu\n", header.correction_ns);
+            }
+
+        }
+    }
+
+}
+
+void ptp_store_config(PtpConfig * pConfig)
+{
+    pConfig->profile = S.profile;
+    pConfig->offset = S.hwoptions.offset;
+    pConfig->logging = (int)(S.logging.def) | (int)(S.logging.info) << 1 | (int)(S.logging.corr) << 2 | (int)(S.logging.timestamps) << 3 | (int)(S.logging.locked) << 4;
+}
+
+void ptp_load_config(const PtpConfig * pConfig)
+{
+    S.profile = pConfig->profile;
+    S.hwoptions.offset = pConfig->offset;
+
+    S.logging.def = pConfig->logging & 1;
+    S.logging.info = (pConfig->logging >> 1) & 1;
+    S.logging.corr = (pConfig->logging >> 2) & 1;
+    S.logging.timestamps = (pConfig->logging >> 3) & 1;
+    S.logging.locked = (pConfig->logging >> 4) & 1;
+}
+
+void ptp_load_config_from_dump(const void *pDump)
+{
+    PtpConfig config;
+    memcpy(&config, pDump, sizeof(PtpConfig));
+    ptp_load_config(&config);
+    ptp_reset();
+}
+
+// -----------------------------------------------
--- a/ptp_core.h
+++ b/ptp_core.h
@ -0,0 +1,59 @@
+/* (C) András Wiesner, 2020 */
+
+#ifndef PTP_H
+#define PTP_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <string.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdio.h>
+
+#ifndef SIMULATION
+#include "FreeRTOS.h"
+#include "task.h"
+#include "timers.h"
+#endif
+
+#include "timeutils.h"
+#include "ptp_types.h"
+#include "stats.h"
+#include "settings_interface.h"
+
+// -------------------------------------------
+// ----- FLEXPTP_OPTIONS.H INCLUDE AREA ------
+// -------------------------------------------
+
+#include <flexptp_options.h>
+
+// -------------------------------------------
+// (End of customizable area)
+// -------------------------------------------
+
+// -------------------------------------------
+
+    void ptp_init();            // initialize PTP subsystem
+    void ptp_deinit();          // deinitialize PTP subsystem
+
+    void ptp_reset();           // reset PTP subsystem
+    void ptp_process_packet(RawPtpMessage * pRawMsg);   // process PTP packet
+
+    typedef void (*SyncCallback)(int64_t time_error, const PtpSyncCycleData * pSCD, uint32_t freqCodeWord);
+
+    void ptp_set_sync_callback(SyncCallback syncCB);
+
+#define PTP_IS_LOCKED(th) (ptp_get_stats()->filtTimeErr < (th) && !(ptp_get_current_master_clock_identity() != 0))      // is ptp locked considering threshold passed?
+
+    extern PtpCoreState gPtpCoreState;
+
+    void ptp_store_config(PtpConfig * pConfig); // store PTP-engine configuration (param: output)
+    void ptp_load_config(const PtpConfig * pConfig);    // load PTP-engine configuration
+    void ptp_load_config_from_dump(const void *pDump);  // load PTP-engine configuration from binary dump (i.e. from unaligned address)
+
+#ifdef __cplusplus
+}
+#endif
+#endif                          /* PTP */
--- a/ptp_defs.c
+++ b/ptp_defs.c
@ -0,0 +1,3 @@
+#include "ptp_defs.h"
+
+const ip_addr_t PTP_IGMP_PRIMARY = { 0x810100E0 };      // 224.0.1.129
--- a/ptp_defs.h
+++ b/ptp_defs.h
@ -0,0 +1,41 @@
+#ifndef FLEXPTP_PTP_DEFS_H_
+#define FLEXPTP_PTP_DEFS_H_
+
+#include "flexptp_options.h"
+
+#include <stdint.h>
+
+// IP address of PTP-IGMP groups
+#define PTP_IGMP_DEFAULT_STR ("224.0.1.129")
+
+extern const ip_addr_t PTP_IGMP_PRIMARY;
+
+// Ethernet address of PTP messages
+extern const uint8_t PTP_ETHERNET_PRIMARY[6];
+
+// PTP UDP ports
+#define PTP_PORT_EVENT (319)
+#define PTP_PORT_GENERAL (320)
+
+#define PTP_HEADER_LENGTH (34)
+#define PTP_TIMESTAMP_LENGTH (10)
+#define PTP_PORT_ID_LENGTH (10)
+
+#define PTP_PCKT_SIZE_SYNC (PTP_HEADER_LENGTH + PTP_TIMESTAMP_LENGTH)
+#define PTP_PCKT_SIZE_FOLLOW_UP (PTP_HEADER_LENGTH + PTP_TIMESTAMP_LENGTH)
+#define PTP_PCKT_SIZE_DELAY_REQ (PTP_HEADER_LENGTH + PTP_TIMESTAMP_LENGTH)
+#define PTP_PCKT_SIZE_DELAY_RESP (PTP_HEADER_LENGTH + PTP_TIMESTAMP_LENGTH + PTP_PORT_ID_LENGTH)
+
+// ---- AUTODEFINES ----------
+
+#ifndef PTP_ACCURACY_LIMIT_NS
+#define PTP_ACCURACY_LIMIT_NS (100)
+#endif
+
+// ---- CALCULATED VALUES ----
+
+#define PTP_CLOCK_TICK_FREQ_HZ (1000000000 / PTP_INCREMENT_NSEC)        // clock tick frequency
+#define PTP_ADDEND_INIT ((uint32_t)(0x100000000 / (PTP_MAIN_OSCILLATOR_FREQ_HZ / (float)PTP_CLOCK_TICK_FREQ_HZ)))       // addend value
+#define PTP_ADDEND_CORR_PER_PPB_F ((float)0x100000000 / ((float)PTP_INCREMENT_NSEC * PTP_MAIN_OSCILLATOR_FREQ_HZ))      // addend/ppb
+
+#endif                          /* FLEXPTP_PTP_DEFS_H_ */
--- a/ptp_msg_tx.c
+++ b/ptp_msg_tx.c
@ -0,0 +1,94 @@
+#include <flexptp_options.h>
+
+#include "FreeRTOS.h"
+#include "queue.h"
+
+#include "ptp_msg_tx.h"
+#include "ptp_defs.h"
+#include "ptp_core.h"
+#include "ptp_raw_msg_circbuf.h"
+
+#include "settings_interface.h"
+
+static struct {
+    struct udp_pcb *pPri_Ev;
+    struct udp_pcb *pPri_Gen;
+
+} sPcbLut = { 0 };
+
+static const uint16_t sPortLut[2] = { PTP_PORT_EVENT, PTP_PORT_GENERAL };
+static ip4_addr_t sIpLut[2] = { 0 };
+static const uint8_t *sEthLut[2] = { PTP_ETHERNET_PRIMARY, PTP_ETHERNET_PEER_DELAY };
+
+void ptp_transmit_init(struct udp_pcb *pPriE, struct udp_pcb *pPriG,)
+{
+    sPcbLut.pPri_Ev = pPriE;
+    sPcbLut.pPri_Gen = pPriG;
+
+    sIpLut[0] = PTP_IGMP_PRIMARY;
+
+}
+
+// release buffer
+void ptp_transmit_free(struct pbuf *pPBuf)
+{
+    pbuf_free(pPBuf);
+}
+
+void ptp_transmit_cb_handler(struct pbuf *pPBuf)
+{
+    RawPtpMessage *pMsg = (RawPtpMessage *) pPBuf->tag;
+    pMsg->ts.sec = pPBuf->time_s;
+    pMsg->ts.nanosec = pPBuf->time_ns;
+    if (pMsg->pTxCb) {
+        pMsg->pTxCb(pMsg);
+    }
+}
+
+bool ptp_transmit_enqueue(const RawPtpMessage * pMsg)
+{
+    extern PtpCircBuf gRawTxMsgBuf;
+    extern QueueHandle_t gTxPacketFIFO;
+    RawPtpMessage *pMsgAlloc = ptp_circ_buf_alloc(&gRawTxMsgBuf);
+    if (pMsgAlloc) {
+        *pMsgAlloc = *pMsg;
+        uint8_t idx = ptp_circ_buf_commit(&gRawTxMsgBuf);
+        bool hptWoken = false;
+        if (xPortIsInsideInterrupt()) {
+            xQueueSendFromISR(gTxPacketFIFO, &idx, &hptWoken);
+        } else {
+            xQueueSend(gTxPacketFIFO, &idx, portMAX_DELAY);
+        }
+        return true;
+    } else {
+        MSG("enqueue failed!");
+        return false;
+    }
+}
+
+void ptp_transmit_msg(RawPtpMessage * pMsg)
+{
+    PtpTransportType tp = ptp_get_transport_type();
+    PtpDelayMechanism dm = pMsg->tx_dm;
+    PtpMessageClass mc = pMsg->tx_mc;
+
+    // allocate buffer
+    struct pbuf *txbuf = NULL;
+    txbuf = pbuf_alloc(PBUF_TRANSPORT, pMsg->size, PBUF_RAM);
+
+    // fill buffer
+    memcpy(txbuf->payload, pMsg->data, pMsg->size);
+    txbuf->ts_writeback_addr[0] = (uint32_t *) & (pMsg->pTs->sec);
+    txbuf->ts_writeback_addr[1] = (uint32_t *) & (pMsg->pTs->nanosec);
+    txbuf->tag = pMsg;
+    txbuf->tx_cb = ptp_transmit_cb_handler;
+
+    if (tp == PTP_TP_IPv4) {
+        struct udp_pcb *pPcb = ((struct udp_pcb **)&sPcbLut)[2 * ((int)dm) + (int)mc];
+        uint16_t port = sPortLut[(int)mc];
+        ip_addr_t ipaddr = sIpLut[(int)dm];
+        udp_sendto(pPcb, txbuf, &ipaddr, port);
+    }
+
+    pbuf_free(txbuf);           // release buffer
+}
--- a/ptp_msg_tx.h
+++ b/ptp_msg_tx.h
@ -0,0 +1,10 @@
+#ifndef FLEXPTP_SIM_PTP_MSG_TX_H
+#define FLEXPTP_SIM_PTP_MSG_TX_H
+
+#include "ptp_types.h"
+
+void ptp_transmit_init(struct udp_pcb *pPriE, struct udp_pcb *pPriG,);  // initialize PTP transmitter
+void ptp_transmit_msg(RawPtpMessage * pMsg);    // transmit PTP message
+bool ptp_transmit_enqueue(const RawPtpMessage * pMsg);  // enqueue message TODO: refactor...
+
+#endif                          //FLEXPTP_SIM_PTP_MSG_TX_H
--- a/ptp_profile_presets.c
+++ b/ptp_profile_presets.c
@ -0,0 +1,53 @@
+#include "ptp_profile_presets.h"
+
+#define MAX_PROFILE_NAME_LENGTH (7)
+
+enum {
+    PTP_PROFILE_DEFAULT,        // default profile
+
+    PTP_PROFILE_N
+};
+
+typedef struct {
+    char name[MAX_PROFILE_NAME_LENGTH + 1];
+    PtpProfile profile;         // profile object
+} PtpProfilePreset;
+
+static PtpProfilePreset sPtpProfiles[PTP_PROFILE_N] = {
+    {
+     "default",
+     {
+      PTP_TP_IPv4,
+      PTP_TSPEC_UNKNOWN_DEF,
+      PTP_DM_E2E,
+      0,
+      0}
+     },
+};
+
+const PtpProfile *ptp_profile_preset_get(const char *pName)
+{
+    size_t i = 0;
+    PtpProfile *pProfile = NULL;
+    for (i = 0; i < PTP_PROFILE_N; i++) {
+        if (!strcmp(sPtpProfiles[i].name, pName)) {
+            pProfile = &sPtpProfiles[i].profile;
+            break;
+        }
+    }
+    return pProfile;
+}
+
+size_t ptp_profile_preset_cnt()
+{
+    return PTP_PROFILE_N;
+}
+
+const char *ptp_profile_preset_get_name(size_t i)
+{
+    if (i < PTP_PROFILE_N) {
+        return sPtpProfiles[i].name;
+    } else {
+        return NULL;
+    }
+}
--- a/ptp_profile_presets.h
+++ b/ptp_profile_presets.h
@ -0,0 +1,10 @@
+#ifndef PTP_PROFILE_PRESETS_H_
+#define PTP_PROFILE_PRESETS_H_
+
+#include "ptp_types.h"
+
+const PtpProfile *ptp_profile_preset_get(const char *pName);    // get profile by name
+size_t ptp_profile_preset_cnt();        // get number of preset profiles
+const char *ptp_profile_preset_get_name(size_t i);      // get profile name by index
+
+#endif                          /* PTP_PROFILE_PRESETS_H_ */
--- a/ptp_raw_msg_circbuf.c
+++ b/ptp_raw_msg_circbuf.c
@ -0,0 +1,46 @@
+#include "ptp_raw_msg_circbuf.h"
+
+void ptp_circ_buf_init(PtpCircBuf * pCircBuf, RawPtpMessage * pMsgPool, uint8_t n)
+{
+    pCircBuf->msgs = pMsgPool;
+    pCircBuf->totalSize = n;
+    pCircBuf->freeBufs = n;
+    pCircBuf->lastReceived = 0;
+    pCircBuf->allocPending = -1;
+}
+
+// allocate packet (CALL ONLY IF THERE IS SPACE AVAILABLE!)
+RawPtpMessage *ptp_circ_buf_alloc(PtpCircBuf * pCircBuf)
+{
+    if (pCircBuf->freeBufs > 0 && pCircBuf->allocPending == -1) {
+        uint8_t current = (pCircBuf->lastReceived + 1) % pCircBuf->totalSize;   // allocate a new packet
+        pCircBuf->allocPending = current;
+        return &(pCircBuf->msgs[current]);
+    } else {
+        return NULL;
+    }
+}
+
+int ptp_circ_buf_commit(PtpCircBuf * pCircBuf)
+{
+    if (pCircBuf->allocPending != -1) {
+        pCircBuf->lastReceived = pCircBuf->allocPending;        // advance last index
+        pCircBuf->freeBufs--;   // decrease amount of free buffers
+        pCircBuf->allocPending = -1;    // turn off allocation pending flag
+        return pCircBuf->lastReceived;
+    } else {
+        return -1;
+    }
+}
+
+void ptp_circ_buf_free(PtpCircBuf * pCircBuf)
+{
+    if (pCircBuf->freeBufs < pCircBuf->totalSize) {
+        pCircBuf->freeBufs++;
+    }
+}
+
+RawPtpMessage *ptp_circ_buf_get(PtpCircBuf * pCircBuf, uint8_t idx)
+{
+    return &(pCircBuf->msgs[idx]);
+}
--- a/ptp_raw_msg_circbuf.h
+++ b/ptp_raw_msg_circbuf.h
@ -0,0 +1,24 @@
+#ifndef FLEXPTP_PTP_RAW_MSG_CIRCBUF_H_
+#define FLEXPTP_PTP_RAW_MSG_CIRCBUF_H_
+
+#include <stdbool.h>
+
+#include "ptp_types.h"
+
+// "ring" buffer for PTP-messages
+#define PTP_MSG_BUF_SIZE (32)
+typedef struct {
+    RawPtpMessage *msgs;        // messages
+    uint8_t totalSize;          // total buffer size (element count)
+    uint8_t lastReceived;       // pointer to last received and last processed messages
+    uint8_t freeBufs;           // number of free buffers
+    int allocPending;           // allocation pending (by index)
+} PtpCircBuf;
+
+void ptp_circ_buf_init(PtpCircBuf * pCircBuf, RawPtpMessage * pMsgPool, uint8_t n);     // initialize circular buffer
+RawPtpMessage *ptp_circ_buf_alloc(PtpCircBuf * pCircBuf);       // allocate next available circular buffer
+int ptp_circ_buf_commit(PtpCircBuf * pCircBuf); // commit last allocation
+void ptp_circ_buf_free(PtpCircBuf * pCircBuf);  // free oldest allocation
+RawPtpMessage *ptp_circ_buf_get(PtpCircBuf * pCircBuf, uint8_t idx);    // get message by index
+
+#endif                          /* FLEXPTP_PTP_RAW_MSG_CIRCBUF_H_ */
--- a/ptp_servo_types.h
+++ b/ptp_servo_types.h
@ -0,0 +1,16 @@
+#ifndef FLEXPTP_SIM_PTP_SERVO_TYPES_H
+#define FLEXPTP_SIM_PTP_SERVO_TYPES_H
+
+#include "ptp_sync_cycle_data.h"
+
+// Data to perform a full synchronization
+typedef struct {
+    PtpSyncCycleData scd;       // sync cycle data
+
+    // information about sync interval
+    int8_t logMsgPeriod;        // ...
+    double msgPeriodMs;         // message period in ms
+    double measSyncPeriod;      // measured synchronization period (t1->t1) TODO rename! (suffix: _ns)
+} PtpServoAuxInput;
+
+#endif                          //FLEXPTP_SIM_PTP_SERVO_TYPES_H
--- a/ptp_sync_cycle_data.h
+++ b/ptp_sync_cycle_data.h
@ -0,0 +1,11 @@
+#ifndef FLEXPTP_PTP_SYNC_CYCLE_DATA_H_
+#define FLEXPTP_PTP_SYNC_CYCLE_DATA_H_
+
+#include "timeutils.h"
+
+typedef struct {
+
+    TimestampI t[6];
+} PtpSyncCycleData;
+
+#endif                          /* FLEXPTP_PTP_SYNC_CYCLE_DATA_H_ */
--- a/ptp_types.h
+++ b/ptp_types.h
@ -0,0 +1,268 @@
+#ifndef FLEXPTP_PTP_TYPES_H_
+#define FLEXPTP_PTP_TYPES_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "ptp_sync_cycle_data.h"
+
+#include <flexptp_options.h>
+
+#ifndef SIMULATION
+#include "FreeRTOS.h"
+#include "timers.h"
+#endif
+
+// PTP packet types
+enum PTPMessageType {
+    PTP_MT_Sync = 0,
+    PTP_MT_Delay_Req = 1,
+
+    PTP_MT_Follow_Up = 8,
+    PTP_MT_Delay_Resp = 9,
+
+    PTP_MT_Announce = 11
+};
+
+// PTP header control field values
+enum PTPControl {
+    PTP_CON_Sync = 0,
+    PTP_CON_Delay_Req = 1,
+    PTP_CON_Follow_Up = 2,
+    PTP_CON_Delay_Resp = 3,
+    PTP_CON_Other = 5
+};
+
+// PTP flags structure
+typedef struct {
+    bool PTP_SECURITY;
+    bool PTP_ProfileSpecific_2;
+    bool PTP_ProfileSpecific_1;
+    bool PTP_UNICAST;
+    bool PTP_TWO_STEP;
+    bool PTP_ALTERNATE_MASTER;
+    bool FREQUENCY_TRACEABLE;
+    bool TIME_TRACEABLE;
+    bool PTP_TIMESCALE;
+    bool PTP_UTC_REASONABLE;
+    bool PTP_LI_59;
+    bool PTP_LI_61;
+} PTPFlags;
+
+// PTP message header structure
+typedef struct {
+    // 0.
+    uint8_t messageType;        // ID
+    uint8_t transportSpecific;
+
+    // 1.
+    uint8_t versionPTP;         // PTP version
+    uint8_t _r1;
+
+    // 2-3.
+    uint16_t messageLength;     // length
+
+    // 4.
+    uint8_t domainNumber;
+
+    // 5.
+    uint8_t _r2;
+
+    // 6-7.
+    PTPFlags flags;
+
+    // 8-15.
+    uint64_t correction_ns;
+    uint32_t correction_subns;
+
+    // 16-19.
+    uint32_t _r3;
+
+    // 20-27.
+    uint64_t clockIdentity;
+
+    // 28-29
+    uint16_t sourcePortID;
+
+    // 30-31.
+    uint16_t sequenceID;
+
+    // 32.
+    uint8_t control;
+
+    // 33.
+    int8_t logMessagePeriod;    // ...
+} PtpHeader;
+
+// identification carrying Delay_Resp message
+typedef struct {
+    uint64_t requestingSourceClockIdentity;
+    uint16_t requestingSourcePortIdentity;
+} Delay_RespIdentification;
+
+typedef enum {
+    PTP_TP_IPv4 = 0,
+
+} PtpTransportType;
+
+typedef enum {
+    PTP_DM_E2E = 0,
+
+} PtpDelayMechanism;
+
+typedef enum {
+    PTP_TSPEC_UNKNOWN_DEF = 0,
+
+} PtpTransportSpecific;
+
+typedef enum {
+    PTP_MC_EVENT = 0,
+    PTP_MC_GENERAL = 1
+} PtpMessageClass;
+
+#define MAX_PTP_MSG_SIZE (128)
+typedef struct RawPtpMessage_ {
+    TimestampI ts;              // timestamp
+    uint32_t size;              // packet size
+
+    // --- transmit related ---
+    TimestampI *pTs;            // pointer to timestamp
+    void (*pTxCb)(const struct RawPtpMessage_ * pMsg);  // transmit callback function
+    PtpDelayMechanism tx_dm;    // transmit transport type
+    PtpMessageClass tx_mc;      // transmit message class
+
+    // --- data ---
+    uint8_t data[MAX_PTP_MSG_SIZE];     // raw packet data
+} RawPtpMessage;
+
+// contents of an announce message
+typedef struct {
+    uint16_t originCurrentUTCOffset;
+    uint8_t priority1;
+    uint8_t grandmasterClockClass;
+    uint8_t grandmasterClockAccuracy;
+    uint16_t grandmasterClockVariance;
+    uint8_t priority2;
+    uint64_t grandmasterClockIdentity;
+    uint16_t localStepsRemoved;
+    uint8_t timeSource;
+} PtpAnnounceBody;
+
+typedef PtpAnnounceBody PtpMasterProperties;
+
+// core state machine states
+typedef enum PtpM2SState {
+    SIdle, SWaitFollowUp
+} PtpM2SState;
+
+typedef enum {
+    SBMC_NO_MASTER = 0,
+    SBMC_MASTER_OK
+} SBmcMasterState;
+
+typedef enum {
+    SBMC_NO_CANDIDATE = 0,
+    SBMC_CANDIDATE_COLLECTION
+} SBmcCandidateState;
+
+typedef enum {
+    PTP_LOGPER_MIN = -4,
+    PTP_LOGPER_MAX = 4,
+    PTP_LOGPER_SYNCMATCHED = 127
+} PtpLogMsgPeriods;
+
+typedef struct {
+    PtpMasterProperties masterProps;    // master clock properties
+    uint16_t masterAnnPer_ms;   // message period of current master
+    PtpMasterProperties candProps;      // new master candidate properties
+    uint16_t candAnnPer_ms;     // message period for master candidate
+    bool preventMasterSwitchOver;       // set if master switchover is prohibited
+    SBmcMasterState mstState;   // master clock state machine
+    SBmcCandidateState candState;       // candidate state machine
+    uint8_t candCntr;           // counter before switching master
+    uint16_t masterTOCntr;      // current master announce dropout counter
+    uint16_t candTOCntr;        // candidate master announce cntr
+} PtpSBmcState;
+
+typedef struct {
+    PtpTransportType transportType;     // transport layer
+    PtpTransportSpecific transportSpecific;     // majorSdoId ('transportSpecific')
+    PtpDelayMechanism delayMechanism;   // delay mechanism
+    int8_t logDelayReqPeriod;   // logarithm of (P)DelayReq period
+    uint8_t domainNumber;       // PTP domain number
+} PtpProfile;
+
+typedef struct {
+    uint16_t sequenceID, delay_reqSequenceID;   // last sequence IDs
+    uint16_t lastRespondedDelReqId;     // ID of the last (P)Delay_Req got responded
+    PtpM2SState m2sState;       // Sync-FollowUp state
+    int8_t logSyncPeriod;       // logarithm of Sync interval
+    uint16_t syncPeriodMs;      // ...
+} PtpMessagingState;
+
+typedef struct {
+    uint32_t addend;            // hardware clock addend value
+} PtpHWClockState;
+
+typedef struct {
+    TimestampI meanPathDelay;   // mean path delay
+} PtpNetworkState;
+
+typedef struct {
+    double filtTimeErr;         // 0.1Hz lowpass-filtered time error
+    bool locked;                // is the PTP locked to defined limit?
+} PtpStats;
+
+typedef struct {
+    // PTP profile
+    PtpProfile profile;
+    struct {
+        uint16_t delReqPeriodMs;
+    } auxProfile;               // auxiliary, auto-calculated values for the selected profile
+
+    // Messaging state
+    PtpMessagingState messaging;
+
+    // Sync cycle data
+    PtpSyncCycleData scd;
+
+    // Hardware clock state
+    PtpHWClockState hwclock;
+    struct {
+        TimestampI offset;      // PPS signal offset
+        uint64_t clockIdentity; // clockIdentity calculated from MAC address
+    } hwoptions;
+
+    // Slave BMC state
+    PtpSBmcState sbmc;
+
+    // Network state
+    PtpNetworkState network;
+
+    // Logging
+    struct {
+        bool def;               // default
+        bool corr;              // correction fields
+        bool timestamps;        // timestamps
+        bool info;              // informative messages
+        bool locked;            // clock lock state change
+    } logging;
+
+    // Statistics
+    PtpStats stats;
+
+    // Timers...
+    struct {
+        TimerHandle_t sbmc;     // timer for managing SBMC operations
+        TimerHandle_t delreq;   // timer for (P)Del_Req
+    } timers;
+} PtpCoreState;
+
+// global storable-loadable configuration
+typedef struct {
+    PtpProfile profile;         // PTP-profile
+    TimestampI offset;          // PPS signal offset
+    uint32_t logging;           // logging compressed into a single bitfield
+} PtpConfig;
+
+#endif                          /* FLEXPTP_PTP_TYPES_H_ */
--- a/sbmc.c
+++ b/sbmc.c
@ -0,0 +1,21 @@
+#include "sbmc.h"
+#include "ptp_types.h"
+
+#define COMPARE_AND_RETURN(pmp1,pmp2,field) {\
+	if (pmp1->field < pmp2->field) {\
+		return 0;\
+	} else if (pmp1->field > pmp2->field) {\
+		return 1;\
+	}\
+}
+
+int ptp_select_better_master(PtpMasterProperties * pMP1, PtpMasterProperties * pMP2)
+{
+    COMPARE_AND_RETURN(pMP1, pMP2, priority1);
+    COMPARE_AND_RETURN(pMP1, pMP2, grandmasterClockClass);
+    COMPARE_AND_RETURN(pMP1, pMP2, grandmasterClockAccuracy);
+    COMPARE_AND_RETURN(pMP1, pMP2, grandmasterClockVariance);
+    COMPARE_AND_RETURN(pMP1, pMP2, priority2);
+    COMPARE_AND_RETURN(pMP1, pMP2, grandmasterClockIdentity);
+    return 1;
+}
--- a/sbmc.h
+++ b/sbmc.h
@ -0,0 +1,10 @@
+#ifndef FLEXPTP_SBMC_H_
+#define FLEXPTP_SBMC_H_
+
+#include "ptp_types.h"
+
+// select better master
+// return 0: if pMP1 is better than pMP2, 1: if reversed
+int ptp_select_better_master(PtpMasterProperties * pMP1, PtpMasterProperties * pMP2);
+
+#endif                          /* FLEXPTP_SBMC_H_ */
--- a/servo/pd_controller.c
+++ b/servo/pd_controller.c
@ -0,0 +1,144 @@
+/* (C) András Wiesner, 2021 */
+
+#include "pd_controller.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <flexptp_options.h>
+
+#include "../ptp_core.h"
+
+// ----------------------------------
+
+static struct {
+    bool internals;
+} sLog;
+
+static struct {
+    bool firstRun;
+} sState;
+
+// ----------------------------------
+
+//static float P_FACTOR = 0.5 * 0.476;
+//static float D_FACTOR = 2.0 * 0.476;
+
+static float P_FACTOR = 0.5 * 0.476;
+static float I_FACTOR = 0;
+static float D_FACTOR = 3.0;
+
+// ----------------------------------
+
+static double rd_prev_ppb;      // relative frequency error measured in previous iteration
+static double integrator_value;
+
+// ----------------------------------
+
+#ifdef CLI_REG_CMD
+
+static int CB_params(const CliToken_Type * ppArgs, uint8_t argc)
+{
+    // set if parameters passed after command
+    if (argc >= 3) {
+        P_FACTOR = atof(ppArgs[0]);
+        I_FACTOR = atof(ppArgs[1]);
+        D_FACTOR = atof(ppArgs[2]);
+    }
+
+    MSG("> PTP params: K_p = %.3f, K_i = %.3f, K_d = %.3f\n", P_FACTOR, I_FACTOR, D_FACTOR);
+
+    return 0;
+}
+
+static int CB_logInternals(const CliToken_Type * ppArgs, uint8_t argc)
+{
+    if (argc >= 1) {
+        int en = ONOFF(ppArgs[0]);
+        if (en >= 0) {
+            if (en && !sLog.internals) {
+                MSG("\nSyncIntv. [ns] | dt [ns] | gamma [ppb]\n\n");
+            }
+            sLog.internals = en;
+            return 0;
+        } else {
+            return -1;
+        }
+    } else {
+        return -1;
+    }
+}
+
+static struct {
+    int params;
+    int internals;
+} sCliCmdIdx = { 0 };
+
+static void pd_ctrl_register_cli_commands()
+{
+    sCliCmdIdx.params = cli_register_command("ptp servo params [Kp Kd] \t\t\tSet or query K_p and K_d servo parameters", 3, 0, CB_params);
+    sCliCmdIdx.internals = cli_register_command("ptp servo log internals {on|off} \t\t\tEnable or disable logging of servo internals", 4, 1, CB_logInternals);
+}
+
+static void pd_ctrl_remove_cli_commands()
+{
+    cli_remove_command(sCliCmdIdx.params);
+    cli_remove_command(sCliCmdIdx.internals);
+}
+
+#endif                          // CLI_REG_CMD
+
+void pd_ctrl_init()
+{
+    pd_ctrl_reset();
+
+#ifdef CLI_REG_CMD
+    pd_ctrl_register_cli_commands();
+#endif                          // CLI_REG_CMD
+}
+
+void pd_ctrl_deinit()
+{
+#ifdef CLI_REG_CMD
+    pd_ctrl_remove_cli_commands();
+#endif                          // CLI_REG_CMD
+}
+
+void pd_ctrl_reset()
+{
+    sState.firstRun = true;
+    integrator_value = 0;
+}
+
+float pd_ctrl_run(int32_t dt, PtpServoAuxInput * pAux)
+{
+    if (sState.firstRun) {
+        sState.firstRun = false;
+        rd_prev_ppb = dt;
+        integrator_value = 0;
+        return 0;
+    }
+
+    // calculate relative frequency error
+    //double rd_ppb = ((double) dt) / (pAux->msgPeriodMs * 1E+06 + dt) * 1E+09;
+    double rd_ppb = ((double)dt) / (pAux->measSyncPeriod + dt) * 1E+09;
+
+    // calculate difference
+    double rd_D_ppb = D_FACTOR * (rd_ppb - rd_prev_ppb);
+
+    // calculate output (run the PD controller)
+    double corr_ppb = -(P_FACTOR * (rd_ppb + rd_D_ppb) + integrator_value) * exp((pAux->measSyncPeriod * 1E-09) - 1.0);
+
+    // update integrator
+    integrator_value += I_FACTOR * rd_ppb;
+
+    // store error value (time difference) for use in next iteration
+    rd_prev_ppb = rd_ppb;
+
+    CLILOG(sLog.internals, "%d %f\n", dt, rd_ppb);
+
+    return corr_ppb;
+}
+
+// ----------------------------------
--- a/servo/pd_controller.h
+++ b/servo/pd_controller.h
@ -0,0 +1,15 @@
+/* (C) András Wiesner, 2021 */
+
+#ifndef SERVO_PD_CONTROLLER_H_
+#define SERVO_PD_CONTROLLER_H_
+
+#include <stdint.h>
+
+#include "../ptp_servo_types.h"
+
+void pd_ctrl_init();            // initialize PD controller
+void pd_ctrl_deinit();          // deinitialize PD controller
+void pd_ctrl_reset();           // reset controller
+float pd_ctrl_run(int32_t dt, PtpServoAuxInput * pAux); // run the controller (input: time error in nanosec)
+
+#endif                          /* SERVO_PD_CONTROLLER_H_ */
--- a/settings_interface.c
+++ b/settings_interface.c
@ -0,0 +1,64 @@
+#include "settings_interface.h"
+
+#include "ptp_core.h"
+
+extern PtpCoreState gPtpCoreState;
+#define S (gPtpCoreState)
+
+// set PPS offset
+void ptp_set_clock_offset(int32_t offset)
+{
+    nsToTsI(&S.hwoptions.offset, offset);
+}
+
+// get PPS offset
+int32_t ptp_get_clock_offset()
+{
+    return nsI(&S.hwoptions.offset);
+}
+
+void ptp_prefer_master_clock(uint64_t clockId)
+{
+    S.sbmc.preventMasterSwitchOver = true;
+    S.sbmc.masterProps.grandmasterClockIdentity = clockId;
+}
+
+void ptp_unprefer_master_clock()
+{
+    S.sbmc.preventMasterSwitchOver = false;
+}
+
+uint64_t ptp_get_current_master_clock_identity()
+{
+    return S.sbmc.masterProps.grandmasterClockIdentity;
+}
+
+uint64_t ptp_get_own_clock_identity()
+{
+    return S.hwoptions.clockIdentity;
+}
+
+void ptp_set_domain(uint8_t domain)
+{
+    S.profile.domainNumber = domain;
+}
+
+uint8_t ptp_get_domain()
+{
+    return S.profile.domainNumber;
+}
+
+void ptp_set_addend(uint32_t addend)
+{
+    S.hwclock.addend = addend;
+}
+
+uint32_t ptp_get_addend()
+{
+    return S.hwclock.addend;
+}
+
+void ptp_time(TimestampU * pT)
+{
+    PTP_HW_GET_TIME(pT);
+}
--- a/settings_interface.h
+++ b/settings_interface.h
@ -0,0 +1,21 @@
+#ifndef FLEXPTP_SETTINGS_INTERFACE_H_
+#define FLEXPTP_SETTINGS_INTERFACE_H_
+
+#include <stdint.h>
+
+#include "ptp_types.h"
+
+void ptp_set_clock_offset(int32_t offset);      // set clock offset in nanoseconds
+int32_t ptp_get_clock_offset(); // get clock offset in nanoseconds
+void ptp_prefer_master_clock(uint64_t clockId); // lock slave to a particular master
+void ptp_unprefer_master_clock();       // allow slave to synchronize to the BMCA-elected master
+uint64_t ptp_get_current_master_clock_identity();       // get current master clock ID
+uint64_t ptp_get_own_clock_identity();  // get out clock identity
+void ptp_set_domain(uint8_t domain);    // set PTP domain
+uint8_t ptp_get_domain();       // get PTP domain
+void ptp_set_addend(uint32_t addend);   // set hardware clock addend (frequency tuning!)
+uint32_t ptp_get_addend();      // get hardware clock addend
+
+void ptp_time(TimestampU * pT); // get time
+
+#endif                          /* FLEXPTP_SETTINGS_INTERFACE_H_ */
--- a/stats.c
+++ b/stats.c
@ -0,0 +1,42 @@
+#include "stats.h"
+
+#include "ptp_core.h"
+#include "ptp_defs.h"
+
+#include <memory.h>
+
+#define S (gPtpCoreState)
+
+// clear statistics
+void ptp_clear_stats()
+{
+    memset(&S.stats, 0, sizeof(PtpStats));
+}
+
+// get statistics
+const PtpStats *ptp_get_stats()
+{
+    return &S.stats;
+}
+
+// filter parameters for statistics calculation
+// WARNING: Data calculation won't be totally accurate due to changing sampling time!
+
+#define PTP_TE_FILT_Fc_HZ (0.1) // cutoff frequency (Hz)
+#define PTP_TE_FILT_Ts_S (1)    // sampling time (s)
+
+// collect statistics
+void ptp_collect_stats(int64_t d)
+{
+    double a = exp(-PTP_TE_FILT_Fc_HZ * 2 * M_PI * (S.messaging.syncPeriodMs / 1000.0));
+
+    // performing time error filtering
+    double y_prev = S.stats.filtTimeErr, y;
+    y = a * y_prev + (1 - a) * d;       // filtering equation
+    S.stats.filtTimeErr = y;
+
+    // set locked state
+    bool locked = ((fabs(S.stats.filtTimeErr) < (PTP_ACCURACY_LIMIT_NS)) && (ptp_get_current_master_clock_identity() != 0));
+    CLILOG(S.logging.locked && (locked != S.stats.locked), "PTP %s!", locked ? "LOCKED" : "DIVERGED");
+    S.stats.locked = locked;
+}
--- a/stats.h
+++ b/stats.h
@ -0,0 +1,13 @@
+#ifndef FLEXPTP_STATS_H_
+#define FLEXPTP_STATS_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "ptp_types.h"
+
+void ptp_clear_stats();         // clear statistics
+const PtpStats *ptp_get_stats();        // get statistics
+void ptp_collect_stats(int64_t d);      // collect statistics
+
+#endif                          /* FLEXPTP_STATS_H_ */
--- a/task_ptp.c
+++ b/task_ptp.c
@ -0,0 +1,244 @@
+#include "FreeRTOS.h"
+#include "task.h"
+#include "queue.h"
+
+#include <flexptp_options.h>
+
+#include "ptp_core.h"
+#include "task_ptp.h"
+
+#include "lwip/igmp.h"
+
+#include "ptp_defs.h"
+
+#include "ptp_msg_tx.h"
+
+#include "ptp_raw_msg_circbuf.h"
+
+#include "settings_interface.h"
+
+// ----- TASK PROPERTIES -----
+static TaskHandle_t sTH;        // task handle
+static uint8_t sPrio = 5;       // priority
+static uint16_t sStkSize = 4096;        // stack size
+void task_ptp(void *pParam);    // task routine function
+// ---------------------------
+
+static bool sPTP_operating = false;     // does the PTP subsystem operate?
+
+// ---------------------------
+
+// udp control blocks
+static struct udp_pcb *spPTP_PRIMARY_EVENT_pcb = NULL;
+static struct udp_pcb *spPTP_PRIMARY_GENERAL_pcb = NULL;
+
+// callback function receiveing data from udp "sockets"
+void ptp_recv_cb(void *pArg, struct udp_pcb *pPCB, struct pbuf *pP, const ip_addr_t * pAddr, uint16_t port);
+
+// FIFO for incoming packets
+#define RX_PACKET_FIFO_LENGTH (32)
+#define TX_PACKET_FIFO_LENGTH (16)
+static QueueHandle_t sRxPacketFIFO;
+QueueHandle_t gTxPacketFIFO;
+static QueueSetHandle_t sRxTxFIFOSet;
+
+// create udp listeners
+void create_ptp_listeners()
+{
+    // create packet FIFO
+    sRxPacketFIFO = xQueueCreate(RX_PACKET_FIFO_LENGTH, sizeof(uint8_t));
+    gTxPacketFIFO = xQueueCreate(TX_PACKET_FIFO_LENGTH, sizeof(uint8_t));
+    sRxTxFIFOSet = xQueueCreateSet(RX_PACKET_FIFO_LENGTH + TX_PACKET_FIFO_LENGTH);
+    xQueueAddToSet(sRxPacketFIFO, sRxTxFIFOSet);
+    xQueueAddToSet(gTxPacketFIFO, sRxTxFIFOSet);
+
+    // PRIMARY EVENT (...1.129:319)
+    spPTP_PRIMARY_EVENT_pcb = udp_new();
+    udp_bind(spPTP_PRIMARY_EVENT_pcb, &PTP_IGMP_PRIMARY, PTP_PORT_EVENT);
+    udp_recv(spPTP_PRIMARY_EVENT_pcb, ptp_recv_cb, NULL);
+
+    // PRIMARY GENERAL (...1.129:320)
+    spPTP_PRIMARY_GENERAL_pcb = udp_new();
+    udp_bind(spPTP_PRIMARY_GENERAL_pcb, &PTP_IGMP_PRIMARY, PTP_PORT_GENERAL);
+    udp_recv(spPTP_PRIMARY_GENERAL_pcb, ptp_recv_cb, NULL);
+
+}
+
+// remove listeners
+void destroy_ptp_listeners()
+{
+    // disconnect UDP "sockets"
+    udp_disconnect(spPTP_PRIMARY_EVENT_pcb);
+    udp_disconnect(spPTP_PRIMARY_GENERAL_pcb);
+
+    // destroy UDP sockets
+    udp_remove(spPTP_PRIMARY_EVENT_pcb);
+    udp_remove(spPTP_PRIMARY_GENERAL_pcb);
+
+    // destroy packet FIFO
+    xQueueRemoveFromSet(sRxPacketFIFO, sRxTxFIFOSet);
+    xQueueRemoveFromSet(gTxPacketFIFO, sRxTxFIFOSet);
+    vQueueDelete(sRxPacketFIFO);
+    vQueueDelete(gTxPacketFIFO);
+    vQueueDelete(sRxTxFIFOSet);
+}
+
+// join PTP IGMP groups
+void join_ptp_igmp_groups()
+{
+    // join group for default set of messages (everything except for peer delay)
+    igmp_joingroup(&netif_default->ip_addr, &PTP_IGMP_PRIMARY);
+
+}
+
+// leave PTP IGMP group
+void leave_ptp_igmp_groups()
+{
+    // leave default group
+    igmp_leavegroup(&netif_default->ip_addr, &PTP_IGMP_PRIMARY);
+
+}
+
+// "ring" buffer for PTP-messages
+PtpCircBuf gRawRxMsgBuf, gRawTxMsgBuf;
+static RawPtpMessage sRawRxMsgBufPool[RX_PACKET_FIFO_LENGTH];
+static RawPtpMessage sRawTxMsgBufPool[TX_PACKET_FIFO_LENGTH];
+
+static void init_raw_buffers()
+{
+    ptp_circ_buf_init(&gRawRxMsgBuf, sRawRxMsgBufPool, RX_PACKET_FIFO_LENGTH);
+    ptp_circ_buf_init(&gRawTxMsgBuf, sRawTxMsgBufPool, TX_PACKET_FIFO_LENGTH);
+}
+
+// register PTP task and initialize
+void reg_task_ptp()
+{
+    init_raw_buffers();         // initialize raw buffers
+
+    ptp_init();                 // initialize PTP subsystem
+
+#ifdef PTP_CONFIG_PTR           // load config if provided
+    MSG("Loading PTP-config!\n");
+    ptp_load_config_from_dump(PTP_CONFIG_PTR());
+#endif
+
+    // create UDP sockets regardless the transfer type
+    join_ptp_igmp_groups();     // enter PTP IGMP groups
+    create_ptp_listeners();     // create listeners
+#ifndef SIMULATION
+    ptp_transmit_init(spPTP_PRIMARY_EVENT_pcb, spPTP_PRIMARY_GENERAL_pcb,);     // initialize transmit function*/
+#endif
+
+    // create task
+    BaseType_t result = xTaskCreate(task_ptp, "ptp", sStkSize, NULL, sPrio, &sTH);
+    if (result != pdPASS) {
+        MSG("Failed to create task! (errcode: %d)\n", result);
+        unreg_task_ptp();
+        return;
+    }
+
+    sPTP_operating = true;      // the PTP subsystem is operating
+}
+
+// unregister PTP task
+void unreg_task_ptp()
+{
+    vTaskDelete(sTH);           // taszk törlése
+
+    ptp_deinit();               // ptp subsystem de-initialization
+
+    // destroy listeners
+    leave_ptp_igmp_groups();    // leave IGMP groups
+    destroy_ptp_listeners();    // delete listeners
+
+    sPTP_operating = false;     // the PTP subsystem is operating
+}
+
+// callback for packet reception on port 319 and 320
+void ptp_recv_cb(void *pArg, struct udp_pcb *pPCB, struct pbuf *pP, const ip_addr_t * pAddr, uint16_t port)
+{
+    // put msg into the queue
+    ptp_enqueue_msg(pP->payload, pP->len, pP->time_s, pP->time_ns, PTP_TP_IPv4);
+
+    // release pbuf resources
+    pbuf_free(pP);
+}
+
+// put ptp message onto processing queue
+void ptp_enqueue_msg(void *pPayload, uint32_t len, uint32_t ts_sec, uint32_t ts_ns, int tp)
+{
+    // only consider messages received on the matching transport layer
+    if (tp != ptp_get_transport_type()) {
+        return;
+    }
+
+    // enqueue message
+    RawPtpMessage *pMsg = ptp_circ_buf_alloc(&gRawRxMsgBuf);
+    if (pMsg) {
+        // copy payload and timestamp
+        uint32_t copyLen = MIN(len, MAX_PTP_MSG_SIZE);
+        memcpy(pMsg->data, pPayload, copyLen);
+        pMsg->size = copyLen;
+        pMsg->ts.sec = ts_sec;
+        pMsg->ts.nanosec = ts_ns;
+        pMsg->pTs = NULL;
+        pMsg->pTxCb = NULL;     // not meaningful...
+
+        uint8_t idx = ptp_circ_buf_commit(&gRawRxMsgBuf);
+
+        xQueueSend(sRxPacketFIFO, &idx, portMAX_DELAY); // send index
+    } else {
+        MSG("PTP-packet buffer full, a packet has been dropped!\n");
+    }
+
+    // MSG("TS: %u.%09u\n", (uint32_t)ts_sec, (uint32_t)ts_ns);
+
+    // if the transport layer matches...
+}
+
+// taszk függvénye
+void task_ptp(void *pParam)
+{
+    while (1) {
+        // wait for received packet or packet to transfer
+        QueueHandle_t activeQueue = xQueueSelectFromSet(sRxTxFIFOSet, pdMS_TO_TICKS(200));
+
+        // if packet is on the RX queue
+        if (activeQueue == sRxPacketFIFO) {
+            // pop packet from FIFO
+            uint8_t bufIdx;
+            xQueueReceive(sRxPacketFIFO, &bufIdx, portMAX_DELAY);
+
+            // fetch buffer
+            RawPtpMessage *pRawMsg = ptp_circ_buf_get(&gRawRxMsgBuf, bufIdx);
+            pRawMsg->pTs = &pRawMsg->ts;
+
+            // process packet
+            ptp_process_packet(pRawMsg);
+
+            // free buffer
+            ptp_circ_buf_free(&gRawRxMsgBuf);
+        } else if (activeQueue == gTxPacketFIFO) {
+            // pop packet from FIFO
+            uint8_t bufIdx;
+            xQueueReceive(gTxPacketFIFO, &bufIdx, portMAX_DELAY);
+
+            // fetch buffer
+            RawPtpMessage *pRawMsg = ptp_circ_buf_get(&gRawTxMsgBuf, bufIdx);
+            ptp_transmit_msg(pRawMsg);
+
+            // free buffer
+            ptp_circ_buf_free(&gRawTxMsgBuf);
+        } else {
+            // ....
+        }
+    }
+}
+
+// --------------------------
+
+// function to query PTP operation state
+bool task_ptp_is_operating()
+{
+    return sPTP_operating;
+}
--- a/task_ptp.h
+++ b/task_ptp.h
@ -0,0 +1,11 @@
+#ifndef TASK_PTP_H_
+#define TASK_PTP_H_
+
+#include <stdint.h>
+
+void reg_task_ptp();            // register PTP task
+void unreg_task_ptp();          // unregister PTP task
+bool task_ptp_is_operating();   // does PTP task operate?
+void ptp_enqueue_msg(void *pPayload, uint32_t len, uint32_t ts_sec, uint32_t ts_ns, int tp);    // put PTP-message on queue
+
+#endif                          // TASK_PTP_H_
--- a/timeutils.c
+++ b/timeutils.c
@ -0,0 +1,135 @@
+/* (C) András Wiesner, 2020 */
+
+#include "timeutils.h"
+
+#include <flexptp_options.h>
+
+#include <stdio.h>
+
+// TimestampU -> TimestampI
+TimestampI *tsUToI(TimestampI * ti, const TimestampU * tu)
+{
+    ti->sec = (int64_t) tu->sec;
+    ti->nanosec = (int32_t) tu->nanosec;
+    return ti;
+}
+
+// r = a + b;
+TimestampI *addTime(TimestampI * r, const TimestampI * a, const TimestampI * b)
+{
+    int64_t ns = nsI(a) + nsI(b);
+    nsToTsI(r, ns);
+    return r;
+}
+
+// r = a - b;
+TimestampI *subTime(TimestampI * r, const TimestampI * a, const TimestampI * b)
+{
+    int64_t ns = nsI(a) - nsI(b);
+    nsToTsI(r, ns);
+    return r;
+}
+
+TimestampI *divTime(TimestampI * r, const TimestampI * a, int divisor)
+{
+    int64_t ns = a->sec * NANO_PREFIX + a->nanosec;
+    ns = ns / divisor;          // így a pontosság +-0.5ns
+    r->sec = ns / NANO_PREFIX;
+    r->nanosec = ns - r->sec * NANO_PREFIX;
+    return r;
+}
+
+uint64_t nsU(const TimestampU * t)
+{
+    return t->sec * NANO_PREFIX + t->nanosec;
+}
+
+int64_t nsI(const TimestampI * t)
+{
+    return t->sec * NANO_PREFIX + t->nanosec;
+}
+
+void normTime(TimestampI * t)
+{
+    uint32_t s = t->nanosec / NANO_PREFIX;
+    t->sec += s;
+    t->nanosec -= s * NANO_PREFIX;
+}
+
+int64_t tsToTick(const TimestampI * ts, uint32_t tps)
+{
+    int64_t ns = ts->sec * NANO_PREFIX + ts->nanosec;
+    int64_t ticks = (ns * tps) / NANO_PREFIX;
+    return ticks;
+}
+
+TimestampI *nsToTsI(TimestampI * r, int64_t ns)
+{
+    r->sec = ns / NANO_PREFIX;
+    r->nanosec = ns % NANO_PREFIX;
+    return r;
+}
+
+bool nonZeroI(const TimestampI * a)
+{
+    return a->sec != 0 || a->nanosec != 0;
+}
+
+static unsigned FIRST_DAY_OF_MONTH[] = { 0, 31, 59, 60, 120, 151, 181, 212, 243, 273, 304, 334, 365 };
+
+void tsPrint(char *str, const TimestampI * ts)
+{
+    unsigned fouryear, year, month, day, hour, minute, sec, rem;
+
+    fouryear = ts->sec / T_SEC_PER_FOURYEAR;    // determine elapsed four year blocks from 1970 (YYLY)
+    rem = ts->sec - fouryear * T_SEC_PER_FOURYEAR;      // compute remaining seconds
+    year = fouryear * 4;        // calculate years fouryear-block part
+
+    // split up four-year blocks into distinct years
+    if (rem > T_SEC_PER_YEAR) {
+        year++;
+        rem -= T_SEC_PER_YEAR;
+    }
+
+    if (rem > T_SEC_PER_YEAR) {
+        year++;
+        rem -= T_SEC_PER_YEAR;
+    }
+
+    if (rem > T_SEC_PER_LEAPYEAR) {
+        year++;
+        rem -= T_SEC_PER_LEAPYEAR;
+    }
+    // convert remaining seconds to days
+    day = rem / T_SEC_PER_DAY;
+    rem -= day * T_SEC_PER_DAY;
+    day++;
+
+    year += 1970;
+    bool leapyear = year % 4 == 0;
+
+    // get month from days
+    unsigned i = 0;
+    for (i = 0; i < 12; i++) {
+        unsigned first1 = FIRST_DAY_OF_MONTH[i] + (((i >= 2) && leapyear) ? 1 : 0);
+        unsigned first2 = FIRST_DAY_OF_MONTH[i + 1] + (((i + 1 >= 2) && leapyear) ? 1 : 0);
+
+        if (day > first1 && day <= first2) {
+            month = i + 1;
+            day = day - first1;
+            break;
+        }
+    }
+
+    // get time
+    hour = rem / T_SEC_PER_HOUR;
+    rem -= hour * T_SEC_PER_HOUR;
+    minute = rem / T_SEC_PER_MINUTE;
+    rem -= minute * T_SEC_PER_MINUTE;
+    sec = rem;
+
+    // -------------------------------
+
+    // print datetime
+    SPRINTF(str, 20, "%02d-%02d-%04d %02d:%02d:%02d", day, month, year, hour, minute, sec);
+}
--- a/timeutils.h
+++ b/timeutils.h
@ -0,0 +1,45 @@
+/* (C) András Wiesner, 2020 */
+
+#ifndef TIMEUTILS_H
+#define TIMEUTILS_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+// timestamp (unsigned)
+typedef struct {
+    uint64_t sec;
+    uint32_t nanosec;
+} TimestampU;
+
+// timestamp (signed)
+typedef struct {
+    int64_t sec;
+    int32_t nanosec;
+} TimestampI;
+
+#define NANO_PREFIX (1000000000)
+#define NANO_PREFIX_F (1000000000.0f)
+
+#define T_SEC_PER_MINUTE (60)
+#define T_SEC_PER_HOUR (60 * T_SEC_PER_MINUTE)
+#define T_SEC_PER_DAY (24 * T_SEC_PER_HOUR)
+#define T_SEC_PER_YEAR (365 * T_SEC_PER_DAY)
+#define T_SEC_PER_LEAPYEAR (366 * T_SEC_PER_DAY)
+#define T_SEC_PER_FOURYEAR (3 * T_SEC_PER_YEAR + T_SEC_PER_LEAPYEAR)
+
+// TIME OPERATIONS
+TimestampI *tsUToI(TimestampI * ti, const TimestampU * tu);     // convert unsigned timestamp to signed
+TimestampI *addTime(TimestampI * r, const TimestampI * a, const TimestampI * b);        // sum timestamps (r = a + b)
+TimestampI *subTime(TimestampI * r, const TimestampI * a, const TimestampI * b);        // substract timestamps (r = a - b)
+TimestampI *divTime(TimestampI * r, const TimestampI * a, int divisor); // divide time value by an integer
+uint64_t nsU(const TimestampU * t);     // convert unsigned time into nanoseconds
+int64_t nsI(const TimestampI * t);      // convert signed time into nanoseconds
+void normTime(TimestampI * t);  // normalize time
+int64_t tsToTick(const TimestampI * ts, uint32_t tps);  // convert time to hardware ticks, tps: ticks per second
+TimestampI *nsToTsI(TimestampI * r, int64_t ns);        // convert nanoseconds to time
+bool nonZeroI(const TimestampI * a);    // does the timestamp differ from zero?
+
+void tsPrint(char *str, const TimestampI * ts); // print datetime to string, string must be 20-long at least!
+
+#endif                          /* TIMEUTILS_H */