/*- * 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. */ /* modification history -------------------- 17oct23,rjq Fix Y2038 issue. 13apr17,xxy Add PTP task priority to vip kernel configuration (V7NET-827) 23feb17,xxy Fix Missing BMCA support (V7NET-1274) 09feb17,xxy Fix float point unavailable exception (V7NET-1263) 25jan17,xxy Add support for TSN Clocks and Timers (F6548) 12dec16,myt fix spe unavailable exception (V7NET-1215) 19aug16,xxy Fix overflow in PTP servo.(V7NET-1086) 15aug16,xxy Add PTP features.(F6696) 19jul16,xxy Add PTPD API documentation (V7NET-879) 15jun16,xxy Fix 802.1AS error 02feb16,h_l Init version. */ /** * @file ptpd.c * @date Wed Jun 23 10:13:38 2010 * * @brief The main() function for the PTP daemon * * This file contains very little code, as should be obvious, * and only serves to tie together the rest of the daemon. * All of the default options are set here, but command line * arguments and configuration file is processed in the * ptpdStartup() routine called * below. */ #include "ipptp_config.h" #include "ptpd.h" #include extern int sysClkRateGet (void); typedef struct { int slaveMaster; /*0 slave 1 master*/ RunTimeOpts rtOpt; PtpClock *ptpClock; } ptp_thread_info; typedef struct { int threadNum; ptp_thread_info threadInfo[8]; PtpClock *ptpClock; char mainTimeEthString[16]; int mainTimeEthIndex; void *mainTimeEthCookie; int mainTimeReadCost; } ptp_info; static ptp_info ptpInfo; /* * Global variable with the main PTP port. This is used to show the current state in DBG()/message() * without having to pass the pointer everytime. * * if ptpd is extended to handle multiple ports (eg, to instantiate a Boundary Clock), * then DBG()/message() needs a per-port pointer argument */ static int ptpdInited = 0; int ptpTaskPriority = PTP_TASK_PRIORITY; static void ptpd(int threadIndex); static void ptpdSyncTime(int threadNum); #ifdef VXWORKS_OS #include "vxWorks.h" #include "netBufLib.h" #endif int64_t llabs(int64_t i) { return (i < 0 ? -i : i); } /****************************************************************************** * * ptpdAdd - bind ptpd to ethernet interface * * This function is used to bind ptpd to different ethernet interfaces. * ethName ethernet interface name you want to use * layer transport layer PTP_DEVICE_LAYER_2 or PTP_DEVICE_LAYER_4 * slaveMaster 0=SLAVE, 1=MASTER, 2 (others)= BCMA * delayMechanism PTP_DEVICE_E2E or PTP_DEVICE_P2P * oneStepMode PTP_DEVICE_TWO_STEP or PTP_DEVICE_ONE_STEP * pri PTP Priority When Configured as Master Clock * PdelayReq PTP Pdelay Request Interval,reciprocal of N times of power of 2, * (1 ~ -4) means (0.5PPS ~ 16PPS) * profile Select ptp profile PTP_802_1AS_PROFILE or PTP_DEFAULT_PROFILE * RETURNS: PTP_OK or PTP_ERROR * * ERRNO: N/A */ int ptpdAdd ( char *pEthName, int layer, int slaveMaster, int delayMechanism, int oneStepMode, unsigned char pri, char PdelayReq, int profile ) { int i, ethIndex; RunTimeOpts *rtOpts; struct timespec ts1; long offset1; char ethName[16]; if (profile == PTP_802_1AS_PROFILE) { if (layer != PTP_DEVICE_LAYER_2) { (void)printf("IEEE802.1AS only supports 2 layer\n"); return PTP_ERROR; } if (delayMechanism != PTP_DEVICE_P2P) { (void)printf("IEEE802.1AS only supports P2P mode\n"); return PTP_ERROR; } } else/* PTP DEFAULT PROFILE */ { if ((layer != PTP_DEVICE_LAYER_2) && (layer != PTP_DEVICE_LAYER_4)) { (void)printf("IEEE1588 only supports 2 or 4 layer\n"); return PTP_ERROR; } if ((layer == PTP_DEVICE_LAYER_4) && (oneStepMode != PTP_DEVICE_TWO_STEP)) { (void)printf("IEEE1588 only supports two-step mode under 4 layer\n"); return PTP_ERROR; } } if (ptpdInited == 0) { memset(&ptpInfo, 0, sizeof(ptp_info)); ptpdInited = 1; } if (ptpInfo.threadNum > PTP_MAX_THREADS) { (void)printf("Achieve max ptp thread number.\n"); return PTP_ERROR; } rtOpts = &ptpInfo.threadInfo[ptpInfo.threadNum].rtOpt; loadDefaultSettings(rtOpts); if (slaveMaster == PTP_DEVICE_MASTER) /*master*/ { rtOpts->priority1 = pri; rtOpts->priority2 = pri; rtOpts->servoKP = 0.1; rtOpts->servoKI = 0.1; } if (profile == PTP_802_1AS_PROFILE) { rtOpts->delayMechanism = P2P; rtOpts->ptp_profile = PTP_802_1AS_PROFILE; if (slaveMaster == PTP_DEVICE_SLAVE) /*slave*/ { rtOpts->logMinPdelayReqInterval = (Integer8)PdelayReq; } else/*master or BCMA*/ { rtOpts->announceInterval = (Integer8)ANNOUNCE_INTERVAL_802_1AS; rtOpts->syncInterval = (Integer8)SYNC_INTERVAL_802_1AS; rtOpts->logMinPdelayReqInterval = (Integer8)PDELAYREQ_INTERVAL_802_1AS; } } i = (int)strlen(pEthName); if (i < 15) { strcpy(ethName, pEthName); strcpy(rtOpts->primaryIfaceName, ethName); ethIndex = ethName[i - 1] - 48; ethName[i - 1] = '\0'; strcpy(rtOpts->ethName, ethName); rtOpts->ethIndex = ethIndex; rtOpts->ethCookie = (void *)endFindByName(ethName, ethIndex); if (rtOpts->ethCookie == NULL) { (void)printf("Interface %s%d does not exist.\n", ethName, ethIndex); return PTP_ERROR; } } else { (void)printf("Interface name too long.\n"); return PTP_ERROR; } if (slaveMaster == PTP_DEVICE_SLAVE) /*slave*/ { for (i = 0; i < ptpInfo.threadNum; i++) { if (ptpInfo.threadInfo[i].slaveMaster == PTP_DEVICE_SLAVE) { (void)printf("Only one slave allowed.\n"); return PTP_ERROR; } } } ptpInfo.threadInfo[ptpInfo.threadNum].slaveMaster = slaveMaster; if (layer == PTP_DEVICE_LAYER_2) { rtOpts->transport = IEEE_802_3; } else { rtOpts->transport = UDP_IPV4; } if (oneStepMode == PTP_DEVICE_ONE_STEP) { if ((netGetHwOneStepCap(rtOpts->ethCookie) == PTP_TRUE) && (rtOpts->transport == IEEE_802_3)) { rtOpts->oneStepMode = TRUE; /*use one step mode*/ } else { (void)printf("Warning: one-step sync not supported on %s%d.\n", ethName, ethIndex); } } if (netGetHwTimeStampCap(rtOpts->ethCookie) == PTP_TRUE) { (void)clock_gettime(CLOCK_REALTIME, &ts1); (void)netSetSysTime(rtOpts->ethCookie, &ts1); if (netEnableHwTimeStamp(rtOpts->ethCookie, 1) == PTP_ERROR) { (void)printf("netEnableHwTimeStamp fail.\n"); return PTP_ERROR; } ptpInfo.threadInfo[ptpInfo.threadNum].rtOpt.hardTimeStamp = PTP_TRUE; if (slaveMaster == PTP_DEVICE_SLAVE) { strcpy(ptpInfo.mainTimeEthString, ethName); ptpInfo.mainTimeEthIndex = ethIndex; ptpInfo.mainTimeEthCookie = rtOpts->ethCookie; (void)netGetSysSyncDelayTime(ptpInfo.mainTimeEthCookie, &offset1); ptpInfo.mainTimeReadCost = (int)offset1; } } else { (void)printf("Warning: Hardware timestamp not supported on %s%d.\n", ethName, ethIndex); ptpInfo.threadInfo[ptpInfo.threadNum].rtOpt.hardTimeStamp = PTP_FALSE; } ptpInfo.threadNum++; return PTP_OK; } /****************************************************************************** * * ptpdClearAll - clear all ptpd attached ethernet interfaces * * This function is used to clear all ptpd attached ethernet interfaces. * * RETURNS: N/A * * ERRNO: N/A */ void ptpdClearAll() { ptpInfo.threadNum = 0; ptpdInited = 0; } /****************************************************************************** * * ptpdTaskUpdate - specified ptpd thread update * * This function is used to update specified ptpd thread. * * RETURNS: N/A * * ERRNO: N/A */ static void ptpdTaskUpdate(int threadNum) { RunTimeOpts *rtOpts; PtpClock *ptpClock; int i = threadNum; rtOpts = &ptpInfo.threadInfo[i].rtOpt; ptpClock = ptpInfo.threadInfo[i].ptpClock; do { if (!rtOpts->runningFlag) { break; } ptpdSyncTime(i); ShowStatistics(rtOpts, ptpClock); (void)taskDelay(sysClkRateGet()); } while (1); return; } /****************************************************************************** * * ptpdStartClock - start ptpd task * * This function is used to start single ptpd on interface. * * RETURNS: N/A * * ERRNO: N/A */ void ptpdStartClock ( char *ethName ) { int i; char name[48]; i = (int)strlen(ethName); if (i < 2) { return; } for (i = 0; i < ptpInfo.threadNum; i++) { if (strcmp(ptpInfo.threadInfo[i].rtOpt.primaryIfaceName, ethName) == 0) { (void)snprintf(name, 48, "ptpd-%s", ptpInfo.threadInfo[i].rtOpt.primaryIfaceName); taskSpawn (name, ptpTaskPriority, #ifdef _WRS_CONFIG_ARCH_ppc VX_FP_TASK | VX_SPE_TASK , #else VX_FP_TASK, #endif 8 * 1024, (FUNCPTR) ptpd, ((_Vx_usr_arg_t)i), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0)); break; } } } /****************************************************************************** * * ptpdStart - start ptpd tasks * * This function is used to start all ptpd on interfaces. * * RETURNS: N/A * * ERRNO: N/A */ void ptpdStart() { int i; char name[48]; if (ptpdInited == 0 || ptpInfo.threadNum == 0) { (void)printf("You need call ptpdAdd first\n"); return; } for (i = 0; i < ptpInfo.threadNum; i++) { (void)snprintf(name, 48, "ptpd-%s", ptpInfo.threadInfo[i].rtOpt.primaryIfaceName); taskSpawn (name, ptpTaskPriority, #ifdef _WRS_CONFIG_ARCH_ppc VX_FP_TASK | VX_SPE_TASK , #else VX_FP_TASK, #endif 8 * 1024, (FUNCPTR) ptpd, ((_Vx_usr_arg_t)i), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0)); } } /****************************************************************************** * * ptpdStop - stop ptpd interface * * This function is used to stop specified ptpd interface. * * ethName ethernet interface name you want to stop * * RETURNS: N/A * * ERRNO: N/A */ void ptpdStop(char *ethName) { int i; for (i = 0; i < ptpInfo.threadNum; i++) { if (strcmp(ptpInfo.threadInfo[i].rtOpt.primaryIfaceName, ethName) == 0) { ptpInfo.threadInfo[i].rtOpt.debug_level = PTP_DEBUG_LEVEL_0; ptpInfo.threadInfo[i].rtOpt.runningFlag = 0; break; } } } /****************************************************************************** * * ptpdClockShow - show all data sets and counters of PtpClock * * This function is used to show all data sets and counters of PtpClock. * * RETURNS: N/A * * ERRNO: N/A */ void ptpdClockShow() { int i; for (i = 0; i < ptpInfo.threadNum; i++) { displayPtpClock(ptpInfo.threadInfo[i].ptpClock); } } /****************************************************************************** * * ptpdRuntimeOptsShow - show all ptpd run time Options structure * * This function is used to show all ptpd run time Options structure. * * RETURNS: N/A * * ERRNO: N/A */ void ptpdRuntimeOptsShow() { int i; for (i = 0; i < ptpInfo.threadNum; i++) { displayRunTimeOpts(&ptpInfo.threadInfo[i].rtOpt); } } /****************************************************************************** * * ptpdSetDebugLevel - set ptp threads debug level * * This function is used to track log output on different ethernet card. * ethName ethernet interface name you want to use, eg "gei0" * level 0 to 6 * * RETURNS: N/A * * ERRNO: N/A */ void ptpdSetDebugLevel ( char *ethName, int level ) { int i; i = (int)strlen(ethName); if (i < 2) { return; } for (i = 0; i < ptpInfo.threadNum; i++) { if (strcmp(ptpInfo.threadInfo[i].rtOpt.primaryIfaceName, ethName) == 0) { ptpInfo.threadInfo[i].rtOpt.debug_level = level; break; } } } /****************************************************************************** * * ptpdGetDebugLevel - get ptp threads debug level * * This function is used to get ptp threads debug level on different card. * ethName ethernet interface name you want to use, eg "gei0" * * RETURNS: debug level * * ERRNO: N/A */ int ptpdGetDebugLevel(char *ethName) { int i; for (i = 0; i < ptpInfo.threadNum; i++) { if (strcmp(ptpInfo.threadInfo[i].rtOpt.primaryIfaceName, ethName) == 0) { return ptpInfo.threadInfo[i].rtOpt.debug_level; } } return PTP_ERROR; } /****************************************************************************** * * ptpd - ptp thread * * This function is used to run ptp state machine * * RETURNS: N/A * * ERRNO: N/A */ static void ptpd(int threadIndex) { PtpClock *ptpClock; int i; int ethNameLen; char name[48]; /*start master later if slave exists*/ if (ptpInfo.threadInfo[threadIndex].slaveMaster == PTP_DEVICE_MASTER) { for (i = 0; i < ptpInfo.threadNum; i++) { if (ptpInfo.threadInfo[i].slaveMaster == PTP_DEVICE_SLAVE) { (void)taskDelay(3 * sysClkRateGet()); } } } ptpClock = ptpdStartup(ptpInfo.threadInfo[threadIndex].slaveMaster, &ptpInfo.threadInfo[threadIndex].rtOpt); if (ptpClock == NULL) { return; } ptpInfo.threadInfo[threadIndex].ptpClock = ptpClock; ethNameLen = sizeof(ptpClock->netPath.ethName); strncpy(ptpClock->netPath.ethName, ptpInfo.threadInfo[threadIndex].rtOpt.ethName, ethNameLen); ptpClock->netPath.ethName[ethNameLen - 1] = 0; ptpClock->netPath.ethIndex = ptpInfo.threadInfo[threadIndex].rtOpt.ethIndex; ptpInfo.threadInfo[threadIndex].rtOpt.runningFlag = TRUE; (void)snprintf(name, 48, "ptpdMon-%s", ptpInfo.threadInfo[threadIndex].rtOpt.ethName); taskSpawn (name, (ptpTaskPriority+2), #ifdef _WRS_CONFIG_ARCH_ppc VX_FP_TASK | VX_SPE_TASK , #else VX_FP_TASK, #endif 4 * 1024, (FUNCPTR) ptpdTaskUpdate, ((_Vx_usr_arg_t)threadIndex), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0), ((_Vx_usr_arg_t)0)); /* do the protocol engine */ protocol(&ptpInfo.threadInfo[threadIndex].rtOpt, ptpClock); /* forever loop.. */ ptpdShutdown(&ptpInfo.threadInfo[threadIndex].rtOpt, ptpClock); return ; } /****************************************************************************** * * ptpdSyncTime - ptp sync with multi cards and OS time * * This function is used to sync with boundary clock and OS time * * RETURNS: N/A * * ERRNO: N/A */ static void ptpdSyncTime(int threadNum) { long offset = 0; time_t nsMain = 0; time_t nsReal = 0; IEEE1588_TIMER_SYNC sync; double adj; RunTimeOpts *rtOpts; PtpClock *ptpClock; struct timespec mainTime; struct timespec realTime; offset_from_master_filter *ofm_filt; double dFreq; int i = threadNum; if ((ptpInfo.threadNum < PTP_MIN_BUN_THREADS) || strlen(ptpInfo.mainTimeEthString) == 0) { return; } if (ptpInfo.threadInfo[i].slaveMaster == PTP_DEVICE_MASTER) /*master*/ { rtOpts = &ptpInfo.threadInfo[i].rtOpt; ptpClock = ptpInfo.threadInfo[i].ptpClock; if (rtOpts->hardTimeStamp) { sync.ethCookie = rtOpts->ethCookie; sync.interval = &offset; (void)netGetSysIntervalTime(ptpInfo.mainTimeEthCookie, &sync); /*this is what we cost by calling netGetSysTime*/ offset = offset - ptpInfo.mainTimeReadCost; if ((offset > 10000000) || (offset < -10000000)) { if (rtOpts->debug_level >= PTP_DEBUG_LEVEL_2) (void)printf("sync clock on %s, offset %ldns,ReadCost %dns\n", rtOpts->primaryIfaceName, offset, ptpInfo.mainTimeReadCost); offset = ptpInfo.mainTimeReadCost; (void)netSetSysIntervalTime(ptpInfo.mainTimeEthCookie, &sync); } else { ofm_filt = &ptpClock->ofm_filt; ofm_filt->y = (Integer32)offset / 2 + ofm_filt->nsec_prev / 2; ofm_filt->nsec_prev = (Integer32)offset; offset = ofm_filt->y; adj = runPIservo(rtOpts, &ptpInfo.threadInfo[i].ptpClock->servo, (Integer32)offset); if (adj > rtOpts->servoMaxPpb) { adj = rtOpts->servoMaxPpb; } else if (adj < -rtOpts->servoMaxPpb) { adj = -rtOpts->servoMaxPpb; } dFreq = adj ; (void)netAdjustFreq(rtOpts->ethCookie, (int)round(dFreq)); if (rtOpts->debug_level >= PTP_DEBUG_LEVEL_2) (void)printf("sync clock on %s, offset %ld , freq adjust %d\n", rtOpts->primaryIfaceName, offset, (int)round(dFreq)); } } } else if (ptpInfo.threadInfo[i].slaveMaster == PTP_DEVICE_SLAVE) /* slave */ { rtOpts = &ptpInfo.threadInfo[i].rtOpt; ptpClock = ptpInfo.threadInfo[i].ptpClock; (void)clock_gettime(CLOCK_REALTIME, &realTime); (void)netGetSysTime(ptpInfo.mainTimeEthCookie, &mainTime); (void)clock_settime(CLOCK_REALTIME, &mainTime); if (rtOpts->debug_level >= PTP_DEBUG_LEVEL_2) { nsMain= mainTime.tv_sec * 1000000000 + mainTime.tv_nsec ; nsReal= realTime.tv_sec * 1000000000 + realTime.tv_nsec ; offset= (long)(nsMain - nsReal); (void)printf("OS real time offset from PTP time: %ld ns\n",offset); } } return; }