1051 lines
25 KiB
C
1051 lines
25 KiB
C
/*-
|
|
* 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 <ctype.h>
|
|
#endif
|
|
|
|
#define HAVE_STRUCT_ETHER_ADDR_OCTET
|
|
#undef fileno
|
|
|
|
#ifdef HAVE_NETINET_ETHER_H
|
|
# include <netinet/ether.h>
|
|
#endif
|
|
|
|
#ifdef HAVE_NET_ETHERNET_H
|
|
# include <net/ethernet.h>
|
|
#endif
|
|
|
|
#ifdef HAVE_NET_IF_H
|
|
# include <net/if.h>
|
|
#endif
|
|
|
|
#ifdef HAVE_NET_IF_ETHER_H
|
|
# include <net/if_ether.h>
|
|
#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
|
|
|
|
|