diff --git a/.gitignore b/.gitignore index e19e25a0..ea1d8b70 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ .vscode/ +.idea/ __pycache__/ *.egg-info/ diff --git a/NetX/inc/rtc.h b/NetX/inc/rtc.h new file mode 100644 index 00000000..26b0f510 --- /dev/null +++ b/NetX/inc/rtc.h @@ -0,0 +1,12 @@ +#ifndef RTC_H +#define RTC_H + +#include "nxd_ptp_client.h" +#include "nx_api.h" + +UINT nx_ptp_client_hard_clock_callback(NX_PTP_CLIENT *client_ptr, + UINT operation, NX_PTP_TIME *time_ptr, + NX_PACKET *packet_ptr, + VOID *callback_data); + +#endif // RTC_H diff --git a/NetX/src/nxd_ptp_client.c b/NetX/src/nxd_ptp_client.c index 98804715..9477177b 100644 --- a/NetX/src/nxd_ptp_client.c +++ b/NetX/src/nxd_ptp_client.c @@ -6152,6 +6152,6 @@ LONG ns; /* return result */ time_ptr -> second_high = sec_hi; time_ptr -> second_low = sec_lo; - time_ptr -> nanosecond = ns; + time_ptr -> nanosecond = ns; } - // clang-format on \ No newline at end of file + // clang-format on diff --git a/NetX/src/u_nx_ethernet.c b/NetX/src/u_nx_ethernet.c index d1a6ee38..88773644 100644 --- a/NetX/src/u_nx_ethernet.c +++ b/NetX/src/u_nx_ethernet.c @@ -1,5 +1,6 @@ // clang-format off #include "u_nx_ethernet.h" +#include "rtc.h" #include "nx_stm32_eth_driver.h" #include "nxd_ptp_client.h" #include "u_nx_debug.h" @@ -227,7 +228,7 @@ uint8_t ethernet_init(ethernet_node_t node_id, DriverFunction driver, OnRecieve /* Create the PTP client instance */ status = nx_ptp_client_create(&device.ptp_client, &device.ip, 0, &device.packet_pool, _PTP_THREAD_PRIORITY, (UCHAR *)&device.ptp_stack, sizeof(device.ptp_stack), - _nx_ptp_client_soft_clock_callback, NX_NULL); + nx_ptp_client_hard_clock_callback, NX_NULL); if(status != NX_SUCCESS) { PRINTLN_ERROR("Failed to create PTP client (Status: %d/%s).", status, nx_status_toString(status)); return status; diff --git a/NetX/src/u_rtc.c b/NetX/src/u_rtc.c new file mode 100644 index 00000000..867d6529 --- /dev/null +++ b/NetX/src/u_rtc.c @@ -0,0 +1,166 @@ +#include "nxd_ptp_client.h" +#include "rtc.h" +#include "stm32h5xx_hal.h" +#include "u_tx_debug.h" +#include + +#define PTP_UTC_OFFSET 0 // UTC 0 +extern RTC_HandleTypeDef hrtc; + +UINT interrupt_save; // do not remove + +static UINT us_to_second_ticks(ULONG us, UINT second_fractions) +{ + // Second fraction = SS / (PREDIV_S + 1) + return (uint64_t)us * (second_fractions + 1) / 1000000L; +} + +static ULONG second_ticks_to_us(UINT second_ticks, UINT second_fractions) +{ + return (uint64_t)1000000L * second_ticks / (second_fractions + 1); +} + +static void set_subsecond(UINT rtc_sub_second_tick, UINT second_fractions, ULONG nanoseconds) +{ + if (rtc_sub_second_tick > second_fractions) { + PRINTLN_ERROR("rtc SS overflow"); + } + + UINT rtp_sub_second_tick = us_to_second_ticks( + nanoseconds / 1000, second_fractions); + + UINT offset_tick = 0; // ticks to go backwards + UINT offset_ahead_1s = RTC_SHIFTADD1S_RESET; + if (rtc_sub_second_tick >= rtp_sub_second_tick) { // local ahead + offset_tick = rtc_sub_second_tick - rtp_sub_second_tick; + } else { // local behind + offset_ahead_1s = RTC_SHIFTADD1S_SET; + UINT delta = rtp_sub_second_tick - rtc_sub_second_tick; + offset_tick = (second_fractions + 1) - delta; + } + HAL_RTCEx_SetSynchroShift(&hrtc, offset_ahead_1s, offset_tick); +} + +static UINT rtc_to_nx_time(RTC_TimeTypeDef *rtc_time, RTC_DateTypeDef *rtc_date, NX_PTP_TIME *time_ptr) { + // helpful way to get UNIX time from RTC so we can give it to NetX layer + time_t current_time_unix = { 0 }; + struct tm tim = {0}; + tim.tm_year = rtc_date->Year + 100; + tim.tm_mon = rtc_date->Month - 1; + tim.tm_mday = rtc_date->Date; + tim.tm_hour = rtc_time->Hours; + tim.tm_min = rtc_time->Minutes; + tim.tm_sec = rtc_time->Seconds; + current_time_unix = mktime(&tim); + + time_ptr->second_high = 0; // todo fix by 2038 + time_ptr->second_low = (ULONG) current_time_unix; + time_ptr->nanosecond = 1000 * second_ticks_to_us( + rtc_time->SecondFraction - rtc_time->SubSeconds, + rtc_time->SecondFraction); // pull directly from rtc + + return 0; +} + +UINT nx_ptp_client_hard_clock_callback(NX_PTP_CLIENT *client_ptr, + UINT operation, NX_PTP_TIME *time_ptr, + NX_PACKET *packet_ptr, + VOID *callback_data) +{ + NX_PTP_DATE_TIME current_date_time = { 0 }; + RTC_TimeTypeDef rtc_time = {0}; + RTC_DateTypeDef rtc_date = {0}; + + switch (operation) { + case NX_PTP_CLIENT_CLOCK_INIT: + break; + + case NX_PTP_CLIENT_CLOCK_SET: + TX_DISABLE + + UINT status = nx_ptp_client_utility_convert_time_to_date( + time_ptr, -PTP_UTC_OFFSET, ¤t_date_time); + + rtc_time = (RTC_TimeTypeDef) { + .Hours = current_date_time.hour, + .Minutes = current_date_time.minute, + .Seconds = current_date_time.second, + .TimeFormat = 0, + }; + + rtc_date = (RTC_DateTypeDef) { + .Year = current_date_time.year % 100, + .Month = current_date_time.month, + .Date = current_date_time.day, + .WeekDay = current_date_time.weekday, + }; + + // PRINTLN_INFO("GOT TIME SET: %d, sending NX time (%lu, %lu) from year %d, month %d, day %d, hour %d, minute %d, second %d", + // status, + // time_ptr->second_high, time_ptr->second_low, + // current_date_time.year, current_date_time.month, current_date_time.day, current_date_time.hour, current_date_time.minute, current_date_time.second); + // PRINTLN_INFO("GOT TIME SET, sending RTC from year %d, month %d, day %d, hour %d, minute %d, second %d", + // rtc_date.Year, rtc_date.Month, rtc_date.Date, rtc_time.Hours, rtc_time.Minutes, rtc_time.Seconds); + + HAL_RTC_SetTime(&hrtc, &rtc_time, RTC_FORMAT_BIN); + HAL_RTC_SetDate(&hrtc, &rtc_date, RTC_FORMAT_BIN); + + // After SetTime, SubSeconds is reset to SecondFraction (0 elapsed). + // Shift from 0 elapsed ticks to wherever PTP nanosecond says we should be. + RTC_TimeTypeDef after_set = { 0 }; + HAL_RTC_GetTime(&hrtc, &after_set, RTC_FORMAT_BIN); // to get a valid SecondFraction + set_subsecond(0, after_set.SecondFraction, time_ptr->nanosecond); + + // dummy get date + HAL_RTC_GetDate(&hrtc, &rtc_date, RTC_FORMAT_BIN); + TX_RESTORE + break; + case NX_PTP_CLIENT_CLOCK_PACKET_TS_EXTRACT: // FALL THROUGH + case NX_PTP_CLIENT_CLOCK_GET: + TX_DISABLE + + HAL_RTC_GetTime(&hrtc, &rtc_time, RTC_FORMAT_BIN); + HAL_RTC_GetDate(&hrtc, &rtc_date, RTC_FORMAT_BIN); + + rtc_to_nx_time(&rtc_time, &rtc_date, time_ptr); + + // PRINTLN_INFO("GOT TIME REQ, recv NX from year %d, month %d, day %d, hour %d, minute %d, second %d", + // rtc_date.Year, rtc_date.Month, rtc_date.Date, rtc_time.Hours, rtc_time.Minutes, rtc_time.Seconds); + + TX_RESTORE + break; + case NX_PTP_CLIENT_CLOCK_ADJUST: + TX_DISABLE + + HAL_RTC_GetTime(&hrtc, &rtc_time, RTC_FORMAT_BIN); + + set_subsecond( + rtc_time.SecondFraction - rtc_time.SubSeconds, + rtc_time.SecondFraction, time_ptr->nanosecond); // (between 0 and PREDIV_S (aka SecondFraction), counting up) + + + // dummy get date + HAL_RTC_GetDate(&hrtc, &rtc_date, RTC_FORMAT_BIN); + + TX_RESTORE + break; + case NX_PTP_CLIENT_CLOCK_PACKET_TS_PREPARE: + HAL_RTC_GetTime(&hrtc, &rtc_time, RTC_FORMAT_BIN); + HAL_RTC_GetDate(&hrtc, &rtc_date, RTC_FORMAT_BIN); + + rtc_to_nx_time(&rtc_time, &rtc_date, time_ptr); + + // PRINTLN_INFO("GOT TIME REQ, recv NX from year %d, month %d, day %d, hour %d, minute %d, second %d", + // rtc_date.Year, rtc_date.Month, rtc_date.Date, rtc_time.Hours, rtc_time.Minutes, rtc_time.Seconds); + + nx_ptp_client_packet_timestamp_notify(client_ptr, packet_ptr, time_ptr); + break; + case NX_PTP_CLIENT_CLOCK_SOFT_TIMER_UPDATE: // do nothing + break; + default: + PRINTLN_ERROR("How Did We Get Here? (rtc.h)"); + break; + } + + return NX_SUCCESS; +}