commit ac63947fe1eb65a8f7de4dfb957952516b37b901 Author: Wiesner András Date: Sun Apr 16 21:20:57 2023 +0200 flexPTP basic excerpt diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..8901861 --- /dev/null +++ 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) \ No newline at end of file diff --git a/cli_cmds.c b/cli_cmds.c new file mode 100644 index 0000000..7abb7fb --- /dev/null +++ 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 + +//#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 +} diff --git a/cli_cmds.h b/cli_cmds.h new file mode 100644 index 0000000..e2a7723 --- /dev/null +++ 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_ */ diff --git a/clock_utils.c b/clock_utils.c new file mode 100644 index 0000000..606b671 --- /dev/null +++ b/clock_utils.c @@ -0,0 +1,34 @@ +#include +#include + +#include + +#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"); +} diff --git a/clock_utils.h b/clock_utils.h new file mode 100644 index 0000000..c9b59dc --- /dev/null +++ b/clock_utils.h @@ -0,0 +1,13 @@ +#ifndef FLEXPTP_CLOCK_UTILS_H_ +#define FLEXPTP_CLOCK_UTILS_H_ + +#include + +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_ */ diff --git a/format_utils.c b/format_utils.c new file mode 100644 index 0000000..9ab4180 --- /dev/null +++ b/format_utils.c @@ -0,0 +1,39 @@ +/* (C) András Wiesner, 2020-2022 */ + +#include "format_utils.h" + +#include + +// 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); +} diff --git a/format_utils.h b/format_utils.h new file mode 100644 index 0000000..65f920b --- /dev/null +++ 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 + +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_ */ diff --git a/hw_port/flexptp_options_simulation.h b/hw_port/flexptp_options_simulation.h new file mode 100644 index 0000000..7fd6950 --- /dev/null +++ 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 + +#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 + +// 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 + +#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_ diff --git a/hw_port/flexptp_options_stm32h743.h b/hw_port/flexptp_options_stm32h743.h new file mode 100644 index 0000000..2390ea7 --- /dev/null +++ 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_ diff --git a/hw_port/flexptp_options_tm4c1294.h b/hw_port/flexptp_options_tm4c1294.h new file mode 100644 index 0000000..3db9d53 --- /dev/null +++ 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_ diff --git a/hw_port/ptp_port_simulation.cpp b/hw_port/ptp_port_simulation.cpp new file mode 100644 index 0000000..9a980da --- /dev/null +++ b/hw_port/ptp_port_simulation.cpp @@ -0,0 +1,42 @@ +#include "ptp_port_simulation.h" + +#include +#include +#include +#include + +#include "simsrc/PtpClock.h" + +std::shared_ptr 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(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); +} + +} + diff --git a/hw_port/ptp_port_simulation.h b/hw_port/ptp_port_simulation.h new file mode 100644 index 0000000..7eaa92e --- /dev/null +++ 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_ */ diff --git a/hw_port/ptp_port_stm32h743.c b/hw_port/ptp_port_stm32h743.c new file mode 100644 index 0000000..20736f4 --- /dev/null +++ b/hw_port/ptp_port_stm32h743.c @@ -0,0 +1,146 @@ +#include "ptp_port_stm32h743.h" + +#include +#include +#include + +#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; +} diff --git a/hw_port/ptp_port_stm32h743.h b/hw_port/ptp_port_stm32h743.h new file mode 100644 index 0000000..9fce2fa --- /dev/null +++ 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_ */ diff --git a/hw_port/ptp_port_tiva_tm4c1294.c b/hw_port/ptp_port_tiva_tm4c1294.c new file mode 100644 index 0000000..f5d0e00 --- /dev/null +++ b/hw_port/ptp_port_tiva_tm4c1294.c @@ -0,0 +1,30 @@ +#include "ptp_port_tiva_tm4c1295.h" + +#include +#include + +#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)); +} diff --git a/hw_port/ptp_port_tiva_tm4c1295.h b/hw_port/ptp_port_tiva_tm4c1295.h new file mode 100644 index 0000000..9fce2fa --- /dev/null +++ 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_ */ diff --git a/hw_port/simsrc/FreeRTOS_simulation.cpp b/hw_port/simsrc/FreeRTOS_simulation.cpp new file mode 100644 index 0000000..e86a6e5 --- /dev/null +++ b/hw_port/simsrc/FreeRTOS_simulation.cpp @@ -0,0 +1,100 @@ +#include "FreeRTOS_simulation.h" + +#include +#include +#include +#include + +struct TmrInfo { + std::string name; + uint32_t timeout; + bool singleShot; + void *timerID; + tmrcb cb; +}; + +static std::map 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); +//} + +} \ No newline at end of file diff --git a/hw_port/simsrc/FreeRTOS_simulation.h b/hw_port/simsrc/FreeRTOS_simulation.h new file mode 100644 index 0000000..f129b39 --- /dev/null +++ b/hw_port/simsrc/FreeRTOS_simulation.h @@ -0,0 +1,32 @@ +#ifndef FLEXPTP_SIM_FREERTOS_SIMULATION_H +#define FLEXPTP_SIM_FREERTOS_SIMULATION_H + +#include +#include +#include +#include + +#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 diff --git a/hw_port/simsrc/PtpClock.cpp b/hw_port/simsrc/PtpClock.cpp new file mode 100644 index 0000000..31b8b59 --- /dev/null +++ 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); +} diff --git a/hw_port/simsrc/PtpClock.h b/hw_port/simsrc/PtpClock.h new file mode 100644 index 0000000..f704e92 --- /dev/null +++ b/hw_port/simsrc/PtpClock.h @@ -0,0 +1,36 @@ +#ifndef FLEXPTP_SIM_PTPCLOCK_H +#define FLEXPTP_SIM_PTPCLOCK_H + +#include + +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 diff --git a/hw_port/simsrc/PtpMasterClock.cpp b/hw_port/simsrc/PtpMasterClock.cpp new file mode 100644 index 0000000..bb14fa9 --- /dev/null +++ b/hw_port/simsrc/PtpMasterClock.cpp @@ -0,0 +1,21 @@ +// +// Created by epagris on 2022.10.22.. +// + +#include "PtpMasterClock.h" + +#include + +void PtpMasterClock::setup(const PtpMasterClock::PtpMasterClockSettings &settings) { + mSettings = settings; +} + +void PtpMasterClock::start() { + mBeaconThread = std::make_shared(&PtpMasterClock::mBeaconThread, this); +} + +void PtpMasterClock::mBeaconThread_CB() { + while (mRunning) { + + } +} diff --git a/hw_port/simsrc/PtpMasterClock.h b/hw_port/simsrc/PtpMasterClock.h new file mode 100644 index 0000000..8654d8a --- /dev/null +++ b/hw_port/simsrc/PtpMasterClock.h @@ -0,0 +1,35 @@ +#ifndef FLEXPTP_SIM_PTPMASTERCLOCK_H +#define FLEXPTP_SIM_PTPMASTERCLOCK_H + +#include +#include +#include +#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 diff --git a/hw_port/simsrc/PtpSlaveClock.cpp b/hw_port/simsrc/PtpSlaveClock.cpp new file mode 100644 index 0000000..8c82942 --- /dev/null +++ b/hw_port/simsrc/PtpSlaveClock.cpp @@ -0,0 +1,5 @@ +// +// Created by epagris on 2022.10.22.. +// + +#include "PtpSlaveClock.h" diff --git a/hw_port/simsrc/PtpSlaveClock.h b/hw_port/simsrc/PtpSlaveClock.h new file mode 100644 index 0000000..3398da2 --- /dev/null +++ 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 diff --git a/hw_port/simsrc/lwip_simulation.cpp b/hw_port/simsrc/lwip_simulation.cpp new file mode 100644 index 0000000..303bd9d --- /dev/null +++ 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; +} + +} diff --git a/hw_port/simsrc/lwip_simulation.h b/hw_port/simsrc/lwip_simulation.h new file mode 100644 index 0000000..40259d0 --- /dev/null +++ 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 +#include + + 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 diff --git a/logging.c b/logging.c new file mode 100644 index 0000000..372e759 --- /dev/null +++ b/logging.c @@ -0,0 +1,60 @@ +#include "logging.h" + +#include + +#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++; + } +} diff --git a/logging.h b/logging.h new file mode 100644 index 0000000..cdced0c --- /dev/null +++ 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_ */ diff --git a/msg_utils.c b/msg_utils.c new file mode 100644 index 0000000..6210650 --- /dev/null +++ b/msg_utils.c @@ -0,0 +1,243 @@ +/* (C) András Wiesner, 2020-2022 */ + +#include "msg_utils.h" + +#include + +#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); +} diff --git a/msg_utils.h b/msg_utils.h new file mode 100644 index 0000000..58a813e --- /dev/null +++ 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 + +#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_ */ diff --git a/ptp_core.c b/ptp_core.c new file mode 100644 index 0000000..71cfcd6 --- /dev/null +++ b/ptp_core.c @@ -0,0 +1,610 @@ +/* (C) András Wiesner, 2020-2022 */ + +#include + +#include + +#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(); +} + +// ----------------------------------------------- diff --git a/ptp_core.h b/ptp_core.h new file mode 100644 index 0000000..f7b8eaf --- /dev/null +++ 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 +#include +#include +#include + +#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 + +// ------------------------------------------- +// (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 */ diff --git a/ptp_defs.c b/ptp_defs.c new file mode 100644 index 0000000..e5279b2 --- /dev/null +++ b/ptp_defs.c @@ -0,0 +1,3 @@ +#include "ptp_defs.h" + +const ip_addr_t PTP_IGMP_PRIMARY = { 0x810100E0 }; // 224.0.1.129 diff --git a/ptp_defs.h b/ptp_defs.h new file mode 100644 index 0000000..1f191ed --- /dev/null +++ b/ptp_defs.h @@ -0,0 +1,41 @@ +#ifndef FLEXPTP_PTP_DEFS_H_ +#define FLEXPTP_PTP_DEFS_H_ + +#include "flexptp_options.h" + +#include + +// 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_ */ diff --git a/ptp_msg_tx.c b/ptp_msg_tx.c new file mode 100644 index 0000000..cfd8506 --- /dev/null +++ b/ptp_msg_tx.c @@ -0,0 +1,94 @@ +#include + +#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 +} diff --git a/ptp_msg_tx.h b/ptp_msg_tx.h new file mode 100644 index 0000000..bf01dee --- /dev/null +++ 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 diff --git a/ptp_profile_presets.c b/ptp_profile_presets.c new file mode 100644 index 0000000..e25f916 --- /dev/null +++ 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; + } +} diff --git a/ptp_profile_presets.h b/ptp_profile_presets.h new file mode 100644 index 0000000..905e450 --- /dev/null +++ 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_ */ diff --git a/ptp_raw_msg_circbuf.c b/ptp_raw_msg_circbuf.c new file mode 100644 index 0000000..ca6ce38 --- /dev/null +++ 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]); +} diff --git a/ptp_raw_msg_circbuf.h b/ptp_raw_msg_circbuf.h new file mode 100644 index 0000000..7cbea97 --- /dev/null +++ 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 + +#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_ */ diff --git a/ptp_servo_types.h b/ptp_servo_types.h new file mode 100644 index 0000000..bd295e3 --- /dev/null +++ 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 diff --git a/ptp_sync_cycle_data.h b/ptp_sync_cycle_data.h new file mode 100644 index 0000000..7bbbd5c --- /dev/null +++ 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_ */ diff --git a/ptp_types.h b/ptp_types.h new file mode 100644 index 0000000..db5e852 --- /dev/null +++ b/ptp_types.h @@ -0,0 +1,268 @@ +#ifndef FLEXPTP_PTP_TYPES_H_ +#define FLEXPTP_PTP_TYPES_H_ + +#include +#include + +#include "ptp_sync_cycle_data.h" + +#include + +#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_ */ diff --git a/sbmc.c b/sbmc.c new file mode 100644 index 0000000..71f179f --- /dev/null +++ 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; +} diff --git a/sbmc.h b/sbmc.h new file mode 100644 index 0000000..13d29ee --- /dev/null +++ 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_ */ diff --git a/servo/pd_controller.c b/servo/pd_controller.c new file mode 100644 index 0000000..1ebab5d --- /dev/null +++ b/servo/pd_controller.c @@ -0,0 +1,144 @@ +/* (C) András Wiesner, 2021 */ + +#include "pd_controller.h" + +#include +#include +#include + +#include + +#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; +} + +// ---------------------------------- diff --git a/servo/pd_controller.h b/servo/pd_controller.h new file mode 100644 index 0000000..9321e15 --- /dev/null +++ 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 + +#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_ */ diff --git a/settings_interface.c b/settings_interface.c new file mode 100644 index 0000000..21e0f6f --- /dev/null +++ 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); +} diff --git a/settings_interface.h b/settings_interface.h new file mode 100644 index 0000000..17c9d32 --- /dev/null +++ b/settings_interface.h @@ -0,0 +1,21 @@ +#ifndef FLEXPTP_SETTINGS_INTERFACE_H_ +#define FLEXPTP_SETTINGS_INTERFACE_H_ + +#include + +#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_ */ diff --git a/stats.c b/stats.c new file mode 100644 index 0000000..a367ee2 --- /dev/null +++ b/stats.c @@ -0,0 +1,42 @@ +#include "stats.h" + +#include "ptp_core.h" +#include "ptp_defs.h" + +#include + +#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; +} diff --git a/stats.h b/stats.h new file mode 100644 index 0000000..cb345a8 --- /dev/null +++ b/stats.h @@ -0,0 +1,13 @@ +#ifndef FLEXPTP_STATS_H_ +#define FLEXPTP_STATS_H_ + +#include +#include + +#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_ */ diff --git a/task_ptp.c b/task_ptp.c new file mode 100644 index 0000000..6808ec5 --- /dev/null +++ b/task_ptp.c @@ -0,0 +1,244 @@ +#include "FreeRTOS.h" +#include "task.h" +#include "queue.h" + +#include + +#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; +} diff --git a/task_ptp.h b/task_ptp.h new file mode 100644 index 0000000..92ffa85 --- /dev/null +++ b/task_ptp.h @@ -0,0 +1,11 @@ +#ifndef TASK_PTP_H_ +#define TASK_PTP_H_ + +#include + +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_ diff --git a/timeutils.c b/timeutils.c new file mode 100644 index 0000000..1f75ed1 --- /dev/null +++ b/timeutils.c @@ -0,0 +1,135 @@ +/* (C) András Wiesner, 2020 */ + +#include "timeutils.h" + +#include + +#include + +// 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); +} diff --git a/timeutils.h b/timeutils.h new file mode 100644 index 0000000..e50465b --- /dev/null +++ b/timeutils.h @@ -0,0 +1,45 @@ +/* (C) András Wiesner, 2020 */ + +#ifndef TIMEUTILS_H +#define TIMEUTILS_H + +#include +#include + +// 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 */