mirror of
https://gitee.com/Vancouver2017/luban-lite-t3e-pro.git
synced 2025-12-14 18:38:55 +00:00
1468 lines
36 KiB
C
1468 lines
36 KiB
C
|
/* protocol.c */
|
||
|
|
|
||
|
|
#include "ptpd.h"
|
||
|
|
|
||
|
|
static void handle(PtpClock*);
|
||
|
|
static void handleAnnounce(PtpClock*, bool);
|
||
|
|
static void handleSync(PtpClock*, TimeInternal*, bool);
|
||
|
|
static void handleFollowUp(PtpClock*, bool);
|
||
|
|
static void handlePDelayReq(PtpClock*, TimeInternal*, bool);
|
||
|
|
static void handleDelayReq(PtpClock*, TimeInternal*, bool);
|
||
|
|
static void handlePDelayResp(PtpClock*, TimeInternal*, bool);
|
||
|
|
static void handleDelayResp(PtpClock*, bool);
|
||
|
|
static void handlePDelayRespFollowUp(PtpClock*, bool);
|
||
|
|
static void handleManagement(PtpClock*, bool);
|
||
|
|
static void handleSignaling(PtpClock*, bool);
|
||
|
|
|
||
|
|
static void issueDelayReqTimerExpired(PtpClock*);
|
||
|
|
static void issueAnnounce(PtpClock*);
|
||
|
|
static void issueSync(PtpClock*);
|
||
|
|
static void issueFollowup(PtpClock*, const TimeInternal*);
|
||
|
|
static void issueDelayReq(PtpClock*);
|
||
|
|
static void issueDelayResp(PtpClock*, const TimeInternal*, const MsgHeader*);
|
||
|
|
static void issuePDelayReq(PtpClock*);
|
||
|
|
static void issuePDelayResp(PtpClock*, TimeInternal*, const MsgHeader*);
|
||
|
|
static void issuePDelayRespFollowUp(PtpClock*, const TimeInternal*, const MsgHeader*);
|
||
|
|
//static void issueManagement(const MsgHeader*,MsgManagement*,PtpClock*);
|
||
|
|
|
||
|
|
static bool doInit(PtpClock*);
|
||
|
|
|
||
|
|
#ifdef PTPD_DBG
|
||
|
|
static char *stateString(uint8_t state)
|
||
|
|
{
|
||
|
|
switch (state)
|
||
|
|
{
|
||
|
|
case PTP_INITIALIZING: return (char *) "PTP_INITIALIZING";
|
||
|
|
case PTP_FAULTY: return (char *) "PTP_FAULTY";
|
||
|
|
case PTP_DISABLED: return (char *) "PTP_DISABLED";
|
||
|
|
case PTP_LISTENING: return (char *) "PTP_LISTENING";
|
||
|
|
case PTP_PRE_MASTER: return (char *) "PTP_PRE_MASTER";
|
||
|
|
case PTP_MASTER: return (char *) "PTP_MASTER";
|
||
|
|
case PTP_PASSIVE: return (char *) "PTP_PASSIVE";
|
||
|
|
case PTP_UNCALIBRATED: return (char *) "PTP_UNCALIBRATED";
|
||
|
|
case PTP_SLAVE: return (char *) "PTP_SLAVE";
|
||
|
|
default: break;
|
||
|
|
}
|
||
|
|
return (char *) "UNKNOWN";
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
/* Perform actions required when leaving 'port_state' and entering 'state' */
|
||
|
|
void toState(PtpClock *ptpClock, uint8_t state)
|
||
|
|
{
|
||
|
|
ptpClock->messageActivity = TRUE;
|
||
|
|
|
||
|
|
DBG("leaving state %s\n", stateString(ptpClock->portDS.portState));
|
||
|
|
|
||
|
|
/* leaving state tasks */
|
||
|
|
switch (ptpClock->portDS.portState)
|
||
|
|
{
|
||
|
|
case PTP_MASTER:
|
||
|
|
|
||
|
|
initClock(ptpClock);
|
||
|
|
timerStop(SYNC_INTERVAL_TIMER);
|
||
|
|
timerStop(ANNOUNCE_INTERVAL_TIMER);
|
||
|
|
timerStop(PDELAYREQ_INTERVAL_TIMER);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_UNCALIBRATED:
|
||
|
|
case PTP_SLAVE:
|
||
|
|
|
||
|
|
if (state == PTP_UNCALIBRATED || state == PTP_SLAVE)
|
||
|
|
{
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
timerStop(ANNOUNCE_RECEIPT_TIMER);
|
||
|
|
switch (ptpClock->portDS.delayMechanism)
|
||
|
|
{
|
||
|
|
case E2E:
|
||
|
|
timerStop(DELAYREQ_INTERVAL_TIMER);
|
||
|
|
break;
|
||
|
|
case P2P:
|
||
|
|
timerStop(PDELAYREQ_INTERVAL_TIMER);
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
/* none */
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
initClock(ptpClock);
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_PASSIVE:
|
||
|
|
|
||
|
|
initClock(ptpClock);
|
||
|
|
timerStop(PDELAYREQ_INTERVAL_TIMER);
|
||
|
|
timerStop(ANNOUNCE_RECEIPT_TIMER);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_LISTENING:
|
||
|
|
|
||
|
|
initClock(ptpClock);
|
||
|
|
timerStop(ANNOUNCE_RECEIPT_TIMER);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_PRE_MASTER:
|
||
|
|
|
||
|
|
initClock(ptpClock);
|
||
|
|
timerStop(QUALIFICATION_TIMEOUT);
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
DBG("entering state %s\n", stateString(state));
|
||
|
|
|
||
|
|
/* Entering state tasks */
|
||
|
|
switch (state)
|
||
|
|
{
|
||
|
|
case PTP_INITIALIZING:
|
||
|
|
|
||
|
|
ptpClock->portDS.portState = PTP_INITIALIZING;
|
||
|
|
ptpClock->recommendedState = PTP_INITIALIZING;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_FAULTY:
|
||
|
|
|
||
|
|
ptpClock->portDS.portState = PTP_FAULTY;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_DISABLED:
|
||
|
|
|
||
|
|
ptpClock->portDS.portState = PTP_DISABLED;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_LISTENING:
|
||
|
|
|
||
|
|
timerStart(ANNOUNCE_RECEIPT_TIMER, (ptpClock->portDS.announceReceiptTimeout) * (pow2ms(ptpClock->portDS.logAnnounceInterval)));
|
||
|
|
ptpClock->portDS.portState = PTP_LISTENING;
|
||
|
|
ptpClock->recommendedState = PTP_LISTENING;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_PRE_MASTER:
|
||
|
|
|
||
|
|
/* If you implement not ordinary clock, you can manage this code */
|
||
|
|
/* timerStart(QUALIFICATION_TIMEOUT, pow2ms(DEFAULT_QUALIFICATION_TIMEOUT));
|
||
|
|
ptpClock->portDS.portState = PTP_PRE_MASTER;
|
||
|
|
break;
|
||
|
|
*/
|
||
|
|
|
||
|
|
case PTP_MASTER:
|
||
|
|
|
||
|
|
ptpClock->portDS.logMinDelayReqInterval = DEFAULT_DELAYREQ_INTERVAL; /* it may change during slave state */
|
||
|
|
timerStart(SYNC_INTERVAL_TIMER, pow2ms(ptpClock->portDS.logSyncInterval));
|
||
|
|
DBG("SYNC INTERVAL TIMER : %d \n", pow2ms(ptpClock->portDS.logSyncInterval));
|
||
|
|
timerStart(ANNOUNCE_INTERVAL_TIMER, pow2ms(ptpClock->portDS.logAnnounceInterval));
|
||
|
|
|
||
|
|
switch (ptpClock->portDS.delayMechanism)
|
||
|
|
{
|
||
|
|
case E2E:
|
||
|
|
/* none */
|
||
|
|
break;
|
||
|
|
case P2P:
|
||
|
|
timerStart(PDELAYREQ_INTERVAL_TIMER, getRand(pow2ms(ptpClock->portDS.logMinPdelayReqInterval) + 1));
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
ptpClock->portDS.portState = PTP_MASTER;
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_PASSIVE:
|
||
|
|
|
||
|
|
timerStart(ANNOUNCE_RECEIPT_TIMER, (ptpClock->portDS.announceReceiptTimeout)*(pow2ms(ptpClock->portDS.logAnnounceInterval)));
|
||
|
|
if (ptpClock->portDS.delayMechanism == P2P)
|
||
|
|
{
|
||
|
|
timerStart(PDELAYREQ_INTERVAL_TIMER, getRand(pow2ms(ptpClock->portDS.logMinPdelayReqInterval + 1)));
|
||
|
|
}
|
||
|
|
ptpClock->portDS.portState = PTP_PASSIVE;
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_UNCALIBRATED:
|
||
|
|
|
||
|
|
timerStart(ANNOUNCE_RECEIPT_TIMER, (ptpClock->portDS.announceReceiptTimeout)*(pow2ms(ptpClock->portDS.logAnnounceInterval)));
|
||
|
|
switch (ptpClock->portDS.delayMechanism)
|
||
|
|
{
|
||
|
|
case E2E:
|
||
|
|
timerStart(DELAYREQ_INTERVAL_TIMER, getRand(pow2ms(ptpClock->portDS.logMinDelayReqInterval + 1)));
|
||
|
|
break;
|
||
|
|
case P2P:
|
||
|
|
timerStart(PDELAYREQ_INTERVAL_TIMER, getRand(pow2ms(ptpClock->portDS.logMinPdelayReqInterval + 1)));
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
/* none */
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
ptpClock->portDS.portState = PTP_UNCALIBRATED;
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_SLAVE:
|
||
|
|
|
||
|
|
ptpClock->portDS.portState = PTP_SLAVE;
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
static bool doInit(PtpClock *ptpClock)
|
||
|
|
{
|
||
|
|
DBG("manufacturerIdentity: %s\n", MANUFACTURER_ID);
|
||
|
|
|
||
|
|
/* initialize networking */
|
||
|
|
netShutdown(&ptpClock->netPath);
|
||
|
|
|
||
|
|
if (!netInit(&ptpClock->netPath, ptpClock))
|
||
|
|
{
|
||
|
|
ERROR("doInit: failed to initialize network\n");
|
||
|
|
return FALSE;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
/* initialize other stuff */
|
||
|
|
initData(ptpClock);
|
||
|
|
initTimer();
|
||
|
|
initClock(ptpClock);
|
||
|
|
m1(ptpClock);
|
||
|
|
msgPackHeader(ptpClock, ptpClock->msgObuf);
|
||
|
|
return TRUE;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Handle actions and events for 'port_state' */
|
||
|
|
void doState(PtpClock *ptpClock)
|
||
|
|
{
|
||
|
|
ptpClock->messageActivity = FALSE;
|
||
|
|
|
||
|
|
switch (ptpClock->portDS.portState)
|
||
|
|
{
|
||
|
|
case PTP_LISTENING:
|
||
|
|
case PTP_UNCALIBRATED:
|
||
|
|
case PTP_SLAVE:
|
||
|
|
case PTP_PRE_MASTER:
|
||
|
|
case PTP_MASTER:
|
||
|
|
case PTP_PASSIVE:
|
||
|
|
|
||
|
|
/* State decision Event */
|
||
|
|
if (getFlag(ptpClock->events, STATE_DECISION_EVENT))
|
||
|
|
{
|
||
|
|
DBGV("event STATE_DECISION_EVENT\n");
|
||
|
|
clearFlag(ptpClock->events, STATE_DECISION_EVENT);
|
||
|
|
ptpClock->recommendedState = bmc(ptpClock);
|
||
|
|
DBGV("recommending state %s\n", stateString(ptpClock->recommendedState));
|
||
|
|
|
||
|
|
switch (ptpClock->recommendedState)
|
||
|
|
{
|
||
|
|
case PTP_MASTER:
|
||
|
|
case PTP_PASSIVE:
|
||
|
|
if (ptpClock->defaultDS.slaveOnly || ptpClock->defaultDS.clockQuality.clockClass == 255)
|
||
|
|
{
|
||
|
|
ptpClock->recommendedState = PTP_LISTENING;
|
||
|
|
DBGV("recommending state %s\n", stateString(ptpClock->recommendedState));
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch (ptpClock->recommendedState)
|
||
|
|
{
|
||
|
|
case PTP_MASTER:
|
||
|
|
switch (ptpClock->portDS.portState)
|
||
|
|
{
|
||
|
|
case PTP_PRE_MASTER:
|
||
|
|
if (timerExpired(QUALIFICATION_TIMEOUT)) toState(ptpClock, PTP_MASTER);
|
||
|
|
break;
|
||
|
|
case PTP_MASTER:
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
toState(ptpClock, PTP_PRE_MASTER);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_PASSIVE:
|
||
|
|
if (ptpClock->portDS.portState != ptpClock->recommendedState) toState(ptpClock, PTP_PASSIVE);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_SLAVE:
|
||
|
|
|
||
|
|
switch (ptpClock->portDS.portState)
|
||
|
|
{
|
||
|
|
case PTP_UNCALIBRATED:
|
||
|
|
|
||
|
|
if (getFlag(ptpClock->events, MASTER_CLOCK_SELECTED))
|
||
|
|
{
|
||
|
|
DBG("event MASTER_CLOCK_SELECTED\n");
|
||
|
|
clearFlag(ptpClock->events, MASTER_CLOCK_SELECTED);
|
||
|
|
toState(ptpClock, PTP_SLAVE);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (getFlag(ptpClock->events, MASTER_CLOCK_CHANGED))
|
||
|
|
{
|
||
|
|
DBG("event MASTER_CLOCK_CHANGED\n");
|
||
|
|
clearFlag(ptpClock->events, MASTER_CLOCK_CHANGED);
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_SLAVE:
|
||
|
|
|
||
|
|
if (getFlag(ptpClock->events, SYNCHRONIZATION_FAULT))
|
||
|
|
{
|
||
|
|
DBG("event SYNCHRONIZATION_FAULT\n");
|
||
|
|
clearFlag(ptpClock->events, SYNCHRONIZATION_FAULT);
|
||
|
|
toState(ptpClock, PTP_UNCALIBRATED);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (getFlag(ptpClock->events, MASTER_CLOCK_CHANGED))
|
||
|
|
{
|
||
|
|
DBG("event MASTER_CLOCK_CHANGED\n");
|
||
|
|
clearFlag(ptpClock->events, MASTER_CLOCK_CHANGED);
|
||
|
|
toState(ptpClock, PTP_UNCALIBRATED);
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
|
||
|
|
toState(ptpClock, PTP_UNCALIBRATED);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_LISTENING:
|
||
|
|
|
||
|
|
if (ptpClock->portDS.portState != ptpClock->recommendedState)
|
||
|
|
{
|
||
|
|
toState(ptpClock, PTP_LISTENING);
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_INITIALIZING:
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
DBG("doState: unrecognized recommended state %d\n", ptpClock->recommendedState);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch (ptpClock->portDS.portState)
|
||
|
|
{
|
||
|
|
case PTP_INITIALIZING:
|
||
|
|
|
||
|
|
if (doInit(ptpClock) == TRUE)
|
||
|
|
{
|
||
|
|
toState(ptpClock, PTP_LISTENING);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_FAULTY:
|
||
|
|
|
||
|
|
/* Imaginary troubleshooting */
|
||
|
|
DBG("event FAULT_CLEARED for state PTP_FAULT\n");
|
||
|
|
toState(ptpClock, PTP_INITIALIZING);
|
||
|
|
return;
|
||
|
|
|
||
|
|
case PTP_DISABLED:
|
||
|
|
handle(ptpClock);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_LISTENING:
|
||
|
|
case PTP_UNCALIBRATED:
|
||
|
|
case PTP_SLAVE:
|
||
|
|
case PTP_PASSIVE:
|
||
|
|
|
||
|
|
if (timerExpired(ANNOUNCE_RECEIPT_TIMER))
|
||
|
|
{
|
||
|
|
DBGV("event ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES for state %s\n", stateString(ptpClock->portDS.portState));
|
||
|
|
ptpClock->foreignMasterDS.count = 0;
|
||
|
|
ptpClock->foreignMasterDS.i = 0;
|
||
|
|
|
||
|
|
if (!(ptpClock->defaultDS.slaveOnly || ptpClock->defaultDS.clockQuality.clockClass == 255))
|
||
|
|
{
|
||
|
|
m1(ptpClock);
|
||
|
|
ptpClock->recommendedState = PTP_MASTER;
|
||
|
|
DBGV("recommending state %s\n", stateString(ptpClock->recommendedState));
|
||
|
|
toState(ptpClock, PTP_MASTER);
|
||
|
|
}
|
||
|
|
else if (ptpClock->portDS.portState != PTP_LISTENING)
|
||
|
|
{
|
||
|
|
toState(ptpClock, PTP_LISTENING);
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
handle(ptpClock);
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_MASTER:
|
||
|
|
|
||
|
|
if (timerExpired(SYNC_INTERVAL_TIMER))
|
||
|
|
{
|
||
|
|
DBGV("event SYNC_INTERVAL_TIMEOUT_EXPIRES for state PTP_MASTER\n");
|
||
|
|
issueSync(ptpClock);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (timerExpired(ANNOUNCE_INTERVAL_TIMER))
|
||
|
|
{
|
||
|
|
DBGV("event ANNOUNCE_INTERVAL_TIMEOUT_EXPIRES for state PTP_MASTER\n");
|
||
|
|
issueAnnounce(ptpClock);
|
||
|
|
}
|
||
|
|
|
||
|
|
handle(ptpClock);
|
||
|
|
issueDelayReqTimerExpired(ptpClock);
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
DBG("doState: do unrecognized state %d\n", ptpClock->portDS.portState);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/* Check and handle received messages */
|
||
|
|
static void handle(PtpClock *ptpClock)
|
||
|
|
{
|
||
|
|
|
||
|
|
int ret;
|
||
|
|
bool isFromSelf;
|
||
|
|
TimeInternal time = { 0, 0 };
|
||
|
|
|
||
|
|
if (FALSE == ptpClock->messageActivity)
|
||
|
|
{
|
||
|
|
ret = netSelect(&ptpClock->netPath, 0);
|
||
|
|
|
||
|
|
if (ret < 0)
|
||
|
|
{
|
||
|
|
ERROR("handle: failed to poll sockets\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
else if (!ret)
|
||
|
|
{
|
||
|
|
// DBGVV("handle: nothing\n");
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
DBGVV("handle: something\n");
|
||
|
|
|
||
|
|
/* Receive an event. */
|
||
|
|
ptpClock->msgIbufLength = netRecvEvent(&ptpClock->netPath, ptpClock->msgIbuf, &time);
|
||
|
|
/* local time is not UTC, we can calculate UTC on demand, otherwise UTC time is not used */
|
||
|
|
/* time.seconds += ptpClock->timePropertiesDS.currentUtcOffset; */
|
||
|
|
DBGV("handle: netRecvEvent returned %d\n", ptpClock->msgIbufLength);
|
||
|
|
|
||
|
|
if (ptpClock->msgIbufLength < 0)
|
||
|
|
{
|
||
|
|
ERROR("handle: failed to receive on the event socket\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
else if (!ptpClock->msgIbufLength)
|
||
|
|
{
|
||
|
|
/* Receive a general packet. */
|
||
|
|
ptpClock->msgIbufLength = netRecvGeneral(&ptpClock->netPath, ptpClock->msgIbuf, &time);
|
||
|
|
DBGV("handle: netRecvGeneral returned %d\n", ptpClock->msgIbufLength);
|
||
|
|
|
||
|
|
if (ptpClock->msgIbufLength < 0)
|
||
|
|
{
|
||
|
|
ERROR("handle: failed to receive on the general socket\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
else if (!ptpClock->msgIbufLength)
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
ptpClock->messageActivity = TRUE;
|
||
|
|
|
||
|
|
if (ptpClock->msgIbufLength < HEADER_LENGTH)
|
||
|
|
{
|
||
|
|
ERROR("handle: message shorter than header length\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
msgUnpackHeader(ptpClock->msgIbuf, &ptpClock->msgTmpHeader);
|
||
|
|
DBGV("handle: unpacked message type %d\n", ptpClock->msgTmpHeader.messageType);
|
||
|
|
|
||
|
|
if (ptpClock->msgTmpHeader.versionPTP != ptpClock->portDS.versionNumber)
|
||
|
|
{
|
||
|
|
DBGV("handle: ignore version %d message\n", ptpClock->msgTmpHeader.versionPTP);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (ptpClock->msgTmpHeader.domainNumber != ptpClock->defaultDS.domainNumber)
|
||
|
|
{
|
||
|
|
DBGV("handle: ignore message from domainNumber %d\n", ptpClock->msgTmpHeader.domainNumber);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Spec 9.5.2.2 */
|
||
|
|
isFromSelf = isSamePortIdentity(
|
||
|
|
&ptpClock->portDS.portIdentity,
|
||
|
|
&ptpClock->msgTmpHeader.sourcePortIdentity);
|
||
|
|
|
||
|
|
/* Subtract the inbound latency adjustment if it is not a loop back and the
|
||
|
|
time stamp seems reasonable */
|
||
|
|
if (!isFromSelf && time.seconds > 0)
|
||
|
|
subTime(&time, &time, &ptpClock->inboundLatency);
|
||
|
|
|
||
|
|
switch (ptpClock->msgTmpHeader.messageType)
|
||
|
|
{
|
||
|
|
|
||
|
|
case ANNOUNCE:
|
||
|
|
handleAnnounce(ptpClock, isFromSelf);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case SYNC:
|
||
|
|
handleSync(ptpClock, &time, isFromSelf);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case FOLLOW_UP:
|
||
|
|
handleFollowUp(ptpClock, isFromSelf);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case DELAY_REQ:
|
||
|
|
handleDelayReq(ptpClock, &time, isFromSelf);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PDELAY_REQ:
|
||
|
|
handlePDelayReq(ptpClock, &time, isFromSelf);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case DELAY_RESP:
|
||
|
|
handleDelayResp(ptpClock, isFromSelf);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PDELAY_RESP:
|
||
|
|
handlePDelayResp(ptpClock, &time, isFromSelf);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PDELAY_RESP_FOLLOW_UP:
|
||
|
|
handlePDelayRespFollowUp(ptpClock, isFromSelf);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case MANAGEMENT:
|
||
|
|
handleManagement(ptpClock, isFromSelf);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case SIGNALING:
|
||
|
|
handleSignaling(ptpClock, isFromSelf);
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
DBG("handle: unrecognized message %d\n", ptpClock->msgTmpHeader.messageType);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* spec 9.5.3 */
|
||
|
|
static void handleAnnounce(PtpClock *ptpClock, bool isFromSelf)
|
||
|
|
{
|
||
|
|
bool isFromCurrentParent = FALSE;
|
||
|
|
|
||
|
|
DBGV("handleAnnounce: received in state %s\n", stateString(ptpClock->portDS.portState));
|
||
|
|
|
||
|
|
if (ptpClock->msgIbufLength < ANNOUNCE_LENGTH)
|
||
|
|
{
|
||
|
|
ERROR("handleAnnounce: short message\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (isFromSelf)
|
||
|
|
{
|
||
|
|
DBGV("handleAnnounce: ignore from self\n");
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch (ptpClock->portDS.portState)
|
||
|
|
{
|
||
|
|
case PTP_INITIALIZING:
|
||
|
|
case PTP_FAULTY:
|
||
|
|
case PTP_DISABLED:
|
||
|
|
|
||
|
|
DBGV("handleAnnounce: disreguard\n");
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_UNCALIBRATED:
|
||
|
|
case PTP_SLAVE:
|
||
|
|
|
||
|
|
/* Valid announce message is received : BMC algorithm will be executed */
|
||
|
|
setFlag(ptpClock->events, STATE_DECISION_EVENT);
|
||
|
|
isFromCurrentParent = isSamePortIdentity(
|
||
|
|
&ptpClock->parentDS.parentPortIdentity,
|
||
|
|
&ptpClock->msgTmpHeader.sourcePortIdentity);
|
||
|
|
msgUnpackAnnounce(ptpClock->msgIbuf, &ptpClock->msgTmp.announce);
|
||
|
|
if (isFromCurrentParent)
|
||
|
|
{
|
||
|
|
s1(ptpClock, &ptpClock->msgTmpHeader, &ptpClock->msgTmp.announce);
|
||
|
|
/* Reset Timer handling Announce receipt timeout */
|
||
|
|
timerStart(ANNOUNCE_RECEIPT_TIMER, (ptpClock->portDS.announceReceiptTimeout) * (pow2ms(ptpClock->portDS.logAnnounceInterval)));
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
DBGV("handleAnnounce: from another foreign master\n");
|
||
|
|
/* addForeign takes care of AnnounceUnpacking */
|
||
|
|
addForeign(ptpClock, &ptpClock->msgTmpHeader, &ptpClock->msgTmp.announce);
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_PASSIVE:
|
||
|
|
timerStart(ANNOUNCE_RECEIPT_TIMER, (ptpClock->portDS.announceReceiptTimeout)*(pow2ms(ptpClock->portDS.logAnnounceInterval)));
|
||
|
|
case PTP_MASTER:
|
||
|
|
case PTP_PRE_MASTER:
|
||
|
|
case PTP_LISTENING:
|
||
|
|
default :
|
||
|
|
|
||
|
|
DBGV("handleAnnounce: from another foreign master\n");
|
||
|
|
msgUnpackAnnounce(ptpClock->msgIbuf, &ptpClock->msgTmp.announce);
|
||
|
|
|
||
|
|
/* Valid announce message is received : BMC algorithm will be executed */
|
||
|
|
setFlag(ptpClock->events, STATE_DECISION_EVENT);
|
||
|
|
addForeign(ptpClock, &ptpClock->msgTmpHeader, &ptpClock->msgTmp.announce);
|
||
|
|
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void handleSync(PtpClock *ptpClock, TimeInternal *time, bool isFromSelf)
|
||
|
|
{
|
||
|
|
TimeInternal originTimestamp;
|
||
|
|
TimeInternal correctionField;
|
||
|
|
bool isFromCurrentParent = FALSE;
|
||
|
|
|
||
|
|
DBGV("handleSync: received in state %s\n", stateString(ptpClock->portDS.portState));
|
||
|
|
|
||
|
|
if (ptpClock->msgIbufLength < SYNC_LENGTH)
|
||
|
|
{
|
||
|
|
ERROR("handleSync: short message\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch (ptpClock->portDS.portState)
|
||
|
|
{
|
||
|
|
case PTP_INITIALIZING:
|
||
|
|
case PTP_FAULTY:
|
||
|
|
case PTP_DISABLED:
|
||
|
|
|
||
|
|
DBGV("handleSync: disreguard\n");
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_UNCALIBRATED:
|
||
|
|
case PTP_SLAVE:
|
||
|
|
|
||
|
|
if (isFromSelf)
|
||
|
|
{
|
||
|
|
DBGV("handleSync: ignore from self\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
isFromCurrentParent = isSamePortIdentity(
|
||
|
|
&ptpClock->parentDS.parentPortIdentity,
|
||
|
|
&ptpClock->msgTmpHeader.sourcePortIdentity);
|
||
|
|
|
||
|
|
if (!isFromCurrentParent)
|
||
|
|
{
|
||
|
|
DBGV("handleSync: ignore from another master\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
ptpClock->timestamp_syncRecieve = *time;
|
||
|
|
scaledNanosecondsToInternalTime(&ptpClock->msgTmpHeader.correctionfield, &correctionField);
|
||
|
|
|
||
|
|
if (getFlag(ptpClock->msgTmpHeader.flagField[0], FLAG0_TWO_STEP))
|
||
|
|
{
|
||
|
|
ptpClock->waitingForFollowUp = TRUE;
|
||
|
|
ptpClock->recvSyncSequenceId = ptpClock->msgTmpHeader.sequenceId;
|
||
|
|
/* Save correctionField of Sync message for future use */
|
||
|
|
ptpClock->correctionField_sync = correctionField;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
msgUnpackSync(ptpClock->msgIbuf, &ptpClock->msgTmp.sync);
|
||
|
|
ptpClock->waitingForFollowUp = FALSE;
|
||
|
|
/* Synchronize local clock */
|
||
|
|
toInternalTime(&originTimestamp, &ptpClock->msgTmp.sync.originTimestamp);
|
||
|
|
/* use correctionField of Sync message for future use */
|
||
|
|
updateOffset(ptpClock, &ptpClock->timestamp_syncRecieve, &originTimestamp, &correctionField);
|
||
|
|
updateClock(ptpClock);
|
||
|
|
issueDelayReqTimerExpired(ptpClock);
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_MASTER:
|
||
|
|
|
||
|
|
if (!isFromSelf)
|
||
|
|
{
|
||
|
|
DBGV("handleSync: from another master\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
DBGV("handleSync: ignore from self\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
// if waitingForLoopback && TWO_STEP_FLAG
|
||
|
|
// {
|
||
|
|
// /* Add latency */
|
||
|
|
// addTime(time, time, &rtOpts->outboundLatency);
|
||
|
|
//
|
||
|
|
// issueFollowup(ptpClock, time);
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
case PTP_PASSIVE:
|
||
|
|
|
||
|
|
DBGV("handleSync: disreguard\n");
|
||
|
|
issueDelayReqTimerExpired(ptpClock);
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
|
||
|
|
DBGV("handleSync: disreguard\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
static void handleFollowUp(PtpClock *ptpClock, bool isFromSelf)
|
||
|
|
{
|
||
|
|
TimeInternal preciseOriginTimestamp;
|
||
|
|
TimeInternal correctionField;
|
||
|
|
bool isFromCurrentParent = FALSE;
|
||
|
|
|
||
|
|
DBGV("handleFollowup: received in state %s\n", stateString(ptpClock->portDS.portState));
|
||
|
|
|
||
|
|
if (ptpClock->msgIbufLength < FOLLOW_UP_LENGTH)
|
||
|
|
{
|
||
|
|
ERROR("handleFollowup: short message\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (isFromSelf)
|
||
|
|
{
|
||
|
|
DBGV("handleFollowup: ignore from self\n");
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch (ptpClock->portDS.portState)
|
||
|
|
{
|
||
|
|
case PTP_INITIALIZING:
|
||
|
|
case PTP_FAULTY:
|
||
|
|
case PTP_DISABLED:
|
||
|
|
case PTP_LISTENING:
|
||
|
|
|
||
|
|
DBGV("handleFollowup: disreguard\n");
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_UNCALIBRATED:
|
||
|
|
case PTP_SLAVE:
|
||
|
|
|
||
|
|
isFromCurrentParent = isSamePortIdentity(
|
||
|
|
&ptpClock->parentDS.parentPortIdentity,
|
||
|
|
&ptpClock->msgTmpHeader.sourcePortIdentity);
|
||
|
|
|
||
|
|
if (!ptpClock->waitingForFollowUp)
|
||
|
|
{
|
||
|
|
DBGV("handleFollowup: not waiting a message\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!isFromCurrentParent)
|
||
|
|
{
|
||
|
|
DBGV("handleFollowup: not from current parent\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (ptpClock->recvSyncSequenceId != ptpClock->msgTmpHeader.sequenceId)
|
||
|
|
{
|
||
|
|
DBGV("handleFollowup: SequenceID doesn't match with last Sync message\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
msgUnpackFollowUp(ptpClock->msgIbuf, &ptpClock->msgTmp.follow);
|
||
|
|
|
||
|
|
ptpClock->waitingForFollowUp = FALSE;
|
||
|
|
/* synchronize local clock */
|
||
|
|
toInternalTime(&preciseOriginTimestamp, &ptpClock->msgTmp.follow.preciseOriginTimestamp);
|
||
|
|
scaledNanosecondsToInternalTime(&ptpClock->msgTmpHeader.correctionfield, &correctionField);
|
||
|
|
addTime(&correctionField, &correctionField, &ptpClock->correctionField_sync);
|
||
|
|
updateOffset(ptpClock, &ptpClock->timestamp_syncRecieve, &preciseOriginTimestamp, &correctionField);
|
||
|
|
updateClock(ptpClock);
|
||
|
|
|
||
|
|
issueDelayReqTimerExpired(ptpClock);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_MASTER:
|
||
|
|
|
||
|
|
DBGV("handleFollowup: from another master\n");
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_PASSIVE:
|
||
|
|
|
||
|
|
DBGV("handleFollowup: disreguard\n");
|
||
|
|
issueDelayReqTimerExpired(ptpClock);
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
|
||
|
|
DBG("handleFollowup: unrecognized state\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
static void handleDelayReq(PtpClock *ptpClock, TimeInternal *time, bool isFromSelf)
|
||
|
|
{
|
||
|
|
switch (ptpClock->portDS.delayMechanism)
|
||
|
|
{
|
||
|
|
case E2E:
|
||
|
|
|
||
|
|
DBGV("handleDelayReq: received in mode E2E in state %s\n", stateString(ptpClock->portDS.portState));
|
||
|
|
if (ptpClock->msgIbufLength < DELAY_REQ_LENGTH)
|
||
|
|
{
|
||
|
|
ERROR("handleDelayReq: short message\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch (ptpClock->portDS.portState)
|
||
|
|
{
|
||
|
|
case PTP_INITIALIZING:
|
||
|
|
case PTP_FAULTY:
|
||
|
|
case PTP_DISABLED:
|
||
|
|
case PTP_UNCALIBRATED:
|
||
|
|
case PTP_LISTENING:
|
||
|
|
DBGV("handleDelayReq: disreguard\n");
|
||
|
|
return;
|
||
|
|
|
||
|
|
case PTP_SLAVE:
|
||
|
|
DBGV("handleDelayReq: disreguard\n");
|
||
|
|
// if (isFromSelf)
|
||
|
|
// {
|
||
|
|
// /* waitingForLoopback? */
|
||
|
|
// /* Get sending timestamp from IP stack with So_TIMESTAMP */
|
||
|
|
// ptpClock->delay_req_send_time = *time;
|
||
|
|
|
||
|
|
// /* Add latency */
|
||
|
|
// addTime(&ptpClock->delay_req_send_time, &ptpClock->delay_req_send_time, &rtOpts->outboundLatency);
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PTP_MASTER:
|
||
|
|
/* TODO: manage the value of ptpClock->logMinDelayReqInterval form logSyncInterval to logSyncInterval + 5 */
|
||
|
|
issueDelayResp(ptpClock, time, &ptpClock->msgTmpHeader);
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
DBG("handleDelayReq: unrecognized state\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
case P2P:
|
||
|
|
|
||
|
|
ERROR("handleDelayReq: disreguard in P2P mode\n");
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
|
||
|
|
/* none */
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
static void handleDelayResp(PtpClock *ptpClock, bool isFromSelf)
|
||
|
|
{
|
||
|
|
bool isFromCurrentParent = FALSE;
|
||
|
|
bool isCurrentRequest = FALSE;
|
||
|
|
TimeInternal correctionField;
|
||
|
|
|
||
|
|
switch (ptpClock->portDS.delayMechanism)
|
||
|
|
{
|
||
|
|
case E2E:
|
||
|
|
|
||
|
|
DBGV("handleDelayResp: received in mode E2E in state %s\n", stateString(ptpClock->portDS.portState));
|
||
|
|
if (ptpClock->msgIbufLength < DELAY_RESP_LENGTH)
|
||
|
|
{
|
||
|
|
ERROR("handleDelayResp: short message\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch (ptpClock->portDS.portState)
|
||
|
|
{
|
||
|
|
case PTP_INITIALIZING:
|
||
|
|
case PTP_FAULTY:
|
||
|
|
case PTP_DISABLED:
|
||
|
|
case PTP_LISTENING:
|
||
|
|
DBGV("handleDelayResp: disreguard\n");
|
||
|
|
return;
|
||
|
|
|
||
|
|
case PTP_UNCALIBRATED:
|
||
|
|
case PTP_SLAVE:
|
||
|
|
|
||
|
|
msgUnpackDelayResp(ptpClock->msgIbuf, &ptpClock->msgTmp.resp);
|
||
|
|
|
||
|
|
isFromCurrentParent = isSamePortIdentity(
|
||
|
|
&ptpClock->parentDS.parentPortIdentity,
|
||
|
|
&ptpClock->msgTmpHeader.sourcePortIdentity);
|
||
|
|
|
||
|
|
isCurrentRequest = isSamePortIdentity(
|
||
|
|
&ptpClock->portDS.portIdentity,
|
||
|
|
&ptpClock->msgTmp.resp.requestingPortIdentity);
|
||
|
|
|
||
|
|
if (((ptpClock->sentDelayReqSequenceId - 1) == ptpClock->msgTmpHeader.sequenceId) && isCurrentRequest && isFromCurrentParent)
|
||
|
|
{
|
||
|
|
/* TODO: revisit 11.3 */
|
||
|
|
toInternalTime(&ptpClock->timestamp_delayReqRecieve, &ptpClock->msgTmp.resp.receiveTimestamp);
|
||
|
|
|
||
|
|
scaledNanosecondsToInternalTime(&ptpClock->msgTmpHeader.correctionfield, &correctionField);
|
||
|
|
updateDelay(ptpClock, &ptpClock->timestamp_delayReqSend, &ptpClock->timestamp_delayReqRecieve, &correctionField);
|
||
|
|
|
||
|
|
ptpClock->portDS.logMinDelayReqInterval = ptpClock->msgTmpHeader.logMessageInterval;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
DBGV("handleDelayResp: doesn't match with the delayReq\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
case P2P:
|
||
|
|
|
||
|
|
ERROR("handleDelayResp: disreguard in P2P mode\n");
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
static void handlePDelayReq(PtpClock *ptpClock, TimeInternal *time, bool isFromSelf)
|
||
|
|
{
|
||
|
|
switch (ptpClock->portDS.delayMechanism)
|
||
|
|
{
|
||
|
|
case E2E:
|
||
|
|
ERROR("handlePDelayReq: disreguard in E2E mode\n");
|
||
|
|
break;
|
||
|
|
|
||
|
|
case P2P:
|
||
|
|
|
||
|
|
DBGV("handlePDelayReq: received in mode P2P in state %s\n", stateString(ptpClock->portDS.portState));
|
||
|
|
if (ptpClock->msgIbufLength < PDELAY_REQ_LENGTH)
|
||
|
|
{
|
||
|
|
ERROR("handlePDelayReq: short message\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch (ptpClock->portDS.portState)
|
||
|
|
{
|
||
|
|
case PTP_INITIALIZING:
|
||
|
|
case PTP_FAULTY:
|
||
|
|
case PTP_DISABLED:
|
||
|
|
case PTP_UNCALIBRATED:
|
||
|
|
case PTP_LISTENING:
|
||
|
|
DBGV("handlePDelayReq: disreguard\n");
|
||
|
|
return;
|
||
|
|
|
||
|
|
case PTP_PASSIVE:
|
||
|
|
case PTP_SLAVE:
|
||
|
|
case PTP_MASTER:
|
||
|
|
|
||
|
|
if (isFromSelf)
|
||
|
|
{
|
||
|
|
DBGV("handlePDelayReq: ignore from self\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
// if (isFromSelf) /* && loopback mode */
|
||
|
|
// {
|
||
|
|
// /* Get sending timestamp from IP stack with So_TIMESTAMP */
|
||
|
|
// ptpClock->pdelay_req_send_time = *time;
|
||
|
|
//
|
||
|
|
// /* Add latency */
|
||
|
|
// addTime(&ptpClock->pdelay_req_send_time, &ptpClock->pdelay_req_send_time, &rtOpts->outboundLatency);
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
// else
|
||
|
|
// {
|
||
|
|
//ptpClock->PdelayReqHeader = ptpClock->msgTmpHeader;
|
||
|
|
|
||
|
|
issuePDelayResp(ptpClock, time, &ptpClock->msgTmpHeader);
|
||
|
|
|
||
|
|
if ((time->seconds != 0) && getFlag(ptpClock->msgTmpHeader.flagField[0], FLAG0_TWO_STEP)) /* not loopback mode */
|
||
|
|
{
|
||
|
|
issuePDelayRespFollowUp(ptpClock, time, &ptpClock->msgTmpHeader);
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
// }
|
||
|
|
|
||
|
|
default:
|
||
|
|
|
||
|
|
DBG("handlePDelayReq: unrecognized state\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void handlePDelayResp(PtpClock *ptpClock, TimeInternal *time, bool isFromSelf)
|
||
|
|
{
|
||
|
|
TimeInternal requestReceiptTimestamp;
|
||
|
|
TimeInternal correctionField;
|
||
|
|
bool isCurrentRequest;
|
||
|
|
|
||
|
|
switch (ptpClock->portDS.delayMechanism)
|
||
|
|
{
|
||
|
|
case E2E:
|
||
|
|
|
||
|
|
ERROR("handlePDelayResp: disreguard in E2E mode\n");
|
||
|
|
break;
|
||
|
|
|
||
|
|
case P2P:
|
||
|
|
|
||
|
|
DBGV("handlePDelayResp: received in mode P2P in state %s\n", stateString(ptpClock->portDS.portState));
|
||
|
|
if (ptpClock->msgIbufLength < PDELAY_RESP_LENGTH)
|
||
|
|
{
|
||
|
|
ERROR("handlePDelayResp: short message\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch (ptpClock->portDS.portState)
|
||
|
|
{
|
||
|
|
case PTP_INITIALIZING:
|
||
|
|
case PTP_FAULTY:
|
||
|
|
case PTP_DISABLED:
|
||
|
|
case PTP_UNCALIBRATED:
|
||
|
|
case PTP_LISTENING:
|
||
|
|
|
||
|
|
DBGV("handlePDelayResp: disreguard\n");
|
||
|
|
return;
|
||
|
|
|
||
|
|
case PTP_MASTER:
|
||
|
|
case PTP_SLAVE:
|
||
|
|
|
||
|
|
if (isFromSelf)
|
||
|
|
{
|
||
|
|
DBGV("handlePDelayResp: ignore from self\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
// if (isFromSelf) && loopback mode
|
||
|
|
// {
|
||
|
|
// addTime(time, time, &rtOpts->outboundLatency);
|
||
|
|
// issuePDelayRespFollowUp(time, ptpClock);
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
|
||
|
|
|
||
|
|
msgUnpackPDelayResp(ptpClock->msgIbuf, &ptpClock->msgTmp.presp);
|
||
|
|
|
||
|
|
isCurrentRequest = isSamePortIdentity(
|
||
|
|
&ptpClock->portDS.portIdentity,
|
||
|
|
&ptpClock->msgTmp.presp.requestingPortIdentity);
|
||
|
|
|
||
|
|
if (((ptpClock->sentPDelayReqSequenceId - 1) == ptpClock->msgTmpHeader.sequenceId) && isCurrentRequest)
|
||
|
|
{
|
||
|
|
if (getFlag(ptpClock->msgTmpHeader.flagField[0], FLAG0_TWO_STEP))
|
||
|
|
{
|
||
|
|
ptpClock->waitingForPDelayRespFollowUp = TRUE;
|
||
|
|
|
||
|
|
/* Store t4 (Fig 35)*/
|
||
|
|
ptpClock->pdelay_t4 = *time;
|
||
|
|
|
||
|
|
/* store t2 (Fig 35)*/
|
||
|
|
toInternalTime(&requestReceiptTimestamp, &ptpClock->msgTmp.presp.requestReceiptTimestamp);
|
||
|
|
ptpClock->pdelay_t2 = requestReceiptTimestamp;
|
||
|
|
|
||
|
|
scaledNanosecondsToInternalTime(&ptpClock->msgTmpHeader.correctionfield, &correctionField);
|
||
|
|
ptpClock->correctionField_pDelayResp = correctionField;
|
||
|
|
}//Two Step Clock
|
||
|
|
else //One step Clock
|
||
|
|
{
|
||
|
|
ptpClock->waitingForPDelayRespFollowUp = FALSE;
|
||
|
|
|
||
|
|
/* Store t4 (Fig 35)*/
|
||
|
|
ptpClock->pdelay_t4 = *time;
|
||
|
|
|
||
|
|
scaledNanosecondsToInternalTime(&ptpClock->msgTmpHeader.correctionfield, &correctionField);
|
||
|
|
updatePeerDelay(ptpClock, &correctionField, FALSE);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
DBGV("handlePDelayResp: PDelayResp doesn't match with the PDelayReq.\n");
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
|
||
|
|
DBG("handlePDelayResp: unrecognized state\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void handlePDelayRespFollowUp(PtpClock *ptpClock, bool isFromSelf)
|
||
|
|
{
|
||
|
|
TimeInternal responseOriginTimestamp;
|
||
|
|
TimeInternal correctionField;
|
||
|
|
|
||
|
|
switch (ptpClock->portDS.delayMechanism)
|
||
|
|
{
|
||
|
|
case E2E:
|
||
|
|
|
||
|
|
ERROR("handlePDelayRespFollowUp: disreguard in E2E mode\n");
|
||
|
|
break;
|
||
|
|
|
||
|
|
case P2P:
|
||
|
|
|
||
|
|
DBGV("handlePDelayRespFollowUp: received in mode P2P in state %s\n", stateString(ptpClock->portDS.portState));
|
||
|
|
if (ptpClock->msgIbufLength < PDELAY_RESP_FOLLOW_UP_LENGTH)
|
||
|
|
{
|
||
|
|
ERROR("handlePDelayRespFollowUp: short message\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch (ptpClock->portDS.portState)
|
||
|
|
{
|
||
|
|
case PTP_INITIALIZING:
|
||
|
|
case PTP_FAULTY:
|
||
|
|
case PTP_DISABLED:
|
||
|
|
case PTP_UNCALIBRATED:
|
||
|
|
DBGV("handlePDelayRespFollowUp: disreguard\n");
|
||
|
|
return;
|
||
|
|
|
||
|
|
case PTP_SLAVE:
|
||
|
|
case PTP_MASTER:
|
||
|
|
|
||
|
|
if (!ptpClock->waitingForPDelayRespFollowUp)
|
||
|
|
{
|
||
|
|
DBG("handlePDelayRespFollowUp: not waiting a message\n");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (ptpClock->msgTmpHeader.sequenceId == ptpClock->sentPDelayReqSequenceId - 1)
|
||
|
|
{
|
||
|
|
msgUnpackPDelayRespFollowUp(ptpClock->msgIbuf, &ptpClock->msgTmp.prespfollow);
|
||
|
|
toInternalTime(&responseOriginTimestamp, &ptpClock->msgTmp.prespfollow.responseOriginTimestamp);
|
||
|
|
ptpClock->pdelay_t3 = responseOriginTimestamp;
|
||
|
|
scaledNanosecondsToInternalTime(&ptpClock->msgTmpHeader.correctionfield, &correctionField);
|
||
|
|
addTime(&correctionField, &correctionField, &ptpClock->correctionField_pDelayResp);
|
||
|
|
updatePeerDelay(ptpClock, &correctionField, TRUE);
|
||
|
|
ptpClock->waitingForPDelayRespFollowUp = FALSE;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
default:
|
||
|
|
|
||
|
|
DBGV("handlePDelayRespFollowUp: unrecognized state\n");
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void handleManagement(PtpClock *ptpClock, bool isFromSelf)
|
||
|
|
{
|
||
|
|
/* ENABLE_PORT -> DESIGNATED_ENABLED -> toState(PTP_INITIALIZING) */
|
||
|
|
/* DISABLE_PORT -> DESIGNATED_DISABLED -> toState(PTP_DISABLED) */
|
||
|
|
}
|
||
|
|
|
||
|
|
static void handleSignaling(PtpClock *ptpClock, bool isFromSelf)
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
static void issueDelayReqTimerExpired(PtpClock *ptpClock)
|
||
|
|
{
|
||
|
|
switch (ptpClock->portDS.delayMechanism)
|
||
|
|
{
|
||
|
|
case E2E:
|
||
|
|
|
||
|
|
if (ptpClock->portDS.portState != PTP_SLAVE)
|
||
|
|
{
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (timerExpired(DELAYREQ_INTERVAL_TIMER))
|
||
|
|
{
|
||
|
|
timerStart(DELAYREQ_INTERVAL_TIMER, getRand(pow2ms(ptpClock->portDS.logMinDelayReqInterval + 1)));
|
||
|
|
DBGV("event DELAYREQ_INTERVAL_TIMEOUT_EXPIRES\n");
|
||
|
|
issueDelayReq(ptpClock);
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
case P2P:
|
||
|
|
|
||
|
|
if (timerExpired(PDELAYREQ_INTERVAL_TIMER))
|
||
|
|
{
|
||
|
|
timerStart(PDELAYREQ_INTERVAL_TIMER, getRand(pow2ms(ptpClock->portDS.logMinPdelayReqInterval + 1)));
|
||
|
|
DBGV("event PDELAYREQ_INTERVAL_TIMEOUT_EXPIRES\n");
|
||
|
|
issuePDelayReq(ptpClock);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/* Pack and send on general multicast ip adress an Announce message */
|
||
|
|
static void issueAnnounce(PtpClock *ptpClock)
|
||
|
|
{
|
||
|
|
msgPackAnnounce(ptpClock, ptpClock->msgObuf);
|
||
|
|
|
||
|
|
if (!netSendGeneral(&ptpClock->netPath, ptpClock->msgObuf, ANNOUNCE_LENGTH))
|
||
|
|
{
|
||
|
|
ERROR("issueAnnounce: can't sent\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
DBGV("issueAnnounce\n");
|
||
|
|
ptpClock->sentAnnounceSequenceId++;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Pack and send on event multicast ip adress a Sync message */
|
||
|
|
static void issueSync(PtpClock *ptpClock)
|
||
|
|
{
|
||
|
|
Timestamp originTimestamp;
|
||
|
|
TimeInternal internalTime;
|
||
|
|
|
||
|
|
/* try to predict outgoing time stamp */
|
||
|
|
getTime(&internalTime);
|
||
|
|
fromInternalTime(&internalTime, &originTimestamp);
|
||
|
|
msgPackSync(ptpClock, ptpClock->msgObuf, &originTimestamp);
|
||
|
|
|
||
|
|
if (!netSendEvent(&ptpClock->netPath, ptpClock->msgObuf, SYNC_LENGTH, &internalTime))
|
||
|
|
{
|
||
|
|
ERROR("issueSync: can't sent\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
DBGV("issueSync\n");
|
||
|
|
ptpClock->sentSyncSequenceId++;
|
||
|
|
|
||
|
|
/* sync TX timestamp is valid */
|
||
|
|
if ((internalTime.seconds != 0) && (ptpClock->defaultDS.twoStepFlag))
|
||
|
|
{
|
||
|
|
// waitingForLoopback = false;
|
||
|
|
addTime(&internalTime, &internalTime, &ptpClock->outboundLatency);
|
||
|
|
issueFollowup(ptpClock, &internalTime);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// waitingForLoopback = ptpClock->twoStepFlag;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Pack and send on general multicast ip adress a FollowUp message */
|
||
|
|
static void issueFollowup(PtpClock *ptpClock, const TimeInternal *time)
|
||
|
|
{
|
||
|
|
Timestamp preciseOriginTimestamp;
|
||
|
|
|
||
|
|
fromInternalTime(time, &preciseOriginTimestamp);
|
||
|
|
msgPackFollowUp(ptpClock, ptpClock->msgObuf, &preciseOriginTimestamp);
|
||
|
|
|
||
|
|
if (!netSendGeneral(&ptpClock->netPath, ptpClock->msgObuf, FOLLOW_UP_LENGTH))
|
||
|
|
{
|
||
|
|
ERROR("issueFollowup: can't sent\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
DBGV("issueFollowup\n");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/* Pack and send on event multicast ip address a DelayReq message */
|
||
|
|
static void issueDelayReq(PtpClock *ptpClock)
|
||
|
|
{
|
||
|
|
Timestamp originTimestamp;
|
||
|
|
TimeInternal internalTime;
|
||
|
|
|
||
|
|
getTime(&internalTime);
|
||
|
|
fromInternalTime(&internalTime, &originTimestamp);
|
||
|
|
|
||
|
|
msgPackDelayReq(ptpClock, ptpClock->msgObuf, &originTimestamp);
|
||
|
|
|
||
|
|
if (!netSendEvent(&ptpClock->netPath, ptpClock->msgObuf, DELAY_REQ_LENGTH, &internalTime))
|
||
|
|
{
|
||
|
|
ERROR("issueDelayReq: can't sent\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
DBGV("issueDelayReq\n");
|
||
|
|
ptpClock->sentDelayReqSequenceId++;
|
||
|
|
|
||
|
|
/* Delay req TX timestamp is valid */
|
||
|
|
if (internalTime.seconds != 0)
|
||
|
|
{
|
||
|
|
addTime(&internalTime, &internalTime, &ptpClock->outboundLatency);
|
||
|
|
ptpClock->timestamp_delayReqSend = internalTime;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Pack and send on event multicast ip adress a PDelayReq message */
|
||
|
|
static void issuePDelayReq(PtpClock *ptpClock)
|
||
|
|
{
|
||
|
|
Timestamp originTimestamp;
|
||
|
|
TimeInternal internalTime;
|
||
|
|
|
||
|
|
getTime(&internalTime);
|
||
|
|
fromInternalTime(&internalTime, &originTimestamp);
|
||
|
|
|
||
|
|
msgPackPDelayReq(ptpClock, ptpClock->msgObuf, &originTimestamp);
|
||
|
|
|
||
|
|
if (!netSendPeerEvent(&ptpClock->netPath, ptpClock->msgObuf, PDELAY_REQ_LENGTH, &internalTime))
|
||
|
|
{
|
||
|
|
ERROR("issuePDelayReq: can't sent\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
DBGV("issuePDelayReq\n");
|
||
|
|
ptpClock->sentPDelayReqSequenceId++;
|
||
|
|
|
||
|
|
/* Delay req TX timestamp is valid */
|
||
|
|
if (internalTime.seconds != 0)
|
||
|
|
{
|
||
|
|
addTime(&internalTime, &internalTime, &ptpClock->outboundLatency);
|
||
|
|
ptpClock->pdelay_t1 = internalTime;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Pack and send on event multicast ip adress a PDelayResp message */
|
||
|
|
static void issuePDelayResp(PtpClock *ptpClock, TimeInternal *time, const MsgHeader * pDelayReqHeader)
|
||
|
|
{
|
||
|
|
Timestamp requestReceiptTimestamp;
|
||
|
|
|
||
|
|
fromInternalTime(time, &requestReceiptTimestamp);
|
||
|
|
msgPackPDelayResp(ptpClock->msgObuf, pDelayReqHeader, &requestReceiptTimestamp);
|
||
|
|
|
||
|
|
if (!netSendPeerEvent(&ptpClock->netPath, ptpClock->msgObuf, PDELAY_RESP_LENGTH, time))
|
||
|
|
{
|
||
|
|
ERROR("issuePDelayResp: can't sent\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
if (time->seconds != 0)
|
||
|
|
{
|
||
|
|
/* Add latency */
|
||
|
|
addTime(time, time, &ptpClock->outboundLatency);
|
||
|
|
}
|
||
|
|
|
||
|
|
DBGV("issuePDelayResp\n");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/* Pack and send on event multicast ip adress a DelayResp message */
|
||
|
|
static void issueDelayResp(PtpClock *ptpClock, const TimeInternal *time, const MsgHeader * delayReqHeader)
|
||
|
|
{
|
||
|
|
Timestamp requestReceiptTimestamp;
|
||
|
|
|
||
|
|
fromInternalTime(time, &requestReceiptTimestamp);
|
||
|
|
msgPackDelayResp(ptpClock, ptpClock->msgObuf, delayReqHeader, &requestReceiptTimestamp);
|
||
|
|
|
||
|
|
if (!netSendGeneral(&ptpClock->netPath, ptpClock->msgObuf, PDELAY_RESP_LENGTH))
|
||
|
|
{
|
||
|
|
ERROR("issueDelayResp: can't sent\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
DBGV("issueDelayResp\n");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void issuePDelayRespFollowUp(PtpClock *ptpClock, const TimeInternal *time, const MsgHeader * pDelayReqHeader)
|
||
|
|
{
|
||
|
|
Timestamp responseOriginTimestamp;
|
||
|
|
fromInternalTime(time, &responseOriginTimestamp);
|
||
|
|
|
||
|
|
msgPackPDelayRespFollowUp(ptpClock->msgObuf, pDelayReqHeader, &responseOriginTimestamp);
|
||
|
|
|
||
|
|
if (!netSendPeerGeneral(&ptpClock->netPath, ptpClock->msgObuf, PDELAY_RESP_FOLLOW_UP_LENGTH))
|
||
|
|
{
|
||
|
|
ERROR("issuePDelayRespFollowUp: can't sent\n");
|
||
|
|
toState(ptpClock, PTP_FAULTY);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
DBGV("issuePDelayRespFollowUp\n");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|