bjayesh/m241Neelima
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
baa4eb02636fc266749780c4b1ed4b017b96de36
m241Neelima/components/ip_net2-6.9/ipptp/src/ptpd.c
T
neelimap afc7070c1f original files of m241
2026-07-30 11:19:44 +05:30

797 lines
21 KiB
C
Raw Blame History

/*-
* 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 <muxLib.h>
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;
}
Reference in New Issue View Git Blame Copy Permalink