Commit 7b289ccc authored by Alessandro Rubini's avatar Alessandro Rubini

Merge branch 'factorize White Rabbit'

This patch set removes duplicate code in ext-whiterabbit, using hooks
in proto-standard to call specific extension code.

There is still some duplication in wr-msg.c from msg.c), so I'll need
to touch stuff again, but this is good and stable enough to merge.

Unfortunately, the set of extension hooks is specifically tailored to
white rabbit, and we'll need to make it more orthogonal and solid as
soon as another extension must be supported (or as soon as we have
time).
parents 756f9d6b cd2a1001
......@@ -254,35 +254,7 @@ typedef struct DSPort {
Integer8 logMinPdelayReqInterval;
UInteger4 versionNumber;
/* White Rabbit extension begin (see wrspec.v2-06-07-2011, page 17) */
Enumeration8 wrConfig;
Enumeration8 wrMode;
Boolean wrModeOn;
Enumeration8 wrPortState;
/* FIXME check doc: knownDeltaTx, knownDeltaRx, deltasKnown?) */
Boolean calibrated;
FixedDelta deltaTx;
FixedDelta deltaRx;
UInteger32 wrStateTimeout;
UInteger8 wrStateRetry;
UInteger32 calPeriod;
UInteger8 calRetry;
Enumeration8 parentWrConfig;
Boolean parentIsWRnode; /* FIXME Not in the doc */
/* FIXME check doc: (parentWrMode?) */
Enumeration16 msgTmpWrMessageID; /* FIXME Not in the doc */
Boolean parentWrModeOn;
Boolean parentCalibrated;
/* FIXME: are they in the doc? */
UInteger16 otherNodeCalSendPattern;
UInteger32 otherNodeCalPeriod;
UInteger8 otherNodeCalRetry;
FixedDelta otherNodeDeltaTx;
FixedDelta otherNodeDeltaRx;
Boolean doRestart;
Boolean linkUP;
/* White Rabbit extension end */
void *ext_dsport;
} DSPort;
/* Time Properties Data Set */
......@@ -323,6 +295,8 @@ enum pp_std_messages {
PPM_ANNOUNCE,
PPM_SIGNALING,
PPM_MANAGEMENT,
PPM_NOTHING_TO_DO = 0x100, /* for hooks.master_msg() */
};
extern const char const * pp_msg_names[];
......
......@@ -297,6 +297,36 @@ static inline struct pp_servo *SRV(struct pp_instance *ppi)
return ppi->servo;
}
/*
* Each extension should fill this structure that is used to augment
* the standard stated and avoid code duplications. Please remember
* that proto-standard functions are picked as a fall-back when non
* extension-specific code is provided. The set of hooks here is designed
* based on what White Rabbit does. If you add more please remember to
* allow NULL pointers.
*/
struct pp_ext_hooks {
int (*init)(struct pp_instance *ppi, unsigned char *pkt, int plen);
int (*open)(struct pp_instance *ppi, struct pp_runtime_opts *rt_opts);
int (*close)(struct pp_instance *ppi);
int (*listening)(struct pp_instance *ppi, unsigned char *pkt, int plen);
int (*master_msg)(struct pp_instance *ppi, unsigned char *pkt,
int plen, int msgtype);
int (*new_slave)(struct pp_instance *ppi, unsigned char *pkt, int plen);
int (*update_delay)(struct pp_instance *ppi);
void (*s1)(struct pp_instance *ppi, MsgHeader *hdr, MsgAnnounce *ann);
int (*execute_slave)(struct pp_instance *ppi);
void (*handle_announce)(struct pp_instance *ppi);
int (*handle_followup)(struct pp_instance *ppi, TimeInternal *orig,
TimeInternal *correction_field);
int (*pack_announce)(struct pp_instance *ppi);
void (*unpack_announce)(void *buf, MsgAnnounce *ann);
};
extern struct pp_ext_hooks pp_hooks; /* The one for the extension we build */
/* The channel for an instance must be created and possibly destroyed. */
extern int pp_open_instance(struct pp_instance *ppi,
struct pp_runtime_opts *rt_opts);
......
......@@ -7,16 +7,7 @@ LIBWRO := $D/libwr.o
LIBS += $(LIBWRO)
OBJ-libwr := $D/fsm-table.o \
$D/state-initializing.o \
$D/state-listening.o \
$D/state-master.o \
$D/state-slave.o \
$D/common-fun.o \
$D/bmc.o \
$D/msg.o \
$D/arith.o \
$D/servo.o \
$D/open-close.o \
$D/hooks.o \
$D/state-wr-present.o \
$D/state-wr-m-lock.o \
$D/state-wr-s-lock.o \
......
/*
* Aurelio Colosimo for CERN, 2011 -- GNU LGPL v2.1 or later
* Based on PTPd project v. 2.1.0 (see AUTHORS for details)
*/
#include <limits.h>
#include <ppsi/ppsi.h>
#include <ppsi/diag.h>
void int64_to_TimeInternal(Integer64 bigint, TimeInternal *internal)
{
uint64_t bigint_val;
if (bigint.msb < 0)
PP_PRINTF("BUG: %s doesn't support negatives\n", __func__);
bigint_val = bigint.lsb;
bigint_val += ((int64_t)bigint.msb) << 32;
/* Use __div64_32 from library, to avoid libgcc on small targets */
internal->nanoseconds = __div64_32(&bigint_val, PP_NSEC_PER_SEC);
internal->seconds = bigint_val;
}
int from_TimeInternal(TimeInternal *internal, Timestamp *external)
{
/*
* fromInternalTime is only used to convert time given by the system
* to a timestamp As a consequence, no negative value can normally
* be found in (internal)
*
* Note that offsets are also represented with TimeInternal structure,
* and can be negative, but offset are never convert into Timestamp
* so there is no problem here.
*/
if ((internal->seconds & ~INT_MAX) ||
(internal->nanoseconds & ~INT_MAX)) {
PP_PRINTF("Error: Negative value cannot be converted into "
"timestamp\n");
return -1;
} else {
external->secondsField.lsb = internal->seconds;
external->nanosecondsField = internal->nanoseconds;
external->secondsField.msb = 0;
}
return 0;
}
int to_TimeInternal(TimeInternal *internal, Timestamp *external)
{
/* Program will not run after 2038... */
if (external->secondsField.lsb < INT_MAX) {
internal->seconds = external->secondsField.lsb;
internal->nanoseconds = external->nanosecondsField;
return 0;
} else {
PP_PRINTF("to_TimeInternal: "
"seconds field is higher than signed integer (32bits)\n");
return -1;
}
}
static void normalize_TimeInternal(TimeInternal *r)
{
r->seconds += r->nanoseconds / PP_NSEC_PER_SEC;
r->nanoseconds -= r->nanoseconds / PP_NSEC_PER_SEC * PP_NSEC_PER_SEC;
if (r->seconds > 0 && r->nanoseconds < 0) {
r->seconds -= 1;
r->nanoseconds += PP_NSEC_PER_SEC;
} else if (r->seconds < 0 && r->nanoseconds > 0) {
r->seconds += 1;
r->nanoseconds -= PP_NSEC_PER_SEC;
}
}
void add_TimeInternal(TimeInternal *r, TimeInternal *x, TimeInternal *y)
{
r->seconds = x->seconds + y->seconds;
r->nanoseconds = x->nanoseconds + y->nanoseconds;
normalize_TimeInternal(r);
}
void sub_TimeInternal(TimeInternal *r, TimeInternal *x, TimeInternal *y)
{
r->seconds = x->seconds - y->seconds;
r->nanoseconds = x->nanoseconds - y->nanoseconds;
normalize_TimeInternal(r);
}
void div2_TimeInternal(TimeInternal *r)
{
r->nanoseconds += r->seconds % 2 * 1000000000;
r->seconds /= 2;
r->nanoseconds /= 2;
normalize_TimeInternal(r);
}
void set_TimeInternal(TimeInternal *t, Integer32 s, Integer32 ns)
{
t->seconds = s;
t->nanoseconds = ns;
}
void display_TimeInternal(const char *label, TimeInternal *t)
{
PP_VPRINTF("%s: %s%d.%09d \n", label,
(t->seconds < 0 || (t->seconds == 0 && t->nanoseconds < 0)) ? "-" : " ",
abs(t->seconds), abs(t->nanoseconds));
}
/*
* Aurelio Colosimo for CERN, 2011 -- GNU LGPL v2.1 or later
* Based on PTPd project v. 2.1.0 (see AUTHORS for details)
*/
#include <ppsi/ppsi.h>
#include <ppsi/diag.h>
#include "wr-api.h"
/* Flag Field bits symbolic names (table 57, pag. 151) */
#define FFB_LI61 0x01
#define FFB_LI59 0x02
#define FFB_UTCV 0x04
#define FFB_PTP 0x08
#define FFB_TTRA 0x10
#define FFB_FTRA 0x20
/* Local clock is becoming Master. Table 13 (9.3.5) of the spec. */
void m1(struct pp_instance *ppi)
{
/* Current data set update */
DSCUR(ppi)->stepsRemoved = 0;
DSCUR(ppi)->offsetFromMaster.nanoseconds = 0;
DSCUR(ppi)->offsetFromMaster.seconds = 0;
DSCUR(ppi)->meanPathDelay.nanoseconds = 0;
DSCUR(ppi)->meanPathDelay.seconds = 0;
/* Parent data set */
memcpy(DSPAR(ppi)->parentPortIdentity.clockIdentity,
DSDEF(ppi)->clockIdentity, PP_CLOCK_IDENTITY_LENGTH);
DSPAR(ppi)->parentPortIdentity.portNumber = 0;
DSPAR(ppi)->parentStats = PP_DEFAULT_PARENTS_STATS;
DSPAR(ppi)->observedParentClockPhaseChangeRate = 0;
DSPAR(ppi)->observedParentOffsetScaledLogVariance = 0;
memcpy(DSPAR(ppi)->grandmasterIdentity, DSDEF(ppi)->clockIdentity,
PP_CLOCK_IDENTITY_LENGTH);
DSPAR(ppi)->grandmasterClockQuality.clockAccuracy =
DSDEF(ppi)->clockQuality.clockAccuracy;
DSPAR(ppi)->grandmasterClockQuality.clockClass =
DSDEF(ppi)->clockQuality.clockClass;
DSPAR(ppi)->grandmasterClockQuality.offsetScaledLogVariance =
DSDEF(ppi)->clockQuality.offsetScaledLogVariance;
DSPAR(ppi)->grandmasterPriority1 = DSDEF(ppi)->priority1;
DSPAR(ppi)->grandmasterPriority2 = DSDEF(ppi)->priority2;
/* Time Properties data set */
DSPRO(ppi)->timeSource = INTERNAL_OSCILLATOR;
}
/* Local clock is synchronized to Ebest Table 16 (9.3.5) of the spec. */
void s1(struct pp_instance *ppi, MsgHeader *hdr, MsgAnnounce *ann)
{
/* Current DS */
DSCUR(ppi)->stepsRemoved = ann->stepsRemoved + 1;
/* Parent DS */
memcpy(DSPAR(ppi)->parentPortIdentity.clockIdentity,
hdr->sourcePortIdentity.clockIdentity,
PP_CLOCK_IDENTITY_LENGTH);
DSPAR(ppi)->parentPortIdentity.portNumber =
hdr->sourcePortIdentity.portNumber;
memcpy(DSPAR(ppi)->grandmasterIdentity,
ann->grandmasterIdentity, PP_CLOCK_IDENTITY_LENGTH);
DSPAR(ppi)->grandmasterClockQuality.clockAccuracy =
ann->grandmasterClockQuality.clockAccuracy;
DSPAR(ppi)->grandmasterClockQuality.clockClass =
ann->grandmasterClockQuality.clockClass;
DSPAR(ppi)->grandmasterClockQuality.offsetScaledLogVariance =
ann->grandmasterClockQuality.offsetScaledLogVariance;
DSPAR(ppi)->grandmasterPriority1 = ann->grandmasterPriority1;
DSPAR(ppi)->grandmasterPriority2 = ann->grandmasterPriority2;
/* Timeproperties DS */
DSPRO(ppi)->currentUtcOffset = ann->currentUtcOffset;
/* "Valid" is bit 2 in second octet of flagfield */
DSPRO(ppi)->currentUtcOffsetValid = ((hdr->flagField[1] & FFB_UTCV)
!= 0);
DSPRO(ppi)->leap59 = ((hdr->flagField[1] & FFB_LI59) != 0);
DSPRO(ppi)->leap61 = ((hdr->flagField[1] & FFB_LI61) != 0);
DSPRO(ppi)->timeTraceable = ((hdr->flagField[1] & FFB_TTRA) != 0);
DSPRO(ppi)->frequencyTraceable = ((hdr->flagField[1] & FFB_FTRA) != 0);
DSPRO(ppi)->ptpTimescale = ((hdr->flagField[1] & FFB_PTP) != 0);
DSPRO(ppi)->timeSource = ann->timeSource;
/* White Rabbit */
DSPOR(ppi)->parentIsWRnode = ((ann->wrFlags & WR_NODE_MODE) != NON_WR);
DSPOR(ppi)->parentWrModeOn =
(ann->wrFlags & WR_IS_WR_MODE) ? TRUE : FALSE;
DSPOR(ppi)->parentCalibrated =
((ann->wrFlags & WR_IS_CALIBRATED) ? 1 : 0);
DSPOR(ppi)->parentWrConfig = ann->wrFlags & WR_NODE_MODE;
DSCUR(ppi)->primarySlavePortNumber =
DSPOR(ppi)->portIdentity.portNumber;
}
/* Copy local data set into header and ann message. 9.3.4 table 12. */
void copy_d0( struct pp_instance *ppi, MsgHeader *hdr, MsgAnnounce *ann)
{
ann->grandmasterPriority1 = DSDEF(ppi)->priority1;
memcpy(ann->grandmasterIdentity, DSDEF(ppi)->clockIdentity,
PP_CLOCK_IDENTITY_LENGTH);
ann->grandmasterClockQuality.clockClass =
DSDEF(ppi)->clockQuality.clockClass;
ann->grandmasterClockQuality.clockAccuracy =
DSDEF(ppi)->clockQuality.clockAccuracy;
ann->grandmasterClockQuality.offsetScaledLogVariance =
DSDEF(ppi)->clockQuality.offsetScaledLogVariance;
ann->grandmasterPriority2 = DSDEF(ppi)->priority2;
ann->stepsRemoved = 0;
memcpy(hdr->sourcePortIdentity.clockIdentity,
DSDEF(ppi)->clockIdentity, PP_CLOCK_IDENTITY_LENGTH);
}
/*
* Data set comparison bewteen two foreign masters (9.3.4 fig 27)
* return similar to memcmp()
*/
Integer8 bmc_dataset_cmp(struct pp_instance *ppi,
MsgHeader *hdr_a, MsgAnnounce *ann_a,
MsgHeader *hdr_b, MsgAnnounce *ann_b)
{
short comp = 0;
Octet *ppci;
PP_VPRINTF("BMC: in bmc_dataset_cmp\n");
/* Identity comparison */
if (!memcmp(ann_a->grandmasterIdentity,
ann_b->grandmasterIdentity, PP_CLOCK_IDENTITY_LENGTH)) {
/* Algorithm part2 Fig 28 */
if (ann_a->stepsRemoved > ann_b->stepsRemoved + 1)
return 1;
else if (ann_b->stepsRemoved > ann_a->stepsRemoved + 1)
return -1;
else { /* A within 1 of B */
ppci = DSPAR(ppi)->parentPortIdentity.clockIdentity;
if (ann_a->stepsRemoved > ann_b->stepsRemoved) {
if (!memcmp(
hdr_a->sourcePortIdentity.clockIdentity,
ppci,
PP_CLOCK_IDENTITY_LENGTH)) {
PP_PRINTF("Sender=Receiver: Error -1");
return 0;
} else
return 1;
} else if (ann_b->stepsRemoved > ann_a->stepsRemoved) {
if (!memcmp(
hdr_b->sourcePortIdentity.clockIdentity,
ppci,
PP_CLOCK_IDENTITY_LENGTH)) {
PP_PRINTF("Sender=Receiver: Error -3");
return 0;
} else {
return -1;
}
} else { /* steps removed A == steps removed B */
if (!memcmp(
hdr_a->sourcePortIdentity.clockIdentity,
hdr_b->sourcePortIdentity.clockIdentity,
PP_CLOCK_IDENTITY_LENGTH)) {
PP_PRINTF("Sender=Receiver: Error -2");
return 0;
} else if ((memcmp(
hdr_a->sourcePortIdentity.clockIdentity,
hdr_b->sourcePortIdentity.clockIdentity,
PP_CLOCK_IDENTITY_LENGTH)) < 0)
return -1;
else
return 1;
}
}
} else { /* GrandMaster are not identical */
/* FIXME: rewrite in a more readable way */
if (ann_a->grandmasterPriority1 == ann_b->grandmasterPriority1) {
if (ann_a->grandmasterClockQuality.clockClass ==
ann_b->grandmasterClockQuality.clockClass) {
if (ann_a->grandmasterClockQuality.clockAccuracy == ann_b->grandmasterClockQuality.clockAccuracy) {
if (ann_a->grandmasterClockQuality.offsetScaledLogVariance == ann_b->grandmasterClockQuality.offsetScaledLogVariance) {
if (ann_a->grandmasterPriority2 == ann_b->grandmasterPriority2) {
comp = memcmp(ann_a->grandmasterIdentity, ann_b->grandmasterIdentity, PP_CLOCK_IDENTITY_LENGTH);
if (comp < 0)
return -1;
else if (comp > 0)
return 1;
else
return 0;
} else {
/* Priority2 are not identical */
comp = memcmp(&ann_a->grandmasterPriority2, &ann_b->grandmasterPriority2, 1);
if (comp < 0)
return -1;
else if (comp > 0)
return 1;
else
return 0;
}
} else {
/* offsetScaledLogVariance are not identical */
comp = memcmp(&ann_a->grandmasterClockQuality.clockClass, &ann_b->grandmasterClockQuality.clockClass, 1);
if (comp < 0)
return -1;
else if (comp > 0)
return 1;
else
return 0;
}
} else { /* Accuracy are not identitcal */
comp = memcmp(&ann_a->grandmasterClockQuality.clockAccuracy, &ann_b->grandmasterClockQuality.clockAccuracy, 1);
if (comp < 0)
return -1;
else if (comp > 0)
return 1;
else
return 0;
}
} else { /* ClockClass are not identical */
comp = memcmp(&ann_a->grandmasterClockQuality.clockClass, &ann_b->grandmasterClockQuality.clockClass, 1);
if (comp < 0)
return -1;
else if (comp > 0)
return 1;
else
return 0;
}
} else { /* Priority1 are not identical */
comp = memcmp(&ann_a->grandmasterPriority1, &ann_b->grandmasterPriority1, 1);
if (comp < 0)
return -1;
else if (comp > 0)
return 1;
else
return 0;
}
}
}
/* State decision algorithm 9.3.3 Fig 26 */
UInteger8 bmc_state_decision( struct pp_instance *ppi,
MsgHeader *hdr, MsgAnnounce *ann)
{
int cmpres;
if (OPTS(ppi)->master_only) {
m1(ppi);
return PPS_MASTER;
}
if (OPTS(ppi)->slave_only) {
s1(ppi, hdr, ann);
return PPS_SLAVE;
}
if ((!ppi->number_foreign_records) && (ppi->state == PPS_LISTENING))
return PPS_LISTENING;
copy_d0(ppi, &ppi->msg_tmp_header, &ppi->msg_tmp.announce);
cmpres = bmc_dataset_cmp(ppi,
&ppi->msg_tmp_header,
&ppi->msg_tmp.announce,
hdr, ann);
if (DSDEF(ppi)->clockQuality.clockClass < 128) {
if (cmpres < 0) {
m1(ppi);
return PPS_MASTER;
} else if (cmpres > 0) {
s1(ppi, hdr, ann);
return PPS_PASSIVE;
}
} else {
if (cmpres < 0) {
m1(ppi);
return PPS_MASTER;
} else if (cmpres > 0) {
s1(ppi, hdr, ann);
return PPS_SLAVE;
}
}
if (cmpres == 0) {
PP_PRINTF("Error in bmc_state_decision, cmpres=0.\n");
}
/* MB: Is this the return code below correct? */
/* Anyway, it's a valid return code. */
return PPS_FAULTY;
}
UInteger8 bmc(struct pp_instance *ppi, struct pp_frgn_master *frgn_master)
{
Integer16 i, best;
if (!ppi->number_foreign_records)
if (ppi->state == PPS_MASTER) {
m1(ppi);
return ppi->state;
}
for (i = 1, best = 0; i < ppi->number_foreign_records; i++)
if (bmc_dataset_cmp(ppi,
&frgn_master[i].hdr,
&frgn_master[i].ann,
&frgn_master[best].hdr,
&frgn_master[best].ann) < 0)
best = i;
PP_VPRINTF("bmc, best record : %d\n", best);
ppi->foreign_record_best = best;
return bmc_state_decision(ppi, &frgn_master[best].hdr,
&frgn_master[best].ann);
}
/*
* Aurelio Colosimo for CERN, 2011 -- GNU LGPL v2.1 or later
* Based on PTPd project v. 2.1.0 (see AUTHORS for details)
*/
#include <ppsi/ppsi.h>
#include <ppsi/diag.h>
#include "common-fun.h"
#include "wr-api.h"
int st_com_execute_slave(struct pp_instance *ppi, int check_delayreq)
{
int ret = 0;
if (DSPOR(ppi)->doRestart) {
ppi->next_state = PPS_INITIALIZING;
st_com_restart_annrec_timer(ppi);
DSPOR(ppi)->doRestart = FALSE;
return 0;
}
if (pp_timer_expired(ppi->timers[PP_TIMER_ANN_RECEIPT])) {
PP_VPRINTF("event ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES\n");
ppi->number_foreign_records = 0;
ppi->foreign_record_i = 0;
if (!DSDEF(ppi)->slaveOnly &&
DSDEF(ppi)->clockQuality.clockClass != 255) {
m1(ppi);
ppi->next_state = PPS_MASTER;
} else {
ppi->next_state = PPS_LISTENING;
st_com_restart_annrec_timer(ppi);
}
}
if (!check_delayreq)
return 0;
if (OPTS(ppi)->e2e_mode) {
if (pp_timer_expired(ppi->timers[PP_TIMER_DELAYREQ])) {
PP_VPRINTF("event DELAYREQ_INTERVAL_TIMEOUT_EXPIRES\n");
ret = msg_issue_delay_req(ppi);
ppi->delay_req_send_time = ppi->last_snt_time;
/* Add latency */
add_TimeInternal(&ppi->delay_req_send_time,
&ppi->delay_req_send_time,
&OPTS(ppi)->outbound_latency);
}
} else {
if (pp_timer_expired(ppi->timers[PP_TIMER_PDELAYREQ]))
{
PP_VPRINTF("event PDELAYREQ_INTERVAL_TOUT_EXPIRES\n");
ret = msg_issue_pdelay_req(ppi);
}
}
return ret;
}
void st_com_restart_annrec_timer(struct pp_instance *ppi)
{
/* 0 <= logAnnounceInterval <= 4, see pag. 237 of spec */
if (DSPOR(ppi)->logAnnounceInterval < 0)
PP_PRINTF("Error: logAnnounceInterval < 0");
pp_timer_start(((DSPOR(ppi)->announceReceiptTimeout) <<
DSPOR(ppi)->logAnnounceInterval) * 1000,
ppi->timers[PP_TIMER_ANN_RECEIPT]);
}
int st_com_check_record_update(struct pp_instance *ppi)
{
if (ppi->record_update) {
PP_VPRINTF("event STATE_DECISION_EVENT\n");
ppi->record_update = FALSE;
ppi->next_state = bmc(ppi, ppi->frgn_master);
if (ppi->next_state != ppi->state)
return 1;
}
return 0;
}
void st_com_add_foreign(struct pp_instance *ppi, unsigned char *buf)
{
int i, j;
int found = 0;
MsgHeader *hdr = &ppi->msg_tmp_header;
j = ppi->foreign_record_best;
/* Check if foreign master is already known */
for (i = 0; i < ppi->number_foreign_records; i++) {
if (!memcmp(hdr->sourcePortIdentity.clockIdentity,
ppi->frgn_master[j].port_identity.
clockIdentity,
PP_CLOCK_IDENTITY_LENGTH) &&
(hdr->sourcePortIdentity.portNumber ==
ppi->frgn_master[j].port_identity.portNumber))
{
/* Foreign Master is already in Foreign master data set
*/
ppi->frgn_master[j].ann_messages++;
found = 1;
PP_VPRINTF("st_com_add_foreign: ann_messages: %d\n",
ppi->frgn_master[j].ann_messages);
msg_copy_header(&ppi->frgn_master[j].hdr, hdr);
msg_unpack_announce(buf, &ppi->frgn_master[j].ann);
break;
}
j = (j + 1) % ppi->number_foreign_records;
}
/* Old foreign master */
if (found)
return;
/* New foreign master */
if (ppi->number_foreign_records <
OPTS(ppi)->max_foreign_records) {
ppi->number_foreign_records++;
}
j = ppi->foreign_record_i;
/* Copy new foreign master data set from announce message */
memcpy(ppi->frgn_master[j].port_identity.clockIdentity,
hdr->sourcePortIdentity.clockIdentity,
PP_CLOCK_IDENTITY_LENGTH);
ppi->frgn_master[j].port_identity.portNumber =
hdr->sourcePortIdentity.portNumber;
ppi->frgn_master[j].ann_messages = 0;
/*
* header and announce field of each Foreign Master are
* usefull to run Best Master Clock Algorithm
*/
msg_copy_header(&ppi->frgn_master[j].hdr, hdr);
msg_unpack_announce(buf, &ppi->frgn_master[j].ann);
PP_VPRINTF("New foreign Master added \n");
ppi->foreign_record_i = (ppi->foreign_record_i+1) %
OPTS(ppi)->max_foreign_records;
}
int st_com_slave_handle_announce(struct pp_instance *ppi, unsigned char *buf,
int len)
{
MsgHeader *hdr = &ppi->msg_tmp_header;
if (len < PP_ANNOUNCE_LENGTH)
return -1;
if (ppi->is_from_self)
return 0;
/*
* Valid announce message is received : BMC algorithm
* will be executed
*/
ppi->record_update = TRUE;
if (ppi->is_from_cur_par) {
msg_unpack_announce(buf, &ppi->msg_tmp.announce);
s1(ppi, hdr, &ppi->msg_tmp.announce);
} else {
/* st_com_add_foreign takes care of announce unpacking */
st_com_add_foreign(ppi, buf);
}
if ((DSPOR(ppi)->wrConfig & WR_S_ONLY) &&
(1 /* FIXME: Recommended State, see page 33*/) &&
(DSPOR(ppi)->parentWrConfig & WR_M_ONLY) &&
(!DSPOR(ppi)->wrModeOn || !DSPOR(ppi)->parentWrModeOn))
ppi->next_state = WRS_PRESENT;
/*Reset Timer handling Announce receipt timeout*/
st_com_restart_annrec_timer(ppi);
return 0;
}
int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf,
int len)
{
TimeInternal *time;
TimeInternal origin_tstamp;
TimeInternal correction_field;
MsgHeader *hdr = &ppi->msg_tmp_header;
if (len < PP_SYNC_LENGTH)
return -1;
if (ppi->is_from_self)
return 0;
time = &ppi->last_rcv_time;
if (ppi->is_from_cur_par) {
ppi->sync_receive_time = *time;
/* FIXME diag to file? will we ever handle it?
if (OPTS(ppi)->recordFP)
fprintf(rtOpts->recordFP, "%d %llu\n",
header->sequenceId,
((time->seconds * 1000000000ULL) +
time->nanoseconds));
*/
if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0) {
ppi->waiting_for_follow = TRUE;
ppi->recv_sync_sequence_id = hdr->sequenceId;
/* Save correctionField of Sync message */
int64_to_TimeInternal(
hdr->correctionfield,
&correction_field);
ppi->last_sync_corr_field.seconds =
correction_field.seconds;
ppi->last_sync_corr_field.nanoseconds =
correction_field.nanoseconds;
} else {
msg_unpack_sync(buf, &ppi->msg_tmp.sync);
int64_to_TimeInternal(
ppi->msg_tmp_header.correctionfield,
&correction_field);
display_TimeInternal("Correction field",
&correction_field);
ppi->waiting_for_follow = FALSE;
to_TimeInternal(&origin_tstamp,
&ppi->msg_tmp.sync.originTimestamp);
pp_update_offset(ppi, &origin_tstamp,
&ppi->sync_receive_time,
&correction_field);
pp_update_clock(ppi);
}
}
return 0;
}
int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
int len)
{
TimeInternal precise_orig_timestamp;
TimeInternal correction_field;
MsgHeader *hdr = &ppi->msg_tmp_header;
if (len < PP_FOLLOW_UP_LENGTH)
return -1;
if (!ppi->is_from_cur_par) {
PP_VPRINTF("SequenceID doesn't match last Sync message\n");
return 0;
}
if (!ppi->waiting_for_follow) {
PP_VPRINTF("Slave was not waiting a follow up message\n");
return 0;
}
if (ppi->recv_sync_sequence_id != hdr->sequenceId) {
PP_VPRINTF("Follow up message is not from current parent\n");
return 0;
}
msg_unpack_follow_up(buf, &ppi->msg_tmp.follow);
ppi->waiting_for_follow = FALSE;
to_TimeInternal(&precise_orig_timestamp,
&ppi->msg_tmp.follow.preciseOriginTimestamp);
int64_to_TimeInternal(ppi->msg_tmp_header.correctionfield,
&correction_field);
add_TimeInternal(&correction_field, &correction_field,
&ppi->last_sync_corr_field);
if (!DSPOR(ppi)->wrModeOn) {
pp_update_offset(ppi, &precise_orig_timestamp,
&ppi->sync_receive_time,
&correction_field);
pp_update_clock(ppi);
}
else {
precise_orig_timestamp.phase = 0;
wr_servo_got_sync(ppi, &precise_orig_timestamp,
&ppi->sync_receive_time);
/* TODO Check: generates unstability (Tx timestamp invalid) */
/* return msg_issue_delay_req(ppi); */
}
return 0;
}
int st_com_handle_pdelay_req(struct pp_instance *ppi, unsigned char *buf,
int len)
{
TimeInternal *time;
MsgHeader *hdr = &ppi->msg_tmp_header;
if (len < PP_PDELAY_REQ_LENGTH)
return -1;
if (OPTS(ppi)->e2e_mode)
return 0;
time = &ppi->last_rcv_time;
if (ppi->is_from_self) {
/* Get sending timestamp from IP stack
* with So_TIMESTAMP */
ppi->pdelay_req_send_time.seconds =
time->seconds;
ppi->pdelay_req_send_time.nanoseconds =
time->nanoseconds;
/*Add latency*/
add_TimeInternal(&ppi->pdelay_req_send_time,
&ppi->pdelay_req_send_time,
&OPTS(ppi)->outbound_latency);
} else {
msg_copy_header(&ppi->pdelay_req_hdr, hdr);
return msg_issue_pdelay_resp(ppi, time, hdr);
}
return 0;
}
int st_com_master_handle_announce(struct pp_instance *ppi, unsigned char *buf,
int len)
{
if (len < PP_ANNOUNCE_LENGTH)
return -1;
if (ppi->is_from_self) {
PP_VPRINTF("master handle_announce: ignore msg from self\n");
return 0;
}
PP_VPRINTF("Announce message from another foreign master\n");
st_com_add_foreign(ppi, buf);
ppi->record_update = TRUE;
return 0;
}
int st_com_master_handle_sync(struct pp_instance *ppi, unsigned char *buf,
int len)
{
/* No more used: follow up is sent right after the corresponding sync */
return 0;
}
/*
* Aurelio Colosimo for CERN, 2011 -- GNU LGPL v2.1 or later
* Based on PTPd project v. 2.1.0 (see AUTHORS for details)
*/
#ifndef __WREXT_COMMON_FUN_H__
#define __WREXT_COMMON_FUN_H__
#include <ppsi/ppsi.h>
/* Contains all functions common to more than one state */
/* The following macros are used both by msg.c and by wr-msg.c */
#ifdef VERB_LOG_MSGS
#define MSG_SEND_AND_RET_VARLEN(x,y,z,w)\
if (pp_send_packet(ppi, ppi->buf_out, w,\
&ppi->last_snt_time, PP_NP_##y , z) < w) {\
PP_PRINTF("%s(%d) Message can't be sent -> FAULTY state!\n",\
pp_msg_names[PPM_##x], PPM_##x);\
return -1;\
}\
PP_VPRINTF("SENT %02d %d.%d %s \n", w,\
ppi->last_snt_time.seconds,\
ppi->last_snt_time.nanoseconds,pp_msg_names[PPM_##x]);\
ppi->sent_seq_id[PPM_## x]++;\
return 0;
#else
#define MSG_SEND_AND_RET_VARLEN(x,y,z,w)\
if (pp_send_packet(ppi, ppi->buf_out, w,\
&ppi->last_snt_time, PP_NP_##y , z) < w) {\
return -1;\
}\
ppi->sent_seq_id[PPM_## x]++;\
return 0;
#endif
#define MSG_SEND_AND_RET(x,y,z)\
MSG_SEND_AND_RET_VARLEN(x,y,z,PP_## x ##_LENGTH)
/* returns -1 in case of error, see below */
int st_com_execute_slave(struct pp_instance *ppi, int check_delayreq);
void st_com_restart_annrec_timer(struct pp_instance *ppi);
int st_com_check_record_update(struct pp_instance *ppi);
/* Each of the following "handle" functions" return 0 in case of correct
* message, -1 in case the message contained in buf is not proper (e.g. size
* is not the expected one
*/
int st_com_slave_handle_announce(struct pp_instance *ppi, unsigned char *buf,
int len);
int st_com_master_handle_announce(struct pp_instance *ppi, unsigned char *buf,
int len);
int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf,
int len);
int st_com_master_handle_sync(struct pp_instance *ppi, unsigned char *buf,
int len);
int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
int len);
int st_com_handle_pdelay_req(struct pp_instance *ppi, unsigned char *buf,
int len);
#endif /* __WREXT_COMMON_FUN_H__ */
#include <ppsi/ppsi.h>
#include "wr-api.h"
#include "../proto-standard/common-fun.h"
/* ext-whiterabbit must offer its own hooks */
static int wr_init(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
struct wr_dsport *wp = WR_DSPOR(ppi);
wp->wrStateTimeout = WR_DEFAULT_STATE_TIMEOUT_MS;
wp->wrStateRetry = WR_DEFAULT_STATE_REPEAT;
wp->calPeriod = WR_DEFAULT_CAL_PERIOD;
wp->wrModeOn = 0;
wp->parentWrConfig = 0;
wp->calibrated = !WR_DEFAULT_PHY_CALIBRATION_REQUIRED;
return 0;
}
/* This currently only works with one interface (i.e. WR Node) */
static int wr_open(struct pp_instance *ppi, struct pp_runtime_opts *rt_opts)
{
static struct wr_data_t wr_data; /* WR-specific global data */
rt_opts->iface_name = "wr1";
ppi->ext_data = &wr_data;
return 0;
}
static int wr_listening(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
struct wr_dsport *wp = WR_DSPOR(ppi);
wp->wrMode = NON_WR;
wp->wrPortState = WRS_IDLE;
return 0;
}
static int wr_master_msg(struct pp_instance *ppi, unsigned char *pkt, int plen,
int msgtype)
{
MsgHeader *hdr = &ppi->msg_tmp_header;
MsgSignaling wrsig_msg;
TimeInternal *time = &ppi->last_rcv_time;
;
switch(msgtype) {
/* This case is modified from the default one */
case PPM_DELAY_REQ:
msg_copy_header(&ppi->delay_req_hdr, hdr);
ppi->delay_req_hdr.correctionfield.msb = 0;
ppi->delay_req_hdr.correctionfield.lsb =
phase_to_cf_units(ppi->last_rcv_time.phase);
msg_issue_delay_resp(ppi, time);
msgtype = PPM_NOTHING_TO_DO;
break;
/* This is missing in the standard protocol */
case PPM_SIGNALING:
msg_unpack_wrsig(ppi, pkt, &wrsig_msg,
&(WR_DSPOR(ppi)->msgTmpWrMessageID));
if ((WR_DSPOR(ppi)->msgTmpWrMessageID == SLAVE_PRESENT) &&
(WR_DSPOR(ppi)->wrConfig & WR_M_ONLY))
ppi->next_state = WRS_M_LOCK;
msgtype = PPM_NOTHING_TO_DO;
break;
}
return msgtype;
}
static int wr_new_slave(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
wr_servo_init(ppi);
return 0;
}
static int wr_update_delay(struct pp_instance *ppi)
{
MsgHeader *hdr = &ppi->msg_tmp_header;
TimeInternal correction_field;
int64_to_TimeInternal(hdr->correctionfield, &correction_field);
/* If no WR mode is on, run normal code */
if (!WR_DSPOR(ppi)->wrModeOn) {
pp_update_delay(ppi, &correction_field);
return 0;
}
wr_servo_got_delay(ppi, hdr->correctionfield.lsb);
wr_servo_update(ppi);
return 0;
}
static void wr_s1(struct pp_instance *ppi, MsgHeader *hdr, MsgAnnounce *ann)
{
WR_DSPOR(ppi)->parentIsWRnode =
((ann->wrFlags & WR_NODE_MODE) != NON_WR);
WR_DSPOR(ppi)->parentWrModeOn =
(ann->wrFlags & WR_IS_WR_MODE) ? TRUE : FALSE;
WR_DSPOR(ppi)->parentCalibrated =
((ann->wrFlags & WR_IS_CALIBRATED) ? 1 : 0);
WR_DSPOR(ppi)->parentWrConfig = ann->wrFlags & WR_NODE_MODE;
DSCUR(ppi)->primarySlavePortNumber =
DSPOR(ppi)->portIdentity.portNumber;
}
static int wr_execute_slave(struct pp_instance *ppi)
{
if (!WR_DSPOR(ppi)->doRestart)
return 0;
ppi->next_state = PPS_INITIALIZING;
st_com_restart_annrec_timer(ppi);
WR_DSPOR(ppi)->doRestart = FALSE;
return 1; /* the caller returns too */
}
static void wr_handle_announce(struct pp_instance *ppi)
{
if ((WR_DSPOR(ppi)->wrConfig & WR_S_ONLY) &&
(1 /* FIXME: Recommended State, see page 33*/) &&
(WR_DSPOR(ppi)->parentWrConfig & WR_M_ONLY) &&
(!WR_DSPOR(ppi)->wrModeOn || !WR_DSPOR(ppi)->parentWrModeOn))
ppi->next_state = WRS_PRESENT;
}
static int wr_handle_followup(struct pp_instance *ppi,
TimeInternal *precise_orig_timestamp,
TimeInternal *correction_field)
{
if (!WR_DSPOR(ppi)->wrModeOn)
return 0;
precise_orig_timestamp->phase = 0;
wr_servo_got_sync(ppi, precise_orig_timestamp,
&ppi->sync_receive_time);
/* TODO Check: generates unstability (Tx timestamp invalid) */
/* return msg_issue_delay_req(ppi); */
return 1; /* the caller returns too */
}
int wr_pack_announce(struct pp_instance *ppi)
{
if (WR_DSPOR(ppi)->wrConfig != NON_WR &&
WR_DSPOR(ppi)->wrConfig != WR_S_ONLY) {
msg_pack_announce_wr_tlv(ppi);
return WR_ANNOUNCE_LENGTH;
}
return PP_ANNOUNCE_LENGTH;
}
void wr_unpack_announce(void *buf, MsgAnnounce *ann)
{
int msg_len = htons(*(UInteger16 *) (buf + 2));
if (msg_len > PP_ANNOUNCE_LENGTH) {
msg_unpack_announce_wr_tlv(buf, ann);
}
}
struct pp_ext_hooks pp_hooks = {
.init = wr_init,
.open = wr_open,
.listening = wr_listening,
.master_msg = wr_master_msg,
.new_slave = wr_new_slave,
.update_delay = wr_update_delay,
.s1 = wr_s1,
.execute_slave = wr_execute_slave,
.handle_announce = wr_handle_announce,
.handle_followup = wr_handle_followup,
.pack_announce = wr_pack_announce,
.unpack_announce = wr_unpack_announce,
};
This diff is collapsed.
/*
* Aurelio Colosimo for CERN, 2011 -- GNU LGPL v2.1 or later
*/
#include <ppsi/ppsi.h>
#include <ppsi/diag.h>
#include "wr-api.h"
/*
* This file deals with opening and closing an instance. The channel
* must already have been created. In practices, this initializes the
* state machine to the first state.
*
* A protocol extension can override none or both of these functions.
*/
struct pp_runtime_opts default_rt_opts = {
.clock_quality = {
.clockClass = PP_DEFAULT_CLOCK_CLASS,
.clockAccuracy = PP_DEFAULT_CLOCK_ACCURACY,
.offsetScaledLogVariance = PP_DEFAULT_CLOCK_VARIANCE,
},
.iface_name = "wr1",
.inbound_latency = {0, PP_DEFAULT_INBOUND_LATENCY},
.outbound_latency = {0, PP_DEFAULT_OUTBOUND_LATENCY},
.max_rst = PP_DEFAULT_MAX_RESET,
.max_dly = PP_DEFAULT_MAX_DELAY,
.no_adjust = PP_DEFAULT_NO_ADJUST,
.no_rst_clk = PP_DEFAULT_NO_RESET_CLOCK,
.ap = PP_DEFAULT_AP,
.ai = PP_DEFAULT_AI,
.s = PP_DEFAULT_DELAY_S,
.ethernet_mode = PP_DEFAULT_ETHERNET_MODE,
.e2e_mode = PP_DEFAULT_E2E_MODE,
.gptp_mode = PP_DEFAULT_GPTP_MODE,
.ofst_first_updated = 0, /* FIXME: is it a option or a state var? */
.max_foreign_records = PP_DEFAULT_MAX_FOREIGN_RECORDS,
.cur_utc_ofst = PP_DEFAULT_UTC_OFFSET,
.announce_intvl = PP_DEFAULT_ANNOUNCE_INTERVAL,
.sync_intvl = PP_DEFAULT_SYNC_INTERVAL,
.prio1 = PP_DEFAULT_PRIORITY1,
.prio2 = PP_DEFAULT_PRIORITY2,
.domain_number = PP_DEFAULT_DOMAIN_NUMBER,
.ttl = PP_DEFAULT_TTL,
};
static struct wr_data_t wr_data; /* white rabbit specific global data */
int pp_open_instance(struct pp_instance *ppi, struct pp_runtime_opts *rt_opts)
{
if (rt_opts)
ppi->rt_opts = rt_opts;
else
ppi->rt_opts = &default_rt_opts;
ppi->ext_data = &wr_data;
ppi->state = PPS_INITIALIZING;
return 0;
}
int pp_close_instance(struct pp_instance *ppi)
{
/* Nothing to do by now */
return 0;
}
/*
* Aurelio Colosimo for CERN, 2011 -- GNU LGPL v2.1 or later
* Based on PTPd project v. 2.1.0 (see AUTHORS for details)
*/
#include <ppsi/ppsi.h>
#include <ppsi/diag.h>
void pp_init_clock(struct pp_instance *ppi)
{
PP_PRINTF("SERVO init clock\n");
/* clear vars */
set_TimeInternal(&SRV(ppi)->m_to_s_dly, 0, 0);
set_TimeInternal(&SRV(ppi)->s_to_m_dly, 0, 0);
SRV(ppi)->obs_drift = 0; /* clears clock servo accumulator (the
* I term) */
SRV(ppi)->owd_fltr.s_exp = 0; /* clears one-way delay filter */
/* level clock */
if (!OPTS(ppi)->no_adjust)
pp_adj_freq(0);
}
void pp_update_delay(struct pp_instance *ppi, TimeInternal *correction_field)
{
TimeInternal s_to_m_dly;
struct pp_owd_fltr *owd_fltr = &SRV(ppi)->owd_fltr;
/* calc 'slave to master' delay */
sub_TimeInternal(&s_to_m_dly, &ppi->delay_req_receive_time,
&ppi->delay_req_send_time);
if (OPTS(ppi)->max_dly) { /* If max_delay is 0 then it's OFF */
if (s_to_m_dly.seconds) {
#ifdef VERB_LOG_MSGS
PP_VPRINTF("pp_update_delay aborted, delay greater than 1"
" second\n");
#endif
return;
}
if (s_to_m_dly.nanoseconds > OPTS(ppi)->max_dly) {
#ifdef VERB_LOG_MSGS
PP_VPRINTF("pp_update_delay aborted, delay %d greater than "
"administratively set maximum %d\n",
s_to_m_dly.nanoseconds,
OPTS(ppi)->max_dly);
#endif
return;
}
}
if (OPTS(ppi)->ofst_first_updated) {
Integer16 s;
/* calc 'slave to_master' delay (master to slave delay is
* already computed in pp_update_offset)
*/
sub_TimeInternal(&SRV(ppi)->delay_sm,
&ppi->delay_req_receive_time,
&ppi->delay_req_send_time);
/* update 'one_way_delay' */
add_TimeInternal(&DSCUR(ppi)->meanPathDelay,
&SRV(ppi)->delay_sm, &SRV(ppi)->delay_ms);
/* Substract correction_field */
sub_TimeInternal(&DSCUR(ppi)->meanPathDelay,
&DSCUR(ppi)->meanPathDelay, correction_field);
/* Compute one-way delay */
div2_TimeInternal(&DSCUR(ppi)->meanPathDelay);
if (DSCUR(ppi)->meanPathDelay.seconds) {
/* cannot filter with secs, clear filter */
owd_fltr->s_exp = 0;
owd_fltr->nsec_prev = 0;
return;
}
/* avoid overflowing filter */
s = OPTS(ppi)->s;
while (abs(owd_fltr->y) >> (31 - s))
--s;
/* crank down filter cutoff by increasing 's_exp' */
if (owd_fltr->s_exp < 1)
owd_fltr->s_exp = 1;
else if (owd_fltr->s_exp < 1 << s)
++owd_fltr->s_exp;
else if (owd_fltr->s_exp > 1 << s)
owd_fltr->s_exp = 1 << s;
/* filter 'meanPathDelay' */
owd_fltr->y = (owd_fltr->s_exp - 1) *
owd_fltr->y / owd_fltr->s_exp +
(DSCUR(ppi)->meanPathDelay.nanoseconds / 2 +
owd_fltr->nsec_prev / 2) / owd_fltr->s_exp;
owd_fltr->nsec_prev = DSCUR(ppi)->meanPathDelay.nanoseconds;
DSCUR(ppi)->meanPathDelay.nanoseconds = owd_fltr->y;
PP_VPRINTF("delay filter %d, %d\n",
owd_fltr->y, owd_fltr->s_exp);
}
}
void pp_update_peer_delay(struct pp_instance *ppi,
TimeInternal *correction_field, int two_step)
{
Integer16 s;
struct pp_owd_fltr *owd_fltr = &SRV(ppi)->owd_fltr;
if (two_step) {
/* calc 'slave_to_master_delay' */
sub_TimeInternal(&SRV(ppi)->pdelay_ms,
&ppi->pdelay_resp_receive_time,
&ppi->pdelay_resp_send_time);
sub_TimeInternal(&SRV(ppi)->pdelay_sm,
&ppi->pdelay_req_receive_time,
&ppi->pdelay_req_send_time);
/* update 'one_way_delay' */
add_TimeInternal(&DSPOR(ppi)->peerMeanPathDelay,
&SRV(ppi)->pdelay_ms,
&SRV(ppi)->pdelay_sm);
/* Substract correctionField */
sub_TimeInternal(&DSPOR(ppi)->peerMeanPathDelay,
&DSPOR(ppi)->peerMeanPathDelay, correction_field);
/* Compute one-way delay */
div2_TimeInternal(&DSPOR(ppi)->peerMeanPathDelay);
} else {
/* One step clock */
sub_TimeInternal(&DSPOR(ppi)->peerMeanPathDelay,
&ppi->pdelay_resp_receive_time,
&ppi->pdelay_req_send_time);
/* Substract correctionField */
sub_TimeInternal(&DSPOR(ppi)->peerMeanPathDelay,
&DSPOR(ppi)->peerMeanPathDelay, correction_field);
/* Compute one-way delay */
div2_TimeInternal(&DSPOR(ppi)->peerMeanPathDelay);
}
if (DSPOR(ppi)->peerMeanPathDelay.seconds) {
/* cannot filter with secs, clear filter */
owd_fltr->s_exp = owd_fltr->nsec_prev = 0;
return;
}
/* avoid overflowing filter */
s = OPTS(ppi)->s;
while (abs(owd_fltr->y) >> (31 - s))
--s;
/* crank down filter cutoff by increasing 's_exp' */
if (owd_fltr->s_exp < 1)
owd_fltr->s_exp = 1;
else if (owd_fltr->s_exp < 1 << s)
++owd_fltr->s_exp;
else if (owd_fltr->s_exp > 1 << s)
owd_fltr->s_exp = 1 << s;
/* filter 'meanPathDelay' */
owd_fltr->y = (owd_fltr->s_exp - 1) *
owd_fltr->y / owd_fltr->s_exp +
(DSPOR(ppi)->peerMeanPathDelay.nanoseconds / 2 +
owd_fltr->nsec_prev / 2) / owd_fltr->s_exp;
owd_fltr->nsec_prev = DSPOR(ppi)->peerMeanPathDelay.nanoseconds;
DSPOR(ppi)->peerMeanPathDelay.nanoseconds = owd_fltr->y;
}
void pp_update_offset(struct pp_instance *ppi, TimeInternal *send_time,
TimeInternal *recv_time, TimeInternal *correction_field)
{
TimeInternal m_to_s_dly;
struct pp_ofm_fltr *ofm_fltr = &SRV(ppi)->ofm_fltr;
/* calc 'master_to_slave_delay' */
sub_TimeInternal(&m_to_s_dly, recv_time, send_time);
if (OPTS(ppi)->max_dly) { /* If maxDelay is 0 then it's OFF */
if (m_to_s_dly.seconds) {
#ifdef VERB_LOG_MSGS
PP_PRINTF("pp_update_offset aborted, delay greater "
"than 1 second\n");
#endif
return;
}
if (m_to_s_dly.nanoseconds > OPTS(ppi)->max_dly) {
#ifdef VERB_LOG_MSGS
PP_PRINTF("updateDelay aborted, delay %d greater than "
"administratively set maximum %d\n",
m_to_s_dly.nanoseconds,
OPTS(ppi)->max_dly);
#endif
return;
}
}
SRV(ppi)->m_to_s_dly = m_to_s_dly;
sub_TimeInternal(&SRV(ppi)->delay_ms, recv_time, send_time);
/* Used just for End to End mode. */
/* Take care about correctionField */
sub_TimeInternal(&SRV(ppi)->m_to_s_dly,
&SRV(ppi)->m_to_s_dly, correction_field);
/* update 'offsetFromMaster' */
if (!OPTS(ppi)->e2e_mode) {
sub_TimeInternal(&DSCUR(ppi)->offsetFromMaster,
&SRV(ppi)->m_to_s_dly,
&DSPOR(ppi)->peerMeanPathDelay);
} else {
/* (End to End mode) */
sub_TimeInternal(&DSCUR(ppi)->offsetFromMaster,
&SRV(ppi)->m_to_s_dly,
&DSCUR(ppi)->meanPathDelay);
}
if (DSCUR(ppi)->offsetFromMaster.seconds) {
/* cannot filter with secs, clear filter */
ofm_fltr->nsec_prev = 0;
return;
}
/* filter 'offsetFromMaster' */
ofm_fltr->y = DSCUR(ppi)->offsetFromMaster.nanoseconds / 2 +
ofm_fltr->nsec_prev / 2;
ofm_fltr->nsec_prev = DSCUR(ppi)->offsetFromMaster.nanoseconds;
DSCUR(ppi)->offsetFromMaster.nanoseconds = ofm_fltr->y;
/* Offset must have been computed at least one time before
* computing end to end delay */
if (!OPTS(ppi)->ofst_first_updated)
OPTS(ppi)->ofst_first_updated = 1;
}
void pp_update_clock(struct pp_instance *ppi)
{
Integer32 adj;
TimeInternal time_tmp;
uint32_t tstamp_diff;
if (OPTS(ppi)->max_rst) { /* If max_rst is 0 then it's OFF */
if (DSCUR(ppi)->offsetFromMaster.seconds) {
#ifdef VERB_LOG_MSGS
PP_PRINTF("pp_update_clock aborted, offset greater "
"than 1 second\n");
goto display;
#endif
}
if (DSCUR(ppi)->offsetFromMaster.nanoseconds >
OPTS(ppi)->max_rst) {
#ifdef VERB_LOG_MSGS
PP_VPRINTF("pp_update_clock aborted, offset %d greater than "
"administratively set maximum %d\n",
DSCUR(ppi)->offsetFromMaster.nanoseconds,
OPTS(ppi)->max_rst);
goto display;
#endif
}
}
if (DSCUR(ppi)->offsetFromMaster.seconds) {
/* if secs, reset clock or set freq adjustment to max */
if (!OPTS(ppi)->no_adjust) {
if (!OPTS(ppi)->no_rst_clk) {
pp_get_tstamp(&time_tmp);
sub_TimeInternal(&time_tmp, &time_tmp,
&DSCUR(ppi)->offsetFromMaster);
tstamp_diff = pp_set_tstamp(&time_tmp);
pp_timer_adjust_all(ppi, tstamp_diff);
pp_init_clock(ppi);
} else {
adj = DSCUR(ppi)->offsetFromMaster.nanoseconds
> 0 ? PP_ADJ_FREQ_MAX:-PP_ADJ_FREQ_MAX;
pp_adj_freq(-adj);
}
}
} else {
static int dc = 0;
/* the PI controller */
/* no negative or zero attenuation */
if (OPTS(ppi)->ap < 1)
OPTS(ppi)->ap = 1;
if (OPTS(ppi)->ai < 1)
OPTS(ppi)->ai = 1;
/* the accumulator for the I component */
SRV(ppi)->obs_drift +=
DSCUR(ppi)->offsetFromMaster.nanoseconds /
OPTS(ppi)->ai;
/* clamp the accumulator to PP_ADJ_FREQ_MAX for sanity */
if (SRV(ppi)->obs_drift > PP_ADJ_FREQ_MAX)
SRV(ppi)->obs_drift = PP_ADJ_FREQ_MAX;
else if (SRV(ppi)->obs_drift < -PP_ADJ_FREQ_MAX)
SRV(ppi)->obs_drift = -PP_ADJ_FREQ_MAX;
adj = DSCUR(ppi)->offsetFromMaster.nanoseconds / OPTS(ppi)->ap +
SRV(ppi)->obs_drift;
/* apply controller output as a clock tick rate adjustment */
if (!OPTS(ppi)->no_adjust)
pp_adj_freq(-adj);
dc++;
if (dc % 2 == 0) { /* Prints statistics every 8s */
PP_PRINTF("ofst %d, raw ofst %d, mean-dly %d, adj %d\n",
DSCUR(ppi)->offsetFromMaster.nanoseconds,
SRV(ppi)->m_to_s_dly.nanoseconds,
DSCUR(ppi)->meanPathDelay.nanoseconds,
adj);
}
}
#ifdef VERB_LOG_MSGS
display:
PP_VPRINTF("\n--Offset Correction-- \n");
PP_VPRINTF("Raw offset from master: %10ds %11dns\n",
SRV(ppi)->m_to_s_dly.seconds,
SRV(ppi)->m_to_s_dly.nanoseconds);
PP_VPRINTF("\n--Offset and Delay filtered-- \n");
if (!OPTS(ppi)->e2e_mode) {
PP_VPRINTF("one-way delay averaged (P2P): %10ds %11dns\n",
DSPOR(ppi)->peerMeanPathDelay.seconds,
DSPOR(ppi)->peerMeanPathDelay.nanoseconds);
} else {
PP_VPRINTF("one-way delay averaged (E2E): %10ds %11dns\n",
DSCUR(ppi)->meanPathDelay.seconds,
DSCUR(ppi)->meanPathDelay.nanoseconds);
}
PP_VPRINTF("offset from master: %10ds %11dns\n",
DSCUR(ppi)->offsetFromMaster.seconds,
DSCUR(ppi)->offsetFromMaster.nanoseconds);
PP_VPRINTF("observed drift: %10d\n", SRV(ppi)->obs_drift);
#endif
}
/*
* Aurelio Colosimo for CERN, 2011 -- GNU LGPL v2.1 or later
* Based on PTPd project v. 2.1.0 (see AUTHORS for details)
*/
#include <ppsi/ppsi.h>
#include <ppsi/diag.h>
#include <pps_gen.h>
#include <softpll_ng.h>
#include <syscon.h>
#include "wr-api.h"
/*
* Initializes network and other stuff
*/
#define LOCK_TIMEOUT_GM (60000)
int pp_initializing(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
unsigned char *id, *mac;
if (NP(ppi)->inited)
pp_net_shutdown(ppi);
if (pp_net_init(ppi) < 0)
goto failure;
NP(ppi)->inited = 1;
DSPOR(ppi)->portState = PPS_INITIALIZING;
/* Initialize default data set */
DSDEF(ppi)->twoStepFlag = PP_TWO_STEP_FLAG;
/* Clock identity comes from mac address with 0xff:0xfe intermixed */
id = DSDEF(ppi)->clockIdentity;
mac = NP(ppi)->ch[PP_NP_GEN].addr;
id[0] = mac[0];
id[1] = mac[1];
id[2] = mac[2];
id[3] = 0xff;
id[4] = 0xfe;
id[5] = mac[3];
id[6] = mac[4];
id[7] = mac[5];
DSDEF(ppi)->numberPorts = 1;
memcpy(&DSDEF(ppi)->clockQuality, &OPTS(ppi)->clock_quality,
sizeof(ClockQuality));
DSDEF(ppi)->priority1 = OPTS(ppi)->prio1;
DSDEF(ppi)->priority2 = OPTS(ppi)->prio2;
DSDEF(ppi)->domainNumber = OPTS(ppi)->domain_number;
DSDEF(ppi)->slaveOnly = OPTS(ppi)->slave_only;
if (OPTS(ppi)->slave_only)
ppi->defaultDS->clockQuality.clockClass = 255;
/* Initialize port data set */
memcpy(ppi->portDS->portIdentity.clockIdentity,
ppi->defaultDS->clockIdentity, PP_CLOCK_IDENTITY_LENGTH);
DSPOR(ppi)->portIdentity.portNumber = 1;
DSPOR(ppi)->logMinDelayReqInterval = PP_DEFAULT_DELAYREQ_INTERVAL;
DSPOR(ppi)->peerMeanPathDelay.seconds = 0;
DSPOR(ppi)->peerMeanPathDelay.nanoseconds = 0;
DSPOR(ppi)->logAnnounceInterval = OPTS(ppi)->announce_intvl;
DSPOR(ppi)->announceReceiptTimeout =
PP_DEFAULT_ANNOUNCE_RECEIPT_TIMEOUT;
DSPOR(ppi)->logSyncInterval = OPTS(ppi)->sync_intvl;
DSPOR(ppi)->delayMechanism = PP_DEFAULT_DELAY_MECHANISM;
DSPOR(ppi)->logMinPdelayReqInterval = PP_DEFAULT_PDELAYREQ_INTERVAL;
DSPOR(ppi)->versionNumber = PP_VERSION_PTP;
DSPOR(ppi)->wrStateTimeout = WR_DEFAULT_STATE_TIMEOUT_MS;
DSPOR(ppi)->wrStateRetry = WR_DEFAULT_STATE_REPEAT;
DSPOR(ppi)->calPeriod = WR_DEFAULT_CAL_PERIOD;
DSPOR(ppi)->wrModeOn = 0;
DSPOR(ppi)->parentWrConfig = 0;
DSPOR(ppi)->calibrated = !WR_DEFAULT_PHY_CALIBRATION_REQUIRED;
if (pp_timer_init(ppi))
goto failure;
pp_init_clock(ppi);
m1(ppi);
msg_pack_header(ppi, ppi->buf_out);
if (!OPTS(ppi)->master_only)
ppi->next_state = PPS_LISTENING;
else
ppi->next_state = PPS_MASTER;
ppi->next_delay = PP_DEFAULT_NEXT_DELAY_MS;
return 0;
failure:
PP_PRINTF("Failed to initialize network\n");
ppi->next_state = PPS_FAULTY;
ppi->next_delay = PP_DEFAULT_NEXT_DELAY_MS;
return 0;
}
/*
* Aurelio Colosimo for CERN, 2011 -- GNU LGPL v2.1 or later
* Based on PTPd project v. 2.1.0 (see AUTHORS for details)
*/
#include <ppsi/ppsi.h>
#include <ppsi/diag.h>
#include "common-fun.h"
#include "wr-api.h"
int pp_listening(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
int e = 0; /* error var, to check errors in msg handling */
if (ppi->is_new_state) {
DSPOR(ppi)->portState = PPS_LISTENING;
DSPOR(ppi)->wrMode = NON_WR;
DSPOR(ppi)->wrPortState = WRS_IDLE;
st_com_restart_annrec_timer(ppi);
}
if (st_com_check_record_update(ppi))
goto state_updated;
if (plen == 0)
goto no_incoming_msg;
switch (ppi->msg_tmp_header.messageType) {
case PPM_ANNOUNCE:
e = st_com_master_handle_announce(ppi, pkt, plen);
break;
case PPM_SYNC:
e = st_com_master_handle_sync(ppi, pkt, plen);
break;
default:
/* disreguard, nothing to do */
break;
}
no_incoming_msg:
if (e == 0)
e = st_com_execute_slave(ppi, 0);
if (e != 0)
ppi->next_state = PPS_FAULTY;
state_updated:
/* Leaving this state */
if (ppi->next_state != ppi->state)
pp_timer_stop(ppi->timers[PP_TIMER_ANN_RECEIPT]);
ppi->next_delay = PP_DEFAULT_NEXT_DELAY_MS;
return 0;
}
/*
* Aurelio Colosimo for CERN, 2011 -- GNU LGPL v2.1 or later
* Based on PTPd project v. 2.1.0 (see AUTHORS for details)
*/
#include <ppsi/ppsi.h>
#include <ppsi/diag.h>
#include "common-fun.h"
#include "wr-constants.h"
#include "wr-api.h"
static Integer32 phase_to_cf_units(Integer32 phase)
{
return (Integer32) ((int64_t)phase * 65536LL / 1000LL);
}
int pp_master(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
TimeInternal *time;
TimeInternal *time_snt;
TimeInternal req_rec_tstamp;
TimeInternal correction_field;
TimeInternal resp_orig_tstamp;
int e = 0; /* error var, to check errors in msg handling */
MsgHeader *hdr = &ppi->msg_tmp_header;
time = &ppi->last_rcv_time;
if (ppi->is_new_state) {
DSPOR(ppi)->portState = PPS_MASTER;
pp_timer_start((1 << DSPOR(ppi)->logSyncInterval) * 1000,
ppi->timers[PP_TIMER_SYNC]);
pp_timer_start((1 << DSPOR(ppi)->logAnnounceInterval) * 1000,
ppi->timers[PP_TIMER_ANN_INTERVAL]);
pp_timer_start(
(1 << DSPOR(ppi)->logMinPdelayReqInterval) * 1000,
ppi->timers[PP_TIMER_PDELAYREQ]);
/* Send an announce immediately, when becomes master */
if (msg_issue_announce(ppi) < 0)
goto failure;
}
if (st_com_check_record_update(ppi))
goto state_updated;
if (pp_timer_expired(ppi->timers[PP_TIMER_SYNC])) {
PP_VPRINTF("event SYNC_INTERVAL_TIMEOUT_EXPIRES\n");
if (msg_issue_sync(ppi) < 0)
goto failure;
time_snt = &ppi->last_snt_time;
add_TimeInternal(time_snt, time_snt, &OPTS(ppi)->outbound_latency);
if (msg_issue_followup(ppi, time_snt))
goto failure;
}
if (pp_timer_expired(ppi->timers[PP_TIMER_ANN_INTERVAL])) {
PP_VPRINTF("event ANNOUNCE_INTERVAL_TIMEOUT_EXPIRES\n");
if (msg_issue_announce(ppi) < 0)
goto failure;
}
if (!OPTS(ppi)->e2e_mode) {
if (pp_timer_expired(ppi->timers[PP_TIMER_PDELAYREQ])) {
PP_VPRINTF("event PDELAYREQ_INTERVAL_TOUT_EXPIRES\n");
if (msg_issue_pdelay_req(ppi) < 0)
goto failure;
}
}
if (plen == 0)
goto no_incoming_msg;
switch (ppi->msg_tmp_header.messageType) {
case PPM_ANNOUNCE:
e = st_com_master_handle_announce(ppi, pkt, plen);
break;
case PPM_SYNC:
e = st_com_master_handle_sync(ppi, pkt, plen);
break;
case PPM_DELAY_REQ:
msg_copy_header(&ppi->delay_req_hdr, hdr);
ppi->delay_req_hdr.correctionfield.msb = 0;
ppi->delay_req_hdr.correctionfield.lsb =
phase_to_cf_units(ppi->last_rcv_time.phase);
msg_issue_delay_resp(ppi, time);
break;
case PPM_PDELAY_REQ:
e = st_com_handle_pdelay_req(ppi, pkt, plen);
break;
case PPM_PDELAY_RESP:
/* Loopback Timestamp */
if (OPTS(ppi)->e2e_mode)
break;
if (ppi->is_from_self) {
/*Add latency*/
add_TimeInternal(time, time,
&OPTS(ppi)->outbound_latency);
e = msg_issue_pdelay_resp_follow_up(ppi, time);
break;
}
msg_unpack_pdelay_resp(pkt, &ppi->msg_tmp.presp);
if (!((ppi->sent_seq_id[PPM_PDELAY_REQ] ==
hdr->sequenceId)
&& (!memcmp(DSPOR(ppi)->portIdentity.clockIdentity,
ppi->msg_tmp.presp.requestingPortIdentity.
clockIdentity,
PP_CLOCK_IDENTITY_LENGTH))
&& (DSPOR(ppi)->portIdentity.portNumber ==
ppi->msg_tmp.presp.requestingPortIdentity.portNumber))
) {
if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0) {
/* Two Step Clock */
/* Store t4 (Fig 35) */
ppi->pdelay_resp_receive_time.seconds =
time->seconds;
ppi->pdelay_resp_receive_time.nanoseconds =
time->nanoseconds;
/* Store t2 (Fig 35) */
to_TimeInternal(&req_rec_tstamp,
&ppi->msg_tmp.presp.
requestReceiptTimestamp);
ppi->pdelay_req_receive_time.seconds =
req_rec_tstamp.seconds;
ppi->pdelay_req_receive_time.nanoseconds =
req_rec_tstamp.nanoseconds;
int64_to_TimeInternal(
hdr->correctionfield,
&correction_field);
ppi->last_pdelay_resp_corr_field.seconds =
correction_field.seconds;
ppi->last_pdelay_resp_corr_field.nanoseconds =
correction_field.nanoseconds;
break;
} else {
/* One step Clock */
/* Store t4 (Fig 35)*/
ppi->pdelay_resp_receive_time.seconds =
time->seconds;
ppi->pdelay_resp_receive_time.nanoseconds =
time->nanoseconds;
int64_to_TimeInternal(
hdr->correctionfield,
&correction_field);
pp_update_peer_delay(ppi, &correction_field, 0);
break;
}
}
break; /* XXX added by gnn for safety */
case PPM_PDELAY_RESP_FOLLOW_UP:
if (hdr->sequenceId == ppi->sent_seq_id[PPM_PDELAY_REQ] - 1) {
msg_unpack_pdelay_resp_followup(pkt,
&ppi->msg_tmp.prespfollow);
to_TimeInternal(&resp_orig_tstamp,
&ppi->msg_tmp.prespfollow.
responseOriginTimestamp);
ppi->pdelay_resp_send_time.seconds =
resp_orig_tstamp.seconds;
ppi->pdelay_resp_send_time.nanoseconds =
resp_orig_tstamp.nanoseconds;
int64_to_TimeInternal(
ppi->msg_tmp_header.correctionfield,
&correction_field);
add_TimeInternal(&correction_field,
&correction_field,
&ppi->last_pdelay_resp_corr_field);
pp_update_peer_delay(ppi, &correction_field, 1);
break;
}
break;
case PPM_SIGNALING:
{
MsgSignaling wrsig_msg;
msg_unpack_wrsig(ppi, pkt, &wrsig_msg,
&(DSPOR(ppi)->msgTmpWrMessageID));
if ((DSPOR(ppi)->msgTmpWrMessageID == SLAVE_PRESENT) &&
(DSPOR(ppi)->wrConfig & WR_M_ONLY))
ppi->next_state = WRS_M_LOCK;
break;
}
default:
/* disreguard, nothing to do */
break;
}
no_incoming_msg:
failure:
if (e == 0) {
if (DSDEF(ppi)->slaveOnly ||
DSDEF(ppi)->clockQuality.clockClass == 255)
ppi->next_state = PPS_LISTENING;
} else {
ppi->next_state = PPS_FAULTY;
}
state_updated:
/* Leaving this state */
if (ppi->next_state != ppi->state) {
pp_timer_stop(ppi->timers[PP_TIMER_SYNC]);
pp_timer_stop(ppi->timers[PP_TIMER_ANN_INTERVAL]);
pp_timer_stop(ppi->timers[PP_TIMER_PDELAYREQ]);
}
ppi->next_delay = PP_DEFAULT_NEXT_DELAY_MS;
return 0;
}
/*
* Aurelio Colosimo for CERN, 2011 -- GNU LGPL v2.1 or later
* Based on PTPd project v. 2.1.0 (see AUTHORS for details)
*/
#include <ppsi/ppsi.h>
#include <ppsi/diag.h>
#include "wr-api.h"
#include "common-fun.h"
int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
int e = 0; /* error var, to check errors in msg handling */
TimeInternal *time;
TimeInternal req_rec_tstamp;
TimeInternal correction_field;
TimeInternal resp_orig_tstamp;
MsgHeader *hdr = &ppi->msg_tmp_header;
time = &ppi->last_rcv_time;
if (ppi->is_new_state) {
DSPOR(ppi)->portState = PPS_SLAVE;
pp_init_clock(ppi);
wr_servo_init(ppi);
ppi->waiting_for_follow = FALSE;
ppi->pdelay_req_send_time.seconds = 0;
ppi->pdelay_req_send_time.nanoseconds = 0;
ppi->pdelay_req_receive_time.seconds = 0;
ppi->pdelay_req_receive_time.nanoseconds = 0;
ppi->pdelay_resp_send_time.seconds = 0;
ppi->pdelay_resp_send_time.nanoseconds = 0;
ppi->pdelay_resp_receive_time.seconds = 0;
ppi->pdelay_resp_receive_time.nanoseconds = 0;
st_com_restart_annrec_timer(ppi);
if (OPTS(ppi)->e2e_mode)
pp_timer_start(
(1 << DSPOR(ppi)->logMinDelayReqInterval) * 1000,
ppi->timers[PP_TIMER_DELAYREQ]);
else
pp_timer_start(
(1 << DSPOR(ppi)->logMinPdelayReqInterval) * 1000,
ppi->timers[PP_TIMER_PDELAYREQ]);
}
if (st_com_check_record_update(ppi))
goto state_updated;
if (plen == 0)
goto no_incoming_msg;
switch (ppi->msg_tmp_header.messageType) {
case PPM_ANNOUNCE:
e = st_com_slave_handle_announce(ppi, pkt, plen);
break;
case PPM_SYNC:
e = st_com_slave_handle_sync(ppi, pkt, plen);
break;
case PPM_FOLLOW_UP:
e = st_com_slave_handle_followup(ppi, pkt, plen);
break;
case PPM_DELAY_REQ:
e = (plen < PP_DELAY_REQ_LENGTH);
if (!e && ppi->is_from_self) {
/* No more used: delay_req_send_time was taken
* when sending it */
}
break;
case PPM_DELAY_RESP:
e = (plen < PP_DELAY_RESP_LENGTH);
if (e)
break;
if (!OPTS(ppi)->e2e_mode)
break;
msg_unpack_delay_resp(pkt, &ppi->msg_tmp.resp);
if ((memcmp(DSPOR(ppi)->portIdentity.clockIdentity,
ppi->msg_tmp.resp.requestingPortIdentity.clockIdentity,
PP_CLOCK_IDENTITY_LENGTH) == 0) &&
((ppi->sent_seq_id[PPM_DELAY_REQ] - 1) ==
hdr->sequenceId) &&
(DSPOR(ppi)->portIdentity.portNumber ==
ppi->msg_tmp.resp.requestingPortIdentity.portNumber)
&& ppi->is_from_cur_par) {
to_TimeInternal(&req_rec_tstamp,
&ppi->msg_tmp.resp.receiveTimestamp);
ppi->delay_req_receive_time.seconds =
req_rec_tstamp.seconds;
ppi->delay_req_receive_time.nanoseconds =
req_rec_tstamp.nanoseconds;
ppi->delay_req_receive_time.phase =
req_rec_tstamp.phase;
ppi->delay_req_receive_time.correct =
req_rec_tstamp.correct;
int64_to_TimeInternal(
hdr->correctionfield,
&correction_field);
if (!DSPOR(ppi)->wrModeOn)
pp_update_delay(ppi, &correction_field);
else {
wr_servo_got_delay(ppi,
hdr->correctionfield.lsb);
wr_servo_update(ppi);
}
ppi->log_min_delay_req_interval =
hdr->logMessageInterval;
} else {
PP_PRINTF("pp_slave : "
"Delay Resp doesn't match Delay Req\n");
}
break;
case PPM_PDELAY_REQ:
e = st_com_handle_pdelay_req(ppi, pkt, plen);
break;
case PPM_PDELAY_RESP:
if (OPTS(ppi)->e2e_mode)
break;
e = (plen < PP_PDELAY_RESP_LENGTH);
if (e)
break;
if (ppi->is_from_self) {
add_TimeInternal(time, time,
&OPTS(ppi)->outbound_latency);
msg_issue_pdelay_resp_follow_up(ppi, time);
break;
}
msg_unpack_pdelay_resp(pkt,
&ppi->msg_tmp.presp);
if (!((ppi->sent_seq_id[PPM_PDELAY_REQ] ==
hdr->sequenceId)
&& (!memcmp(DSPOR(ppi)->portIdentity.clockIdentity,
ppi->msg_tmp.presp.requestingPortIdentity.
clockIdentity, PP_CLOCK_IDENTITY_LENGTH))
&& (DSPOR(ppi)->portIdentity.portNumber ==
ppi->msg_tmp.presp.
requestingPortIdentity.portNumber))) {
if ((hdr->flagField[0] & 0x02) ==
PP_TWO_STEP_FLAG) {
/* Two Step Clock */
/* Store t4 (Fig 35) */
ppi->pdelay_resp_receive_time.seconds =
time->seconds;
ppi->pdelay_resp_receive_time.nanoseconds =
time->nanoseconds;
/* Store t2 (Fig 35) */
to_TimeInternal(&req_rec_tstamp,
&ppi->msg_tmp.presp.
requestReceiptTimestamp);
ppi->pdelay_req_receive_time.seconds =
req_rec_tstamp.seconds;
ppi->pdelay_req_receive_time.nanoseconds =
req_rec_tstamp.nanoseconds;
int64_to_TimeInternal(hdr->correctionfield,
&correction_field);
ppi->last_pdelay_resp_corr_field.seconds =
correction_field.seconds;
ppi->last_pdelay_resp_corr_field.nanoseconds =
correction_field.nanoseconds;
} else {
/* One step Clock */
/* Store t4 (Fig 35) */
ppi->pdelay_resp_receive_time.seconds =
time->seconds;
ppi->pdelay_resp_receive_time.nanoseconds =
time->nanoseconds;
int64_to_TimeInternal(hdr->correctionfield,
&correction_field);
pp_update_peer_delay(ppi,& correction_field, 0);
}
} else {
PP_VPRINTF("pp_slave : PDelay Resp doesn't "
"match PDelay Req.\n");
}
break;
case PPM_PDELAY_RESP_FOLLOW_UP:
if (OPTS(ppi)->e2e_mode)
break;
e = (plen < PP_PDELAY_RESP_FOLLOW_UP_LENGTH);
if (e)
break;
if (hdr->sequenceId ==
ppi->sent_seq_id[PPM_PDELAY_REQ]) {
msg_unpack_pdelay_resp_followup(
pkt,
&ppi->msg_tmp.prespfollow);
to_TimeInternal(
&resp_orig_tstamp,
&ppi->msg_tmp.prespfollow.
responseOriginTimestamp);
ppi->pdelay_resp_send_time.seconds =
resp_orig_tstamp.seconds;
ppi->pdelay_resp_send_time.nanoseconds =
resp_orig_tstamp.nanoseconds;
int64_to_TimeInternal(
hdr->correctionfield,
&correction_field);
add_TimeInternal(&correction_field, &correction_field,
&ppi->last_pdelay_req_corr_field);
pp_update_peer_delay(ppi, &correction_field, 1);
}
break;
default:
/* disreguard, nothing to do */
break;
}
no_incoming_msg:
if (e == 0)
e = st_com_execute_slave(ppi, 1);
if (e != 0)
ppi->next_state = PPS_FAULTY;
state_updated:
/* Leaving this state */
if (ppi->next_state != ppi->state) {
pp_timer_stop(ppi->timers[PP_TIMER_ANN_RECEIPT]);
if (OPTS(ppi)->e2e_mode)
pp_timer_stop(ppi->timers[PP_TIMER_DELAYREQ]);
else
pp_timer_stop(ppi->timers[PP_TIMER_PDELAYREQ]);
pp_init_clock(ppi);
}
ppi->next_delay = PP_DEFAULT_NEXT_DELAY_MS;
return 0;
}
......@@ -11,18 +11,18 @@ int wr_calibrated(struct pp_instance *ppi, unsigned char *pkt, int plen)
MsgSignaling wrsig_msg;
if (ppi->is_new_state) {
DSPOR(ppi)->wrPortState = WRS_CALIBRATED;
WR_DSPOR(ppi)->wrPortState = WRS_CALIBRATED;
ppi->next_delay = PP_DEFAULT_NEXT_DELAY_MS;
pp_timer_start(DSPOR(ppi)->wrStateTimeout,
pp_timer_start(WR_DSPOR(ppi)->wrStateTimeout,
ppi->timers[PP_TIMER_WRS_CALIBRATED]);
}
if (pp_timer_expired(ppi->timers[PP_TIMER_WRS_CALIBRATED])) {
if (DSPOR(ppi)->wrMode == WR_MASTER)
if (WR_DSPOR(ppi)->wrMode == WR_MASTER)
ppi->next_state = PPS_MASTER;
else
ppi->next_state = PPS_LISTENING;
DSPOR(ppi)->wrPortState = WRS_IDLE;
WR_DSPOR(ppi)->wrPortState = WRS_IDLE;
goto state_updated;
}
......@@ -31,13 +31,13 @@ int wr_calibrated(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (ppi->msg_tmp_header.messageType == PPM_SIGNALING) {
msg_unpack_wrsig(ppi, pkt, &wrsig_msg,
&(DSPOR(ppi)->msgTmpWrMessageID));
&(WR_DSPOR(ppi)->msgTmpWrMessageID));
if ((DSPOR(ppi)->msgTmpWrMessageID == CALIBRATE) &&
(DSPOR(ppi)->wrMode == WR_MASTER))
if ((WR_DSPOR(ppi)->msgTmpWrMessageID == CALIBRATE) &&
(WR_DSPOR(ppi)->wrMode == WR_MASTER))
ppi->next_state = WRS_RESP_CALIB_REQ;
else if ((DSPOR(ppi)->msgTmpWrMessageID == WR_MODE_ON) &&
(DSPOR(ppi)->wrMode == WR_SLAVE))
else if ((WR_DSPOR(ppi)->msgTmpWrMessageID == WR_MODE_ON) &&
(WR_DSPOR(ppi)->wrMode == WR_SLAVE))
ppi->next_state = WRS_WR_LINK_ON;
}
......@@ -46,6 +46,6 @@ state_updated:
pp_timer_stop(ppi->timers[PP_TIMER_WRS_CALIBRATED]);
ret:
ppi->next_delay = DSPOR(ppi)->wrStateTimeout;
ppi->next_delay = WR_DSPOR(ppi)->wrStateTimeout;
return 0;
}
......@@ -13,31 +13,31 @@ int wr_calibration(struct pp_instance *ppi, unsigned char *pkt, int plen)
uint32_t delta;
if (ppi->is_new_state) {
DSPOR(ppi)->wrPortState = WRS_CALIBRATION;
WR_DSPOR(ppi)->wrPortState = WRS_CALIBRATION;
e = msg_issue_wrsig(ppi, CALIBRATE);
pp_timer_start(DSPOR(ppi)->calPeriod,
pp_timer_start(WR_DSPOR(ppi)->calPeriod,
ppi->timers[PP_TIMER_WRS_CALIBRATION]);
if (DSPOR(ppi)->calibrated) {
DSPOR(ppi)->wrPortState = WRS_CALIBRATION_2;
if (WR_DSPOR(ppi)->calibrated) {
WR_DSPOR(ppi)->wrPortState = WRS_CALIBRATION_2;
}
}
if (pp_timer_expired(ppi->timers[PP_TIMER_WRS_CALIBRATION])) {
if (DSPOR(ppi)->wrMode == WR_MASTER)
if (WR_DSPOR(ppi)->wrMode == WR_MASTER)
ppi->next_state = PPS_MASTER;
else
ppi->next_state = PPS_LISTENING;
DSPOR(ppi)->wrPortState = WRS_IDLE;
WR_DSPOR(ppi)->wrPortState = WRS_IDLE;
goto state_updated;
}
switch (DSPOR(ppi)->wrPortState) {
switch (WR_DSPOR(ppi)->wrPortState) {
case WRS_CALIBRATION:
/* enable pattern sending */
if (wr_calibration_pattern_enable(ppi, 0, 0, 0) ==
WR_HW_CALIB_OK)
DSPOR(ppi)->wrPortState = WRS_CALIBRATION_1;
WR_DSPOR(ppi)->wrPortState = WRS_CALIBRATION_1;
else
break;
......@@ -45,7 +45,7 @@ int wr_calibration(struct pp_instance *ppi, unsigned char *pkt, int plen)
/* enable Tx calibration */
if (wr_calibrating_enable(ppi, WR_HW_CALIB_TX)
== WR_HW_CALIB_OK)
DSPOR(ppi)->wrPortState = WRS_CALIBRATION_2;
WR_DSPOR(ppi)->wrPortState = WRS_CALIBRATION_2;
else
break;
......@@ -53,16 +53,16 @@ int wr_calibration(struct pp_instance *ppi, unsigned char *pkt, int plen)
/* wait until Tx calibration is finished */
if (wr_calibrating_poll(ppi, WR_HW_CALIB_TX, &delta) ==
WR_HW_CALIB_READY) {
DSPOR(ppi)->deltaTx.scaledPicoseconds.msb =
WR_DSPOR(ppi)->deltaTx.scaledPicoseconds.msb =
0xFFFFFFFF & (((uint64_t)delta) >> 16);
DSPOR(ppi)->deltaTx.scaledPicoseconds.lsb =
WR_DSPOR(ppi)->deltaTx.scaledPicoseconds.lsb =
0xFFFFFFFF & (((uint64_t)delta) << 16);
PP_PRINTF("Tx=>>scaledPicoseconds.msb = 0x%x\n",
DSPOR(ppi)->deltaTx.scaledPicoseconds.msb);
PP_PRINTF("Tx=>>scaledPicoseconds.lsb = 0x%x\n",
DSPOR(ppi)->deltaTx.scaledPicoseconds.lsb);
DSPOR(ppi)->wrPortState = WRS_CALIBRATION_3;
WR_DSPOR(ppi)->wrPortState = WRS_CALIBRATION_3;
}
else
break; /* again */
......@@ -71,14 +71,14 @@ int wr_calibration(struct pp_instance *ppi, unsigned char *pkt, int plen)
/* disable Tx calibration */
if (wr_calibrating_disable(ppi, WR_HW_CALIB_TX)
== WR_HW_CALIB_OK)
DSPOR(ppi)->wrPortState = WRS_CALIBRATION_4;
WR_DSPOR(ppi)->wrPortState = WRS_CALIBRATION_4;
else
break;
case WRS_CALIBRATION_4:
/* disable pattern sending */
if (wr_calibration_pattern_disable(ppi) == WR_HW_CALIB_OK)
DSPOR(ppi)->wrPortState = WRS_CALIBRATION_5;
WR_DSPOR(ppi)->wrPortState = WRS_CALIBRATION_5;
else
break;
......@@ -86,7 +86,7 @@ int wr_calibration(struct pp_instance *ppi, unsigned char *pkt, int plen)
/* enable Rx calibration using the pattern sent by other port */
if (wr_calibrating_enable(ppi, WR_HW_CALIB_RX) ==
WR_HW_CALIB_OK)
DSPOR(ppi)->wrPortState = WRS_CALIBRATION_6;
WR_DSPOR(ppi)->wrPortState = WRS_CALIBRATION_6;
else
break;
......@@ -95,16 +95,16 @@ int wr_calibration(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (wr_calibrating_poll(ppi, WR_HW_CALIB_RX, &delta) ==
WR_HW_CALIB_READY) {
PP_PRINTF("Rx fixed delay = %d\n", (int)delta);
DSPOR(ppi)->deltaRx.scaledPicoseconds.msb =
WR_DSPOR(ppi)->deltaRx.scaledPicoseconds.msb =
0xFFFFFFFF & (delta >> 16);
DSPOR(ppi)->deltaRx.scaledPicoseconds.lsb =
WR_DSPOR(ppi)->deltaRx.scaledPicoseconds.lsb =
0xFFFFFFFF & (delta << 16);
PP_PRINTF("Rx=>>scaledPicoseconds.msb = 0x%x\n",
DSPOR(ppi)->deltaRx.scaledPicoseconds.msb);
PP_PRINTF("Rx=>>scaledPicoseconds.lsb = 0x%x\n",
DSPOR(ppi)->deltaRx.scaledPicoseconds.lsb);
DSPOR(ppi)->wrPortState = WRS_CALIBRATION_7;
WR_DSPOR(ppi)->wrPortState = WRS_CALIBRATION_7;
}
else
break; /* again */
......@@ -113,14 +113,14 @@ int wr_calibration(struct pp_instance *ppi, unsigned char *pkt, int plen)
/* disable Rx calibration */
if (wr_calibrating_disable(ppi, WR_HW_CALIB_RX)
== WR_HW_CALIB_OK)
DSPOR(ppi)->wrPortState = WRS_CALIBRATION_8;
WR_DSPOR(ppi)->wrPortState = WRS_CALIBRATION_8;
else
break;
case WRS_CALIBRATION_8:
/* send deltas to the other port and go to the next state */
e = msg_issue_wrsig(ppi, CALIBRATED);
ppi->next_state = WRS_CALIBRATED;
DSPOR(ppi)->calibrated = TRUE;
WR_DSPOR(ppi)->calibrated = TRUE;
default:
break;
......@@ -130,7 +130,7 @@ state_updated:
if (ppi->next_state != ppi->state)
pp_timer_stop(ppi->timers[PP_TIMER_WRS_CALIBRATION]);
ppi->next_delay = DSPOR(ppi)->wrStateTimeout;
ppi->next_delay = WR_DSPOR(ppi)->wrStateTimeout;
return e;
}
......@@ -11,22 +11,22 @@ int wr_link_on(struct pp_instance *ppi, unsigned char *pkt, int plen)
int e = 0;
if (ppi->is_new_state) {
DSPOR(ppi)->wrModeOn = TRUE;
WR_DSPOR(ppi)->wrModeOn = TRUE;
wr_enable_ptracker(ppi);
if (DSPOR(ppi)->wrMode == WR_MASTER)
if (WR_DSPOR(ppi)->wrMode == WR_MASTER)
e = msg_issue_wrsig(ppi, WR_MODE_ON);
DSPOR(ppi)->parentWrModeOn = TRUE;
DSPOR(ppi)->wrPortState = WRS_WR_LINK_ON;
WR_DSPOR(ppi)->parentWrModeOn = TRUE;
WR_DSPOR(ppi)->wrPortState = WRS_WR_LINK_ON;
}
if (e != 0)
return -1;
DSPOR(ppi)->wrPortState = WRS_IDLE;
WR_DSPOR(ppi)->wrPortState = WRS_IDLE;
if (DSPOR(ppi)->wrMode == WR_SLAVE)
if (WR_DSPOR(ppi)->wrMode == WR_SLAVE)
ppi->next_state = PPS_SLAVE;
else
ppi->next_state = PPS_MASTER;
......
......@@ -13,8 +13,8 @@ int wr_locked(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (ppi->is_new_state) {
DSPOR(ppi)->portState = PPS_UNCALIBRATED;
DSPOR(ppi)->wrPortState = WRS_LOCKED;
pp_timer_start(DSPOR(ppi)->wrStateTimeout,
WR_DSPOR(ppi)->wrPortState = WRS_LOCKED;
pp_timer_start(WR_DSPOR(ppi)->wrStateTimeout,
ppi->timers[PP_TIMER_WRS_LOCKED]);
e = msg_issue_wrsig(ppi, LOCKED);
......@@ -22,8 +22,8 @@ int wr_locked(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (pp_timer_expired(ppi->timers[PP_TIMER_WRS_LOCKED])) {
ppi->next_state = PPS_LISTENING;
DSPOR(ppi)->wrMode = NON_WR;
DSPOR(ppi)->wrPortState = WRS_IDLE;
WR_DSPOR(ppi)->wrMode = NON_WR;
WR_DSPOR(ppi)->wrPortState = WRS_IDLE;
goto state_updated;
}
......@@ -33,9 +33,9 @@ int wr_locked(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (ppi->msg_tmp_header.messageType == PPM_SIGNALING) {
msg_unpack_wrsig(ppi, pkt, &wrsig_msg,
&(DSPOR(ppi)->msgTmpWrMessageID));
&(WR_DSPOR(ppi)->msgTmpWrMessageID));
if (DSPOR(ppi)->msgTmpWrMessageID == CALIBRATE)
if (WR_DSPOR(ppi)->msgTmpWrMessageID == CALIBRATE)
ppi->next_state = WRS_RESP_CALIB_REQ;
}
......@@ -47,7 +47,7 @@ state_updated:
if (ppi->next_state != ppi->state)
pp_timer_stop(ppi->timers[PP_TIMER_WRS_LOCKED]);
ppi->next_delay = DSPOR(ppi)->wrStateTimeout;
ppi->next_delay = WR_DSPOR(ppi)->wrStateTimeout;
return e;
}
......@@ -13,8 +13,8 @@ int wr_m_lock(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (ppi->is_new_state) {
DSPOR(ppi)->portState = PPS_MASTER;
DSPOR(ppi)->wrPortState = WRS_M_LOCK;
DSPOR(ppi)->wrMode = WR_MASTER;
WR_DSPOR(ppi)->wrPortState = WRS_M_LOCK;
WR_DSPOR(ppi)->wrMode = WR_MASTER;
e = msg_issue_wrsig(ppi, LOCK);
pp_timer_start(WR_M_LOCK_TIMEOUT_MS,
ppi->timers[PP_TIMER_WRS_M_LOCK]);
......@@ -22,7 +22,7 @@ int wr_m_lock(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (pp_timer_expired(ppi->timers[PP_TIMER_WRS_M_LOCK])) {
ppi->next_state = PPS_MASTER;
DSPOR(ppi)->wrPortState = WRS_IDLE;
WR_DSPOR(ppi)->wrPortState = WRS_IDLE;
goto state_updated;
}
......@@ -32,9 +32,9 @@ int wr_m_lock(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (ppi->msg_tmp_header.messageType == PPM_SIGNALING) {
msg_unpack_wrsig(ppi, pkt, &wrsig_msg,
&(DSPOR(ppi)->msgTmpWrMessageID));
&(WR_DSPOR(ppi)->msgTmpWrMessageID));
if (DSPOR(ppi)->msgTmpWrMessageID == LOCKED)
if (WR_DSPOR(ppi)->msgTmpWrMessageID == LOCKED)
ppi->next_state = WRS_CALIBRATION;
}
......@@ -46,7 +46,7 @@ state_updated:
if (ppi->next_state != ppi->state)
pp_timer_stop(ppi->timers[PP_TIMER_WRS_M_LOCK]);
ppi->next_delay = DSPOR(ppi)->wrStateTimeout;
ppi->next_delay = WR_DSPOR(ppi)->wrStateTimeout;
return e;
}
......@@ -5,7 +5,7 @@
#include <ppsi/ppsi.h>
#include "wr-api.h"
#include "common-fun.h"
#include "../proto-standard/common-fun.h"
int wr_present(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
......@@ -15,8 +15,8 @@ int wr_present(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (ppi->is_new_state) {
DSPOR(ppi)->portState = PPS_UNCALIBRATED;
DSPOR(ppi)->wrPortState = WRS_PRESENT;
DSPOR(ppi)->wrMode = WR_SLAVE;
WR_DSPOR(ppi)->wrPortState = WRS_PRESENT;
WR_DSPOR(ppi)->wrMode = WR_SLAVE;
pp_timer_start(WR_WRS_PRESENT_TIMEOUT_MS,
ppi->timers[PP_TIMER_WRS_PRESENT]);
st_com_restart_annrec_timer(ppi);
......@@ -25,8 +25,8 @@ int wr_present(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (pp_timer_expired(ppi->timers[PP_TIMER_WRS_PRESENT])) {
ppi->next_state = PPS_LISTENING;
DSPOR(ppi)->wrMode = NON_WR;
DSPOR(ppi)->wrPortState = WRS_IDLE;
WR_DSPOR(ppi)->wrMode = NON_WR;
WR_DSPOR(ppi)->wrPortState = WRS_IDLE;
goto state_updated;
}
......@@ -36,9 +36,9 @@ int wr_present(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (ppi->msg_tmp_header.messageType == PPM_SIGNALING) {
msg_unpack_wrsig(ppi, pkt, &wrsig_msg,
&(DSPOR(ppi)->msgTmpWrMessageID));
&(WR_DSPOR(ppi)->msgTmpWrMessageID));
if (DSPOR(ppi)->msgTmpWrMessageID == LOCK)
if (WR_DSPOR(ppi)->msgTmpWrMessageID == LOCK)
ppi->next_state = WRS_S_LOCK;
}
......@@ -54,7 +54,7 @@ state_updated:
pp_timer_stop(ppi->timers[PP_TIMER_ANN_RECEIPT]);
}
ppi->next_delay = DSPOR(ppi)->wrStateTimeout;
ppi->next_delay = WR_DSPOR(ppi)->wrStateTimeout;
return e;
}
......@@ -12,24 +12,24 @@ int wr_resp_calib_req(struct pp_instance *ppi, unsigned char *pkt, int plen)
MsgSignaling wrsig_msg;
if (ppi->is_new_state) {
DSPOR(ppi)->wrPortState = WRS_RESP_CALIB_REQ;
WR_DSPOR(ppi)->wrPortState = WRS_RESP_CALIB_REQ;
ppi->next_delay = PP_DEFAULT_NEXT_DELAY_MS;
if (DSPOR(ppi)->otherNodeCalSendPattern) {
if (WR_DSPOR(ppi)->otherNodeCalSendPattern) {
wr_calibration_pattern_enable(ppi, 0, 0, 0);
pp_timer_start(
DSPOR(ppi)->otherNodeCalPeriod / 1000,
WR_DSPOR(ppi)->otherNodeCalPeriod / 1000,
ppi->timers[PP_TIMER_WRS_RESP_CALIB_REQ]);
}
}
if ((DSPOR(ppi)->otherNodeCalSendPattern) &&
if ((WR_DSPOR(ppi)->otherNodeCalSendPattern) &&
(pp_timer_expired(ppi->timers[PP_TIMER_WRS_RESP_CALIB_REQ]))) {
if (DSPOR(ppi)->wrMode == WR_MASTER)
if (WR_DSPOR(ppi)->wrMode == WR_MASTER)
ppi->next_state = PPS_MASTER;
else
ppi->next_state = PPS_LISTENING;
DSPOR(ppi)->wrPortState = WRS_IDLE;
WR_DSPOR(ppi)->wrPortState = WRS_IDLE;
goto state_updated;
}
......@@ -39,12 +39,12 @@ int wr_resp_calib_req(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (ppi->msg_tmp_header.messageType == PPM_SIGNALING) {
msg_unpack_wrsig(ppi, pkt, &wrsig_msg,
&(DSPOR(ppi)->msgTmpWrMessageID));
&(WR_DSPOR(ppi)->msgTmpWrMessageID));
if (DSPOR(ppi)->msgTmpWrMessageID == CALIBRATED) {
if (DSPOR(ppi)->otherNodeCalSendPattern)
if (WR_DSPOR(ppi)->msgTmpWrMessageID == CALIBRATED) {
if (WR_DSPOR(ppi)->otherNodeCalSendPattern)
wr_calibration_pattern_disable(ppi);
if (DSPOR(ppi)->wrMode == WR_MASTER)
if (WR_DSPOR(ppi)->wrMode == WR_MASTER)
ppi->next_state = WRS_WR_LINK_ON;
else
ppi->next_state = WRS_CALIBRATION;
......@@ -56,7 +56,7 @@ state_updated:
if (ppi->next_state != ppi->state)
pp_timer_stop(ppi->timers[PP_TIMER_WRS_M_LOCK]);
ppi->next_delay = DSPOR(ppi)->wrStateTimeout;
ppi->next_delay = WR_DSPOR(ppi)->wrStateTimeout;
return e;
}
......@@ -11,7 +11,7 @@ int wr_s_lock(struct pp_instance *ppi, unsigned char *pkt, int plen)
int e = 0;
if (ppi->is_new_state) {
DSPOR(ppi)->portState = PPS_UNCALIBRATED;
DSPOR(ppi)->wrPortState = WRS_S_LOCK;
WR_DSPOR(ppi)->wrPortState = WRS_S_LOCK;
ppi->next_delay = PP_DEFAULT_NEXT_DELAY_MS;
wr_locking_enable(ppi);
pp_timer_start(WR_S_LOCK_TIMEOUT_MS,
......@@ -20,8 +20,8 @@ int wr_s_lock(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (pp_timer_expired(ppi->timers[PP_TIMER_WRS_S_LOCK])) {
ppi->next_state = PPS_FAULTY;
DSPOR(ppi)->wrPortState = WRS_IDLE;
DSPOR(ppi)->wrMode = NON_WR;
WR_DSPOR(ppi)->wrPortState = WRS_IDLE;
WR_DSPOR(ppi)->wrMode = NON_WR;
goto state_updated;
}
......@@ -34,7 +34,7 @@ state_updated:
if (ppi->next_state != ppi->state)
pp_timer_stop(ppi->timers[PP_TIMER_WRS_S_LOCK]);
ppi->next_delay = DSPOR(ppi)->wrStateTimeout;
ppi->next_delay = WR_DSPOR(ppi)->wrStateTimeout;
return e;
}
......@@ -8,6 +8,52 @@
#include "wr-constants.h"
/*
* This structure is used as extension-specific data in the DSPort
* (see wrspec.v2-06-07-2011, page 17)
*/
struct wr_dsport {
Enumeration8 wrConfig;
Enumeration8 wrMode;
Boolean wrModeOn;
Enumeration8 wrPortState;
/* FIXME check doc: knownDeltaTx, knownDeltaRx, deltasKnown?) */
Boolean calibrated;
FixedDelta deltaTx;
FixedDelta deltaRx;
UInteger32 wrStateTimeout;
UInteger8 wrStateRetry;
UInteger32 calPeriod;
UInteger8 calRetry;
Enumeration8 parentWrConfig;
Boolean parentIsWRnode; /* FIXME Not in the doc */
/* FIXME check doc: (parentWrMode?) */
Enumeration16 msgTmpWrMessageID; /* FIXME Not in the doc */
Boolean parentWrModeOn;
Boolean parentCalibrated;
/* FIXME: are they in the doc? */
UInteger16 otherNodeCalSendPattern;
UInteger32 otherNodeCalPeriod;
UInteger8 otherNodeCalRetry;
FixedDelta otherNodeDeltaTx;
FixedDelta otherNodeDeltaRx;
Boolean doRestart;
Boolean linkUP;
};
/* This uppercase name matches "DSPOR(ppi)" used by standard protocol */
static inline struct wr_dsport *WR_DSPOR(struct pp_instance *ppi)
{
return ppi->portDS->ext_dsport;
}
static inline Integer32 phase_to_cf_units(Integer32 phase)
{
return (Integer32) ((int64_t)phase * 65536LL / 1000LL);
}
/* Pack/Unkpack White rabbit message in the suffix of PTP announce message */
void msg_pack_announce_wr_tlv(struct pp_instance *ppi);
void msg_unpack_announce_wr_tlv(void *buf, MsgAnnounce *ann);
......
......@@ -6,7 +6,7 @@
#include <ppsi/ppsi.h>
#include <ppsi/diag.h>
#include "wr-api.h"
#include "common-fun.h"
#include "../proto-standard/common-fun.h"
/*
* WR way to handle little/big endianess
......@@ -69,12 +69,12 @@ void msg_pack_announce_wr_tlv(struct pp_instance *ppi)
| WR_TLV_WR_VERSION_NUMBER)));
//wrMessageId
*(UInteger16*)(buf + 74) = htons(ANN_SUFIX);
wr_flags = wr_flags | DSPOR(ppi)->wrConfig;
wr_flags = wr_flags | WR_DSPOR(ppi)->wrConfig;
if (DSPOR(ppi)->calibrated)
if (WR_DSPOR(ppi)->calibrated)
wr_flags = WR_IS_CALIBRATED | wr_flags;
if (DSPOR(ppi)->wrModeOn)
if (WR_DSPOR(ppi)->wrModeOn)
wr_flags = WR_IS_WR_MODE | wr_flags;
*(UInteger16*)(buf + 76) = htons(wr_flags);
}
......@@ -112,7 +112,7 @@ int msg_pack_wrsig(struct pp_instance *ppi, Enumeration16 wr_msg_id)
void *buf;
UInteger16 len = 0;
if ((DSPOR(ppi)->wrMode == NON_WR) || (wr_msg_id == ANN_SUFIX)) {
if ((WR_DSPOR(ppi)->wrMode == NON_WR) || (wr_msg_id == ANN_SUFIX)) {
PP_PRINTF("BUG: Trying to send invalid wr_msg mode=%x id=%x",
DSPOR(ppi)->wrMode, wr_msg_id);
return 0;
......@@ -144,24 +144,26 @@ int msg_pack_wrsig(struct pp_instance *ppi, Enumeration16 wr_msg_id)
switch (wr_msg_id) {
case CALIBRATE:
if(DSPOR(ppi)->calibrated) {
put_be16(buf+56, (DSPOR(ppi)->calRetry << 8 | 0x0000));
if(WR_DSPOR(ppi)->calibrated) {
put_be16(buf+56,
(WR_DSPOR(ppi)->calRetry << 8 | 0x0000));
}
else {
put_be16(buf+56, (DSPOR(ppi)->calRetry << 8 | 0x0001));
put_be16(buf+56,
(WR_DSPOR(ppi)->calRetry << 8 | 0x0001));
}
put_be32(buf+58, DSPOR(ppi)->calPeriod);
put_be32(buf+58, WR_DSPOR(ppi)->calPeriod);
len = 20;
break;
case CALIBRATED: /* new fsm */
/* delta TX */
put_be32(buf+56, DSPOR(ppi)->deltaTx.scaledPicoseconds.msb);
put_be32(buf+60, DSPOR(ppi)->deltaTx.scaledPicoseconds.lsb);
put_be32(buf+56, WR_DSPOR(ppi)->deltaTx.scaledPicoseconds.msb);
put_be32(buf+60, WR_DSPOR(ppi)->deltaTx.scaledPicoseconds.lsb);
/* delta RX */
put_be32(buf+64, DSPOR(ppi)->deltaRx.scaledPicoseconds.msb);
put_be32(buf+68, DSPOR(ppi)->deltaRx.scaledPicoseconds.lsb);
put_be32(buf+64, WR_DSPOR(ppi)->deltaRx.scaledPicoseconds.msb);
put_be32(buf+68, WR_DSPOR(ppi)->deltaRx.scaledPicoseconds.lsb);
len = 24;
break;
......@@ -235,22 +237,24 @@ void msg_unpack_wrsig(struct pp_instance *ppi, void *buf,
switch (wr_msg_id) {
case CALIBRATE:
DSPOR(ppi)->otherNodeCalSendPattern = 0x00FF & get_be16(buf+56);
DSPOR(ppi)->otherNodeCalRetry = 0x00FF & (get_be16(buf+56) >> 8);
DSPOR(ppi)->otherNodeCalPeriod = get_be32(buf+58);
WR_DSPOR(ppi)->otherNodeCalSendPattern =
0x00FF & get_be16(buf+56);
WR_DSPOR(ppi)->otherNodeCalRetry =
0x00FF & (get_be16(buf+56) >> 8);
WR_DSPOR(ppi)->otherNodeCalPeriod = get_be32(buf+58);
break;
case CALIBRATED:
/* delta TX */
DSPOR(ppi)->otherNodeDeltaTx.scaledPicoseconds.msb =
WR_DSPOR(ppi)->otherNodeDeltaTx.scaledPicoseconds.msb =
get_be32(buf+56);
DSPOR(ppi)->otherNodeDeltaTx.scaledPicoseconds.lsb =
WR_DSPOR(ppi)->otherNodeDeltaTx.scaledPicoseconds.lsb =
get_be32(buf+60);
/* delta RX */
DSPOR(ppi)->otherNodeDeltaRx.scaledPicoseconds.msb =
WR_DSPOR(ppi)->otherNodeDeltaRx.scaledPicoseconds.msb =
get_be32(buf+64);
DSPOR(ppi)->otherNodeDeltaRx.scaledPicoseconds.lsb =
WR_DSPOR(ppi)->otherNodeDeltaRx.scaledPicoseconds.lsb =
get_be32(buf+68);
break;
......
......@@ -190,11 +190,11 @@ int wr_servo_init(struct pp_instance *ppi)
s->cur_setpoint = 0;
s->missed_iters = 0;
s->delta_tx_m = ((((int32_t)DSPOR(ppi)->otherNodeDeltaTx.scaledPicoseconds.lsb) >> 16) & 0xffff) | (((int32_t)DSPOR(ppi)->otherNodeDeltaTx.scaledPicoseconds.msb) << 16);
s->delta_rx_m = ((((int32_t)DSPOR(ppi)->otherNodeDeltaRx.scaledPicoseconds.lsb) >> 16) & 0xffff) | (((int32_t)DSPOR(ppi)->otherNodeDeltaRx.scaledPicoseconds.msb) << 16);
s->delta_tx_m = ((((int32_t)WR_DSPOR(ppi)->otherNodeDeltaTx.scaledPicoseconds.lsb) >> 16) & 0xffff) | (((int32_t)WR_DSPOR(ppi)->otherNodeDeltaTx.scaledPicoseconds.msb) << 16);
s->delta_rx_m = ((((int32_t)WR_DSPOR(ppi)->otherNodeDeltaRx.scaledPicoseconds.lsb) >> 16) & 0xffff) | (((int32_t)WR_DSPOR(ppi)->otherNodeDeltaRx.scaledPicoseconds.msb) << 16);
s->delta_tx_s = ((((int32_t)DSPOR(ppi)->deltaTx.scaledPicoseconds.lsb) >> 16) & 0xffff) | (((int32_t)DSPOR(ppi)->deltaTx.scaledPicoseconds.msb) << 16);
s->delta_rx_s = ((((int32_t)DSPOR(ppi)->deltaRx.scaledPicoseconds.lsb) >> 16) & 0xffff) | (((int32_t)DSPOR(ppi)->deltaRx.scaledPicoseconds.msb) << 16);
s->delta_tx_s = ((((int32_t)WR_DSPOR(ppi)->deltaTx.scaledPicoseconds.lsb) >> 16) & 0xffff) | (((int32_t)WR_DSPOR(ppi)->deltaTx.scaledPicoseconds.msb) << 16);
s->delta_rx_s = ((((int32_t)WR_DSPOR(ppi)->deltaRx.scaledPicoseconds.lsb) >> 16) & 0xffff) | (((int32_t)WR_DSPOR(ppi)->deltaRx.scaledPicoseconds.msb) << 16);
cur_servo_state.delta_tx_m = (int64_t)s->delta_tx_m;
cur_servo_state.delta_rx_m = (int64_t)s->delta_rx_m;
......
......@@ -22,6 +22,7 @@ OBJ-libstd := $D/fsm-table.o \
$D/msg.o \
$D/arith.o \
$D/servo.o \
$D/hooks.o \
$D/open-close.o
$(TARGET).o: $(LIBSTD)
......
......@@ -84,6 +84,9 @@ void s1(struct pp_instance *ppi, MsgHeader *hdr, MsgAnnounce *ann)
DSPRO(ppi)->frequencyTraceable = ((hdr->flagField[1] & FFB_FTRA) != 0);
DSPRO(ppi)->ptpTimescale = ((hdr->flagField[1] & FFB_PTP) != 0);
DSPRO(ppi)->timeSource = ann->timeSource;
if (pp_hooks.s1)
pp_hooks.s1(ppi, hdr, ann);
}
......@@ -242,6 +245,11 @@ UInteger8 bmc_state_decision( struct pp_instance *ppi,
{
int cmpres;
if (OPTS(ppi)->master_only) {
m1(ppi);
return PPS_MASTER;
}
if (OPTS(ppi)->slave_only) {
s1(ppi, hdr, ann);
return PPS_SLAVE;
......
......@@ -10,6 +10,14 @@
int st_com_execute_slave(struct pp_instance *ppi, int check_delayreq)
{
int ret = 0;
if (pp_hooks.execute_slave)
ret = pp_hooks.execute_slave(ppi);
if (ret == 1) /* done: just return */
return 0;
if (ret < 0)
return ret;
if (pp_timer_expired(ppi->timers[PP_TIMER_ANN_RECEIPT])) {
PP_VPRINTF("event ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES\n");
ppi->number_foreign_records = 0;
......@@ -33,8 +41,7 @@ int st_com_execute_slave(struct pp_instance *ppi, int check_delayreq)
ret = msg_issue_delay_req(ppi);
set_TimeInternal(&ppi->delay_req_send_time,
ppi->last_snt_time.seconds, ppi->last_snt_time.nanoseconds);
ppi->delay_req_send_time = ppi->last_snt_time;
/* Add latency */
add_TimeInternal(&ppi->delay_req_send_time,
......@@ -173,6 +180,9 @@ int st_com_slave_handle_announce(struct pp_instance *ppi, unsigned char *buf,
/*Reset Timer handling Announce receipt timeout*/
st_com_restart_annrec_timer(ppi);
if (pp_hooks.handle_announce)
pp_hooks.handle_announce(ppi);
return 0;
}
......@@ -193,8 +203,7 @@ int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf,
time = &ppi->last_rcv_time;
if (ppi->is_from_cur_par) {
ppi->sync_receive_time.seconds = time->seconds;
ppi->sync_receive_time.nanoseconds = time->nanoseconds;
ppi->sync_receive_time = *time;
/* FIXME diag to file? will we ever handle it?
if (OPTS(ppi)->recordFP)
......@@ -241,6 +250,7 @@ int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
{
TimeInternal precise_orig_timestamp;
TimeInternal correction_field;
int ret = 0;
MsgHeader *hdr = &ppi->msg_tmp_header;
......@@ -273,6 +283,15 @@ int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
add_TimeInternal(&correction_field, &correction_field,
&ppi->last_sync_corr_field);
/* Call the extension; it may do it all and ask to return */
if (pp_hooks.handle_followup)
ret = pp_hooks.handle_followup(ppi, &precise_orig_timestamp,
&correction_field);
if (ret == 1)
return 0;
if (ret < 0)
return ret;
pp_update_offset(ppi, &precise_orig_timestamp,
&ppi->sync_receive_time,
&correction_field);
......
......@@ -39,4 +39,30 @@ int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
int st_com_handle_pdelay_req(struct pp_instance *ppi, unsigned char *buf,
int len);
#ifdef VERB_LOG_MSGS
#define MSG_SEND_AND_RET_VARLEN(x,y,z,w)\
if (pp_send_packet(ppi, ppi->buf_out, w,\
&ppi->last_snt_time, PP_NP_##y , z) < w) {\
PP_PRINTF("%s(%d) Message can't be sent -> FAULTY state!\n",\
pp_msg_names[PPM_##x], PPM_##x);\
return -1;\
}\
PP_VPRINTF("SENT %02d %d.%d %s \n", w,\
ppi->last_snt_time.seconds,\
ppi->last_snt_time.nanoseconds,pp_msg_names[PPM_##x]);\
ppi->sent_seq_id[PPM_## x]++;\
return 0;
#else
#define MSG_SEND_AND_RET_VARLEN(x,y,z,w)\
if (pp_send_packet(ppi, ppi->buf_out, w,\
&ppi->last_snt_time, PP_NP_##y , z) < w) {\
return -1;\
}\
ppi->sent_seq_id[PPM_## x]++;\
return 0;
#endif
#define MSG_SEND_AND_RET(x,y,z)\
MSG_SEND_AND_RET_VARLEN(x,y,z,PP_## x ##_LENGTH)
#endif /* __COMMON_FUN_H */
#include <ppsi/ppsi.h>
/* proto-standard offers all-null hooks as a default extension */
struct pp_ext_hooks pp_hooks;
......@@ -5,6 +5,7 @@
#include <ppsi/ppsi.h>
#include <ppsi/diag.h>
#include "common-fun.h"
static inline void Integer64_display(const char *label, Integer64 *bigint)
{
......@@ -71,6 +72,7 @@ static inline void msg_display_announce(MsgAnnounce *announce)
PP_VPRINTF("stepsRemoved: %d\n", announce->stepsRemoved);
PP_VPRINTF("timeSource: %d\n", announce->timeSource);
PP_VPRINTF("\n");
/* FIXME: diagnostic for extension */
#endif
}
......@@ -226,6 +228,8 @@ int msg_pack_announce(struct pp_instance *ppi)
*(UInteger16 *) (buf + 61) = htons(DSCUR(ppi)->stepsRemoved);
*(Enumeration8 *) (buf + 63) = DSPRO(ppi)->timeSource;
if (pp_hooks.pack_announce)
return pp_hooks.pack_announce(ppi);
return PP_ANNOUNCE_LENGTH;
}
......@@ -252,6 +256,8 @@ void msg_unpack_announce(void *buf, MsgAnnounce *ann)
ann->stepsRemoved = htons(*(UInteger16 *) (buf + 61));
ann->timeSource = *(Enumeration8 *) (buf + 63);
if (pp_hooks.unpack_announce)
pp_hooks.unpack_announce(buf, ann);
msg_display_announce(ann);
}
......@@ -592,37 +598,12 @@ const char const * pp_msg_names[] = {
"management"
};
#ifdef PPSI_SLAVE
#define MSG_SEND_AND_RET(x,y,z)\
if (pp_send_packet(ppi, ppi->buf_out, PP_## x ##_LENGTH,\
&ppi->last_snt_time, PP_NP_##y , z) < PP_## x ##_LENGTH) {\
PP_PRINTF("%s(%d) Message can't be sent -> FAULTY state!\n",\
pp_msg_names[PPM_##x], PPM_##x);\
return -1;\
}\
ppi->sent_seq_id[PPM_## x]++;\
return 0;
#else
#define MSG_SEND_AND_RET(x,y,z)\
if (pp_send_packet(ppi, ppi->buf_out, PP_## x ##_LENGTH,\
&ppi->last_snt_time, PP_NP_##y , z) < PP_## x ##_LENGTH) {\
PP_PRINTF("%s(%d) Message can't be sent -> FAULTY state!\n",\
pp_msg_names[PPM_##x], PPM_##x);\
return -1;\
}\
ppi->sent_seq_id[PPM_## x]++;\
PP_VPRINTF("SENT %02d %d.%d %s \n", PP_## x ##_LENGTH,\
ppi->last_snt_time.seconds,\
ppi->last_snt_time.nanoseconds,pp_msg_names[PPM_##x]);\
return 0;
#endif
/* Pack and send on general multicast ip adress an Announce message */
int msg_issue_announce(struct pp_instance *ppi)
{
msg_pack_announce(ppi);
MSG_SEND_AND_RET(ANNOUNCE, GEN, 0);
int len;
len = msg_pack_announce(ppi);
MSG_SEND_AND_RET_VARLEN(ANNOUNCE, GEN, 0, len);
}
/* Pack and send on event multicast ip adress a Sync message */
......
......@@ -9,8 +9,6 @@
* This file deals with opening and closing an instance. The channel
* must already have been created. In practices, this initializes the
* state machine to the first state.
*
* A protocol extension can override none or both of these functions.
*/
struct pp_runtime_opts default_rt_opts = {
......@@ -51,11 +49,14 @@ int pp_open_instance(struct pp_instance *ppi, struct pp_runtime_opts *rt_opts)
ppi->state = PPS_INITIALIZING;
if (pp_hooks.open)
return pp_hooks.open(ppi, ppi->rt_opts);
return 0;
}
int pp_close_instance(struct pp_instance *ppi)
{
/* Nothing to do by now */
if (pp_hooks.open)
return pp_hooks.close(ppi);
return 0;
}
......@@ -10,10 +10,13 @@
* Initializes network and other stuff
*/
int pp_initializing(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
unsigned char *id, *mac;
struct DSDefault *def = DSDEF(ppi);
struct DSPort *port = DSPOR(ppi);
struct pp_runtime_opts *opt = OPTS(ppi);
int ret = 0;
if (NP(ppi)->inited)
pp_net_shutdown(ppi);
......@@ -23,12 +26,12 @@ int pp_initializing(struct pp_instance *ppi, unsigned char *pkt, int plen)
NP(ppi)->inited = 1;
DSPOR(ppi)->portState = PPS_INITIALIZING;
port->portState = PPS_INITIALIZING;
/* Initialize default data set */
DSDEF(ppi)->twoStepFlag = PP_TWO_STEP_FLAG;
def->twoStepFlag = PP_TWO_STEP_FLAG;
/* Clock identity comes from mac address with 0xff:0xfe intermixed */
id = DSDEF(ppi)->clockIdentity;
id = def->clockIdentity;
mac = NP(ppi)->ch[PP_NP_GEN].addr;
id[0] = mac[0];
id[1] = mac[1];
......@@ -39,41 +42,49 @@ int pp_initializing(struct pp_instance *ppi, unsigned char *pkt, int plen)
id[6] = mac[4];
id[7] = mac[5];
DSDEF(ppi)->numberPorts = 1;
memcpy(&DSDEF(ppi)->clockQuality, &OPTS(ppi)->clock_quality,
def->numberPorts = 1;
memcpy(&def->clockQuality, &opt->clock_quality,
sizeof(ClockQuality));
DSDEF(ppi)->priority1 = OPTS(ppi)->prio1;
DSDEF(ppi)->priority2 = OPTS(ppi)->prio2;
DSDEF(ppi)->domainNumber = OPTS(ppi)->domain_number;
DSDEF(ppi)->slaveOnly = OPTS(ppi)->slave_only;
if (OPTS(ppi)->slave_only)
def->priority1 = opt->prio1;
def->priority2 = opt->prio2;
def->domainNumber = opt->domain_number;
def->slaveOnly = opt->slave_only;
if (opt->slave_only)
ppi->defaultDS->clockQuality.clockClass = 255;
/* Initialize port data set */
memcpy(ppi->portDS->portIdentity.clockIdentity,
ppi->defaultDS->clockIdentity, PP_CLOCK_IDENTITY_LENGTH);
DSPOR(ppi)->portIdentity.portNumber = 1;
DSPOR(ppi)->logMinDelayReqInterval = PP_DEFAULT_DELAYREQ_INTERVAL;
DSPOR(ppi)->peerMeanPathDelay.seconds = 0;
DSPOR(ppi)->peerMeanPathDelay.nanoseconds = 0;
DSPOR(ppi)->logAnnounceInterval = OPTS(ppi)->announce_intvl;
DSPOR(ppi)->announceReceiptTimeout =
PP_DEFAULT_ANNOUNCE_RECEIPT_TIMEOUT;
DSPOR(ppi)->logSyncInterval = OPTS(ppi)->sync_intvl;
DSPOR(ppi)->delayMechanism = PP_DEFAULT_DELAY_MECHANISM;
DSPOR(ppi)->logMinPdelayReqInterval = PP_DEFAULT_PDELAYREQ_INTERVAL;
DSPOR(ppi)->versionNumber = PP_VERSION_PTP;
if (pp_timer_init(ppi))
port->portIdentity.portNumber = 1;
port->logMinDelayReqInterval = PP_DEFAULT_DELAYREQ_INTERVAL;
port->peerMeanPathDelay.seconds = 0;
port->peerMeanPathDelay.nanoseconds = 0;
port->logAnnounceInterval = opt->announce_intvl;
port->announceReceiptTimeout = PP_DEFAULT_ANNOUNCE_RECEIPT_TIMEOUT;
port->logSyncInterval = opt->sync_intvl;
port->delayMechanism = PP_DEFAULT_DELAY_MECHANISM;
port->logMinPdelayReqInterval = PP_DEFAULT_PDELAYREQ_INTERVAL;
port->versionNumber = PP_VERSION_PTP;
if (pp_hooks.init)
ret = pp_hooks.init(ppi, pkt, plen);
if (ret) {
PP_PRINTF("%s: can't init extension\n");
goto failure;
}
ret = pp_timer_init(ppi);
if (ret) {
PP_PRINTF("%s: can't init timers\n");
goto failure;
}
pp_init_clock(ppi);
m1(ppi);
msg_pack_header(ppi, ppi->buf_out);
if (!OPTS(ppi)->master_only)
if (!opt->master_only)
ppi->next_state = PPS_LISTENING;
else
ppi->next_state = PPS_MASTER;
......@@ -81,8 +92,7 @@ int pp_initializing(struct pp_instance *ppi, unsigned char *pkt, int plen)
return 0;
failure:
PP_PRINTF("Failed to initialize network\n");
ppi->next_state = PPS_FAULTY;
ppi->next_delay = PP_DEFAULT_NEXT_DELAY_MS;
return 0;
return ret;
}
......@@ -11,6 +11,11 @@ int pp_listening(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
int e = 0; /* error var, to check errors in msg handling */
if (pp_hooks.listening)
e = pp_hooks.listening(ppi, pkt, plen);
if (e)
goto no_incoming_msg;
if (ppi->is_new_state) {
DSPOR(ppi)->portState = PPS_LISTENING;
st_com_restart_annrec_timer(ppi);
......
......@@ -14,7 +14,7 @@ int pp_master(struct pp_instance *ppi, unsigned char *pkt, int plen)
TimeInternal req_rec_tstamp;
TimeInternal correction_field;
TimeInternal resp_orig_tstamp;
int msgtype;
int e = 0; /* error var, to check errors in msg handling */
MsgHeader *hdr = &ppi->msg_tmp_header;
......@@ -67,7 +67,23 @@ int pp_master(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (plen == 0)
goto no_incoming_msg;
switch (ppi->msg_tmp_header.messageType) {
/*
* An extension can do special treatment of this message type,
* possibly returning error or eating the message by returning
* PPM_NOTHING_TO_DO
*/
msgtype = ppi->msg_tmp_header.messageType;
if (pp_hooks.master_msg)
msgtype = pp_hooks.master_msg(ppi, pkt, plen, msgtype);
if (msgtype < 0) {
e = msgtype;
goto failure;
}
switch (msgtype) {
case PPM_NOTHING_TO_DO:
break;
case PPM_ANNOUNCE:
e = st_com_master_handle_announce(ppi, pkt, plen);
......
......@@ -11,7 +11,6 @@ int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
int e = 0; /* error var, to check errors in msg handling */
TimeInternal *time;
TimeInternal req_rec_tstamp;
TimeInternal correction_field;
TimeInternal resp_orig_tstamp;
MsgHeader *hdr = &ppi->msg_tmp_header;
......@@ -20,8 +19,14 @@ int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (ppi->is_new_state) {
DSPOR(ppi)->portState = PPS_SLAVE;
pp_init_clock(ppi);
if (pp_hooks.new_slave)
e = pp_hooks.new_slave(ppi, pkt, plen);
if (e)
goto no_incoming_msg;
ppi->waiting_for_follow = FALSE;
ppi->pdelay_req_send_time.seconds = 0;
......@@ -96,18 +101,19 @@ int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen)
ppi->msg_tmp.resp.requestingPortIdentity.portNumber)
&& ppi->is_from_cur_par) {
to_TimeInternal(&req_rec_tstamp,
&ppi->msg_tmp.resp.receiveTimestamp);
ppi->delay_req_receive_time.seconds =
req_rec_tstamp.seconds;
ppi->delay_req_receive_time.nanoseconds =
req_rec_tstamp.nanoseconds;
to_TimeInternal(&ppi->delay_req_receive_time,
&ppi->msg_tmp.resp.receiveTimestamp);
int64_to_TimeInternal(
hdr->correctionfield,
&correction_field);
pp_update_delay(ppi, &correction_field);
if (pp_hooks.update_delay)
e = pp_hooks.update_delay(ppi);
else
pp_update_delay(ppi, &correction_field);
if (e)
goto no_incoming_msg;
ppi->log_min_delay_req_interval =
hdr->logMessageInterval;
......@@ -159,13 +165,9 @@ int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen)
ppi->pdelay_resp_receive_time.nanoseconds =
time->nanoseconds;
/* Store t2 (Fig 35) */
to_TimeInternal(&req_rec_tstamp,
&ppi->msg_tmp.presp.
to_TimeInternal(&ppi->pdelay_req_receive_time,
&ppi->msg_tmp.presp.
requestReceiptTimestamp);
ppi->pdelay_req_receive_time.seconds =
req_rec_tstamp.seconds;
ppi->pdelay_req_receive_time.nanoseconds =
req_rec_tstamp.nanoseconds;
int64_to_TimeInternal(hdr->correctionfield,
&correction_field);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment