/*- * Copyright (c) 2016-2017, 2023 Wind River Systems, Inc, * Copyright (c) 2012-2013 Wojciech Owczarek, * Copyright (c) 2011-2012 George V. Neville-Neil, * Steven Kreuzer, * Martin Burnicki, * Jan Breuer, * Gael Mace, * Alexandre Van Kempen, * Inaqui Delgado, * Rick Ratzel, * National Instruments. * Copyright (c) 2009-2010 George V. Neville-Neil, * Steven Kreuzer, * Martin Burnicki, * Jan Breuer, * Gael Mace, * Alexandre Van Kempen * * Copyright (c) 2005-2008 Kendall Correll, Aidan Williams * * All Rights Reserved * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are * met: * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHORS ``AS IS'' AND ANY EXPRESS OR * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * * ==================================================== * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. * * Developed at SunPro, a Sun Microsystems, Inc. business. * Permission to use, copy, modify, and distribute this * software is freely granted, provided that this notice * is preserved. * ==================================================== */ /* modification history -------------------- 17oct23,rjq Fix Y2038 issue. 25jan17,xxy Add support for TSN Clocks and Timers (F6548) 01nov16,myt Add PTP features.(F7726) 15aug16,xxy Add PTP features.(F6696) 02feb16,h_l Init version. */ /** * @file sys.c * @date Tue Jul 20 16:19:46 2010 * * @brief Code to call kernel time routines and also display server statistics. * * */ #include "../ptpd.h" #ifndef _WRS_CONFIG_ARCH_i86 #include #endif #define HAVE_STRUCT_ETHER_ADDR_OCTET #undef fileno #ifdef HAVE_NETINET_ETHER_H # include #endif #ifdef HAVE_NET_ETHERNET_H # include #endif #ifdef HAVE_NET_IF_H # include #endif #ifdef HAVE_NET_IF_ETHER_H # include #endif /* only C99 has the round function built-in */ double round (double __x); int adjtimex(RunTimeOpts *rtOpts, struct timex *t) { if (t->modes & MOD_FREQUENCY) { if (rtOpts->hardTimeStamp) { (void)netAdjustFreq(rtOpts->ethCookie, t->freq); } } return 0; } /* returns a static char * for the representation of time, for debug purposes DO NOT call this twice in the same printf! */ char *dump_TimeInternal(const TimeInternal *p) { static char buf[100]; (void)snprint_TimeInternal(buf, 100, p); return buf; } /* displays 2 timestamps and their strings in sequence, and the difference between then DO NOT call this twice in the same printf! */ char *dump_TimeInternal2(const char *st1, const TimeInternal *p1, const char *st2, const TimeInternal *p2) { static char buf[BUF_SIZE]; int n = 0; /* display Timestamps */ if (st1) { n += snprintf(buf + n, BUF_SIZE - n, "%s ", st1); } n += snprint_TimeInternal(buf + n, BUF_SIZE - n, p1); n += snprintf(buf + n, BUF_SIZE - n, " "); if (st2) { n += snprintf(buf + n, BUF_SIZE - n, "%s ", st2); } n += snprint_TimeInternal(buf + n, BUF_SIZE - n, p2); n += snprintf(buf + n, BUF_SIZE - n, " "); /* display difference */ TimeInternal r; subTime(&r, p1, p2); n += snprintf(buf + n, BUF_SIZE - n, " (diff: "); n += snprint_TimeInternal(buf + n, BUF_SIZE - n, &r); n += snprintf(buf + n, BUF_SIZE - n, ") "); return buf; } int snprint_TimeInternal(char *s, int max_len, const TimeInternal *p) { int len = 0; /* always print either a space, or the leading "-". This makes the stat files columns-aligned */ len += snprintf(&s[len], max_len - len, "%c", isTimeInternalNegative(p) ? '-' : ' '); len += snprintf(&s[len], max_len - len, "%lld.%09d", llabs(p->seconds), abs(p->nanoseconds)); return len; } /* debug aid: convert a time variable into a static char */ char *time2st(const TimeInternal *p) { static char buf[1000]; (void)snprint_TimeInternal(buf, sizeof(buf), p); return buf; } void DBG_time(const char *name, const TimeInternal p) { DBG(" %s: %s\n", name, time2st(&p)); } char * translatePortState(PtpClock *ptpClock) { char *s; switch (ptpClock->portState) { case PTP_INITIALIZING: s = "init"; break; case PTP_FAULTY: s = "flt"; break; case PTP_LISTENING: /* seperate init-reset from real resets */ if (ptpClock->resetCount == 1) { s = "lstn_init"; } else { s = "lstn_reset"; } break; case PTP_PASSIVE: s = "pass"; break; case PTP_UNCALIBRATED: s = "uncl"; break; case PTP_SLAVE: s = "slv"; break; case PTP_PRE_MASTER: s = "pmst"; break; case PTP_MASTER: s = "mst"; break; case PTP_DISABLED: s = "dsbl"; break; default: s = "?"; break; } return s; } int snprint_ClockIdentity(char *s, int max_len, const ClockIdentity id) { int len = 0; int i; for (i = 0; ;) { len += snprintf(&s[len], max_len - len, "%02x", (unsigned char) id[i]); if (++i >= CLOCK_IDENTITY_LENGTH) { break; } } return len; } /* show the mac address in an easy way */ int snprint_ClockIdentity_mac(char *s, int max_len, const ClockIdentity id) { int len = 0; int i; for (i = 0; ;) { /* skip bytes 3 and 4 */ if (!((i == 3) || (i == 4))) { len += snprintf(&s[len], max_len - len, "%02x", (unsigned char) id[i]); if (++i >= CLOCK_IDENTITY_LENGTH) { break; } /* print a separator after each byte except the last one */ len += snprintf(&s[len], max_len - len, "%s", ":"); } else { i++; } } return len; } /* * wrapper that caches the latest value of ether_ntohost * this function will NOT check the last accces time of /etc/ethers, * so it only have different output on a failover or at restart * */ int ether_ntohost_cache(char *hostname, struct ether_addr *addr) { static int valid = 0; static struct ether_addr prev_addr; static char buf[BUF_SIZE]; #ifdef HAVE_STRUCT_ETHER_ADDR_OCTET if (memcmp(addr->octet, &prev_addr, sizeof(struct ether_addr )) != 0) { valid = 0; } #else if (memcmp(addr->ether_addr_octet, &prev_addr, sizeof(struct ether_addr )) != 0) { valid = 0; } #endif if (!valid) { #ifndef VXWORKS_OS if (ether_ntohost(buf, addr)) { snprintf(buf, BUF_SIZE, "%s", "unknown"); } #endif /* clean possible commas from the string */ while (strchr(buf, ',') != NULL) { *(strchr(buf, ',')) = '_'; } prev_addr = *addr; } valid = 1; strncpy(hostname, buf, 100); return 0; } /* Show the hostname configured in /etc/ethers */ int snprint_ClockIdentity_ntohost(char *s, int max_len, const ClockIdentity id) { int len = 0; int i, j; char buf[100]; struct ether_addr e; /* extract mac address */ for (i = 0, j = 0; i < CLOCK_IDENTITY_LENGTH ; i++ ) { /* skip bytes 3 and 4 */ if (!((i == 3) || (i == 4))) { #ifdef HAVE_STRUCT_ETHER_ADDR_OCTET e.octet[j] = (uint8_t) id[i]; #else e.ether_addr_octet[j] = (uint8_t) id[i]; #endif j++; } } /* convert and print hostname */ (void)ether_ntohost_cache(buf, &e); len += snprintf(&s[len], max_len - len, "(%s)", buf); return len; } int snprint_PortIdentity(char *s, int max_len, const PortIdentity *id) { int len = 0; #ifdef PRINT_MAC_ADDRESSES len += snprint_ClockIdentity_mac(&s[len], max_len - len, id->clockIdentity); #else len += snprint_ClockIdentity(&s[len], max_len - len, id->clockIdentity); #endif len += snprint_ClockIdentity_ntohost(&s[len], max_len - len, id->clockIdentity); len += snprintf(&s[len], max_len - len, "/%02x", (unsigned) id->portNumber); return len; } void logStatistics(RunTimeOpts *rtOpts, PtpClock *ptpClock) { return; } void ShowStatistics(RunTimeOpts *rtOpts, PtpClock *ptpClock) { static char sbuf[SCREEN_BUFSZ]; int len = 0; TimeInternal now; time_t time_s; static TimeInternal prev_now_sync, prev_now_delay; char time_str[MAXTIMESTR]; struct tm *tptr; if (rtOpts->debug_level <= PTP_DEBUG_LEVEL_0) { return; } if ((ptpClock->portState == PTP_INITIALIZING) || (ptpClock->portState == PTP_FAULTY) || (ptpClock->portState == PTP_LISTENING)) { return; } if (ptpClock->resetStatisticsLog) { ptpClock->resetStatisticsLog = FALSE; printf("\n# Timestamp, State, Clock ID, One Way Delay, " "Offset From Master, Slave to Master, " "Master to Slave, Observed Drift, Last packet Received\n"); } memset(sbuf, ' ', sizeof(sbuf)); getTime(rtOpts, &now); /* * print one log entry per X seconds for Sync and DelayResp messages, to reduce disk usage. */ if ((ptpClock->portState == PTP_SLAVE) && (rtOpts->statisticsLogInterval)) { switch (ptpClock->char_last_msg) { case 'S': if ((now.seconds - prev_now_sync.seconds) < rtOpts->statisticsLogInterval) { DBGV("Suppressed Sync statistics log entry - statisticsLogInterval configured\n"); return; } prev_now_sync = now; break; case 'D': case 'P': if ((now.seconds - prev_now_delay.seconds) < rtOpts->statisticsLogInterval) { DBGV("Suppressed Sync statistics log entry - statisticsLogInterval configured\n"); return; } prev_now_delay = now; default: break; } } time_s = now.seconds; tptr = localtime(&time_s); if (tptr) { (void)strftime(time_str, MAXTIMESTR, "%Y-%m-%d %X", tptr); len += snprintf(sbuf + len, sizeof(sbuf) - len, "%s.%09ld, %s, ", time_str, now.nanoseconds, /* Timestamp */ translatePortState(ptpClock)); /* State */ } if (ptpClock->portState == PTP_SLAVE) { len += snprint_PortIdentity(sbuf + len, sizeof(sbuf) - len, &ptpClock->parentPortIdentity); /* Clock ID */ /* * if grandmaster ID differs from parent port ID then * also print GM ID */ if (memcmp(ptpClock->grandmasterIdentity, ptpClock->parentPortIdentity.clockIdentity, CLOCK_IDENTITY_LENGTH)) { len += snprint_ClockIdentity(sbuf + len, sizeof(sbuf) - len, ptpClock->grandmasterIdentity); } len += snprintf(sbuf + len, sizeof(sbuf) - len, ", "); if (rtOpts->delayMechanism == E2E) { len += snprint_TimeInternal(sbuf + len, sizeof(sbuf) - len, &ptpClock->meanPathDelay); } else { len += snprint_TimeInternal(sbuf + len, sizeof(sbuf) - len, &ptpClock->peerMeanPathDelay); } len += snprintf(sbuf + len, sizeof(sbuf) - len, ", "); len += snprint_TimeInternal(sbuf + len, sizeof(sbuf) - len, &ptpClock->offsetFromMaster); /* print MS and SM with sign */ len += snprintf(sbuf + len, sizeof(sbuf) - len, ", "); if (rtOpts->delayMechanism == E2E) { len += snprint_TimeInternal(sbuf + len, sizeof(sbuf) - len, &(ptpClock->delaySM)); } else { len += snprint_TimeInternal(sbuf + len, sizeof(sbuf) - len, &(ptpClock->pdelaySM)); } len += snprintf(sbuf + len, sizeof(sbuf) - len, ", "); len += snprint_TimeInternal(sbuf + len, sizeof(sbuf) - len, &(ptpClock->delayMS)); len += snprintf(sbuf + len, sizeof(sbuf) - len, ", %.09f, %c", ptpClock->servo.observedDrift, ptpClock->char_last_msg); } else { if ((ptpClock->portState == PTP_MASTER) || (ptpClock->portState == PTP_PASSIVE)) { len += snprint_PortIdentity(sbuf + len, sizeof(sbuf) - len, &ptpClock->parentPortIdentity); /*len += snprintf(sbuf + len, sizeof(sbuf) - len, ")");*/ } /* show the current reset number on the log */ if (ptpClock->portState == PTP_LISTENING) { len += snprintf(sbuf + len, sizeof(sbuf) - len, " %d ", ptpClock->resetCount); } } /* add final \n in normal status lines */ len += snprintf(sbuf + len, sizeof(sbuf) - len, "\n"); NOTICE("%s", sbuf); } void displayStatus(const RunTimeOpts *rtOpts, PtpClock *ptpClock, const char *prefixMessage) { static char sbuf[SCREEN_BUFSZ]; int len = 0; if (rtOpts->debug_level == PTP_DEBUG_LEVEL_0) { return; } memset(sbuf, ' ', sizeof(sbuf)); len += snprintf(sbuf + len, sizeof(sbuf) - len, "%s", prefixMessage); len += snprintf(sbuf + len, sizeof(sbuf) - len, "%s", portState_getName(ptpClock->portState)); if (ptpClock->portState >= PTP_MASTER) { len += snprintf(sbuf + len, sizeof(sbuf) - len, ", Best master: "); len += snprint_PortIdentity(sbuf + len, sizeof(sbuf) - len, &ptpClock->parentPortIdentity); } if (ptpClock->portState == PTP_MASTER) { len += snprintf(sbuf + len, sizeof(sbuf) - len, " (self)"); } len += snprintf(sbuf + len, sizeof(sbuf) - len, "\n"); NOTICE("%s", sbuf); } void displayPortIdentity(PortIdentity *port, const char *prefixMessage) { static char sbuf[SCREEN_BUFSZ]; int len = 0; memset(sbuf, ' ', sizeof(sbuf)); len += snprintf(sbuf + len, sizeof(sbuf) - len, "%s ", prefixMessage); len += snprint_PortIdentity(sbuf + len, sizeof(sbuf) - len, port); len += snprintf(sbuf + len, sizeof(sbuf) - len, "\n"); INFO("%s", sbuf); } void getTime(RunTimeOpts *rtOpts, TimeInternal *time) { struct timespec ts; if (rtOpts->hardTimeStamp) { (void)netGetSysTime(rtOpts->ethCookie, &ts); } else { (void)clock_gettime(CLOCK_REALTIME, &ts); } time->seconds = (int64_t)ts.tv_sec; time->nanoseconds = ts.tv_nsec; } void setTime(RunTimeOpts *rtOpts, TimeInternal *time) { struct timespec tp; tp.tv_sec = time->seconds; tp.tv_nsec = time->nanoseconds; DBG("\nsetTime called %lds %ldns\n\n", tp.tv_sec, tp.tv_nsec); (void)clock_settime(CLOCK_REALTIME, &tp); if (rtOpts->hardTimeStamp) { (void)netSetSysTime(rtOpts->ethCookie, &tp); } } /* returns a double beween 0.0 and 1.0 */ double getRand(void) { #if 0 unsigned int randValue; /**************************************************************************** * All slaves send delay request at the samesecond which can cause problems for * the master (because it cannot get RX timestamp if packets come in to frequent). * The reason is that getRand() function does not provide any randomness because * there is not enough entropy.Changed by Markus. */ (void)randPseudoBytes(&randValue, 4); return (double)randValue / 0xffffffff + 1.0; #endif return (double)rand()/ 0xffffffff + 1.0; } /* Whole block of adjtimex() functions starts here - only for systems with sys/timex.h */ /* * Apply a tick / frequency shift to the kernel clock */ Boolean adjFreq(RunTimeOpts *rtOpts, double adj) { struct timex t; memset(&t, 0, sizeof(t)); /* Clamp to max PPM */ if (adj > rtOpts->servoMaxPpb) { adj = rtOpts->servoMaxPpb; } else if (adj < -rtOpts->servoMaxPpb) { adj = -rtOpts->servoMaxPpb; } t.modes |= MOD_FREQUENCY; double dFreq = adj ; t.freq = (int) round(dFreq); #ifdef HAVE_STRUCT_TIMEX_TICK DBG2("adjFreq: oldadj: %.09f, newadj: %.09f, tick: %d, tickadj: %d\n", oldAdj, adj, t.tick, tickAdj); #endif /* HAVE_STRUCT_TIMEX_TICK */ DBG2(" adj is %.09f; t freq is %d (float: %.09f)\n", adj, t.freq, dFreq); return !adjtimex(rtOpts, &t); } double getAdjFreq(void) { double dFreq = 0.0; return (dFreq); } #define DRIFTFORMAT "%.0f" void restoreDrift(PtpClock *ptpClock, RunTimeOpts *rtOpts, Boolean quiet) { Boolean reset_offset = FALSE; double recovered_drift; DBGV("restoreDrift called\n"); if (ptpClock->drift_saved && rtOpts->drift_recovery_method > 0 ) { ptpClock->servo.observedDrift = ptpClock->last_saved_drift; if (!rtOpts->noAdjust) { adjFreq_wrapper(rtOpts, ptpClock, -ptpClock->last_saved_drift); } DBG("loaded cached drift"); return; } switch (rtOpts->drift_recovery_method) { case DRIFT_KERNEL: recovered_drift = -getAdjFreq(); if (recovered_drift == 0) { recovered_drift = 0; } if (quiet) DBGV("Observed_drift loaded from kernel: "DRIFTFORMAT"\n", recovered_drift); else if (rtOpts->debug_level > PTP_DEBUG_LEVEL_0) INFO("Observed_drift loaded from kernel: "DRIFTFORMAT"\n", recovered_drift); break; default: reset_offset = TRUE; } if (reset_offset) { if (!rtOpts->noAdjust) { adjFreq_wrapper(rtOpts, ptpClock, 0); } ptpClock->servo.observedDrift = 0; return; } ptpClock->servo.observedDrift = recovered_drift; ptpClock->drift_saved = TRUE; ptpClock->last_saved_drift = recovered_drift; if (!rtOpts->noAdjust) { if (rtOpts->debug_level > PTP_DEBUG_LEVEL_3) { (void)printf("restoreDrift adjFreq\n"); } (void)adjFreq(rtOpts, -recovered_drift); } } #undef DRIFTFORMAT void saveDrift(PtpClock *ptpClock, RunTimeOpts *rtOpts, Boolean quiet) { FILE *driftFP; DBGV("saveDrift called\n"); if (ptpClock->portState == PTP_PASSIVE || ptpClock->portState == PTP_MASTER || ptpClock->clockQuality.clockClass < 128) { DBGV("We're not slave - not saving drift\n"); return; } if (ptpClock->servo.observedDrift == 0.0 && ptpClock->portState == PTP_LISTENING ) { return; } if (rtOpts->drift_recovery_method > 0) { ptpClock->last_saved_drift = ptpClock->servo.observedDrift; ptpClock->drift_saved = TRUE; } } void setTimexFlags(int flags, Boolean quiet) {} /* First cut on informing the clock */ void informClockSource(PtpClock *ptpClock) { return; } void unsetTimexFlags(int flags, Boolean quiet) {} int getTimexFlags(void) { return (-1); } Boolean checkTimexFlags(int flags) { int tflags = getTimexFlags(); if (tflags == -1) { return FALSE; } return ((tflags & flags) == flags); } /* * TODO: track NTP API changes - NTP API version check * is required - the method of setting the TAI offset * may change with next API versions */ void adjTime(Integer32 nanoseconds) { struct timespec tp; (void)clock_gettime(CLOCK_REALTIME, &tp); tp.tv_nsec = tp.tv_nsec + nanoseconds; if (tp.tv_nsec < 0) { tp.tv_sec--; tp.tv_nsec += 1000000000; } else if (tp.tv_nsec > 1000000000) { tp.tv_sec++; tp.tv_nsec -= 1000000000; } (void)clock_settime(CLOCK_REALTIME, &tp); } #ifndef _WRS_CONFIG_ARCH_i86 /* trunc() is used to get the integer field but cut off the fraction fields for 'x'. */ double trunc(double x) { int signbit; int msw; unsigned int lsw; int exponent_less_1023; EXTRACT_WORDS(msw, lsw, x); /* Extract sign bit. */ signbit = msw & 0x80000000; /* Extract exponent field. The exponent field is 11 bit. */ exponent_less_1023 = ((msw & 0x7ff00000) >> 20) - 1023; /* msw is 32 bit, the sign bit plus exponent field is 12 bit, so the left 20 bit is mantissa */ if (exponent_less_1023 < 20) { /* All significant digits are in msw. */ if (exponent_less_1023 < 0) { /* -1 < x < 1, so result is +0 or -0. */ INSERT_WORDS(x, signbit, 0); } else { /* All relevant fraction bits are in msw, so lsw of the result is 0. */ INSERT_WORDS(x, signbit | (msw & ~(0x000fffff >> exponent_less_1023)), 0); } } else if (exponent_less_1023 > 51) { if (exponent_less_1023 == 1024) { /* x is infinite, or not a number, so trigger an exception. */ return x + x; } /* All bits in the fraction fields of the msw and lsw are needed in the result. */ } else /* "exponent_less_1023 > 20" this means the fraction fields is in lsw */ { /* All fraction bits in msw are relevant. Truncate irrelevant bits from lsw. */ INSERT_WORDS(x, msw, lsw & ~(0xffffffffu >> (exponent_less_1023 - 20))); } return x; } /* * round() is used to make the 'x' to be rounded to the nearest integer. */ double round(double x) { int msw; int exponent_less_1023; unsigned int lsw; unsigned int exponent_mask; unsigned int tmp; EXTRACT_WORDS(msw, lsw, x); /* Extract exponent field. */ exponent_less_1023 = ((msw & 0x7ff00000) >> 20) - 1023; if (exponent_less_1023 < 20) { if (exponent_less_1023 < 0) { msw &= 0x80000000; if (exponent_less_1023 == -1) { /* This means the x is +0.5 or -0.5, so result is +1.0 or -1.0. */ msw |= (1023 << 20); } lsw = 0; } else { exponent_mask = 0x000fffff >> exponent_less_1023; if ((msw & exponent_mask) == 0 && lsw == 0) { /* x in an integral value. */ return x; } /* The fraction bits plus 1, if the fraction field >= 0.5, then +1 will make the integer * also +1, else if the fraction field < 0.5, then +1 will not make the integer +1. e.g. to 0.625 * the binary value is 0.101, +1 will be 1.001, so the value of round is 1. To 0.375, the binary * value is 0.011. +1 will be 0.111, so the value of round is 0. */ msw += 0x00080000 >> exponent_less_1023; msw &= ~exponent_mask; lsw = 0; } } else if (exponent_less_1023 > 51) { if (exponent_less_1023 == 1024) { /* x is NaN or infinite. */ return x + x; } else { return x; } } else { exponent_mask = 0xffffffff >> (exponent_less_1023 - 20); if ((lsw & exponent_mask) == 0) { /* x is an integral value. */ return x; } tmp = lsw + (1 << (51 - exponent_less_1023)); if (tmp < lsw) { msw += 1; } lsw = tmp; lsw &= ~exponent_mask; } INSERT_WORDS(x, msw, lsw); return x; } #endif