Commit 50c32dc4 authored by Alessandro Rubini's avatar Alessandro Rubini

Merge branch 'new-time-structure'

Intermediate commits in the branch do not work or do not even build,
but I'd better keep the various parts separate.

We now have a unified "struct pp_time" everywhere, so se avoid a
number of conversions, some code is more readable and we are
eventually free from the 2038 bug.

All time is now represented as 64b seconds and 64b scaled_nsecs. The
scaling is 16 bits, like the correctionField, which is now added directly
to the time stamps.

Please expect some more housekeeping edits in the area, like
factorizing commong code and constants, or removing fields that are
not needed any more.
parents 828f7350 34ca7d7b
......@@ -16,7 +16,7 @@ unsigned long pp_global_d_flags; /* This is the only "global" file in ppsi */
static void pp_fsm_printf(struct pp_instance *ppi, char *fmt, ...)
{
va_list args;
TimeInternal t;
struct pp_time t;
unsigned long oflags = pp_global_d_flags;
if (!pp_diag_allow(ppi, fsm, 1))
......@@ -28,7 +28,7 @@ static void pp_fsm_printf(struct pp_instance *ppi, char *fmt, ...)
pp_global_d_flags = oflags;
pp_printf("diag-fsm-1-%s: %09d.%03d: ", ppi->port_name,
(int)t.seconds, (int)t.nanoseconds / 1000000);
(int)t.secs, (int)((t.scaled_nsecs >> 16)) / 1000000);
va_start(args, fmt);
pp_vprintf(fmt, args);
va_end(args);
......@@ -203,10 +203,10 @@ int pp_state_machine(struct pp_instance *ppi, uint8_t *packet, int plen)
if (plen) {
msgtype = packet[0] & 0xf;
pp_diag(ppi, frames, 1,
"RECV %02d bytes at %d.%09d (type %x, %s)\n", plen,
(int)ppi->last_rcv_time.seconds,
(int)ppi->last_rcv_time.nanoseconds, msgtype,
pp_msgtype_info[msgtype].name);
"RECV %02d bytes at %9d.%09d (type %x, %s)\n", plen,
(int)ppi->last_rcv_time.secs,
(int)(ppi->last_rcv_time.scaled_nsecs >> 16),
msgtype, pp_msgtype_info[msgtype].name);
}
/*
......
......@@ -11,6 +11,7 @@
#define __PPSI_IEEE_1588_TYPES_H__
#include <stdint.h>
#include <ppsi/pp-time.h>
/* See F.2, pag.223 */
#define PP_ETHERTYPE 0x88f7
......@@ -61,30 +62,6 @@ typedef struct Timestamp { /* page 13 (33) -- no typedef expected */
UInteger32 nanosecondsField;
} Timestamp;
typedef struct TimeInternal {
Integer32 seconds;
Integer32 nanoseconds;
/* White Rabbit extension begin */
Integer32 phase; /* This is the set point */
int correct; /* 0 or 1 */
#if 0
/*
* The following two fields may be used for diagnostics, but
* they cost space. So remove them but keep the code around just
* in case it is useful again (they are only set, never read)
*/
int32_t raw_phase;
int32_t raw_nsec;
#endif
int32_t raw_ahead; /* raw_ahead is used during calibration */
/* White Rabbit extension end */
} TimeInternal;
static inline void clear_TimeInternal(struct TimeInternal *t)
{
memset(t, 0, sizeof(*t));
}
typedef struct ClockIdentity { /* page 13 (33) */
Octet id[8];
} ClockIdentity;
......@@ -136,7 +113,7 @@ typedef struct MsgHeader {
UInteger16 messageLength;
UInteger8 domainNumber;
Octet flagField[2];
Integer64 correctionfield;
struct pp_time cField;;
PortIdentity sourcePortIdentity;
UInteger16 sequenceId;
UInteger8 controlField;
......@@ -158,40 +135,40 @@ typedef struct MsgAnnounce {
/* Sync Message (table 26, page 129) */
typedef struct MsgSync {
Timestamp originTimestamp;
struct pp_time originTimestamp;
} MsgSync;
/* DelayReq Message (table 26, page 129) */
typedef struct MsgDelayReq {
Timestamp originTimestamp;
struct pp_time originTimestamp;
} MsgDelayReq;
/* DelayResp Message (table 27, page 130) */
typedef struct MsgFollowUp {
Timestamp preciseOriginTimestamp;
struct pp_time preciseOriginTimestamp;
} MsgFollowUp;
/* DelayResp Message (table 28, page 130) */
typedef struct MsgDelayResp {
Timestamp receiveTimestamp;
struct pp_time receiveTimestamp;
PortIdentity requestingPortIdentity;
} MsgDelayResp;
/* PdelayReq Message (table 29, page 131) */
typedef struct MsgPDelayReq {
Timestamp originTimestamp;
struct pp_time originTimestamp;
} MsgPDelayReq;
/* PdelayResp Message (table 30, page 131) */
typedef struct MsgPDelayResp {
Timestamp requestReceiptTimestamp;
struct pp_time requestReceiptTimestamp;
PortIdentity requestingPortIdentity;
} MsgPDelayResp;
/* PdelayRespFollowUp Message (table 31, page 132) */
typedef struct MsgPDelayRespFollowUp {
Timestamp responseOriginTimestamp;
struct pp_time responseOriginTimestamp;
PortIdentity requestingPortIdentity;
} MsgPDelayRespFollowUp;
......@@ -229,8 +206,8 @@ typedef struct DSDefault { /* page 65 */
typedef struct DSCurrent { /* page 67 */
/* Dynamic */
UInteger16 stepsRemoved;
TimeInternal offsetFromMaster;
TimeInternal meanPathDelay; /* oneWayDelay */
struct pp_time offsetFromMaster;
struct pp_time meanPathDelay; /* oneWayDelay */
/* White Rabbit extension begin */
UInteger16 primarySlavePortNumber;
/* White Rabbit extension end */
......@@ -256,7 +233,6 @@ typedef struct DSPort { /* page 72 */
/* Dynamic */
/* Enumeration8 portState; -- not used */
Integer8 logMinDelayReqInterval; /* -- same as pdelay one */
/* TimeInternal peerMeanPathDelay; -- not used */
/* Configurable */
Integer8 logAnnounceInterval;
UInteger8 announceReceiptTimeout;
......
......@@ -82,14 +82,14 @@ struct pp_frgn_master {
* where increasing the stiffness (s) lowers the cutoff and increases the delay.
*/
struct pp_avg_fltr {
Integer32 m; /* magnitude */
Integer32 y;
Integer32 s_exp;
int64_t m; /* magnitude */
int64_t y;
int64_t s_exp;
};
struct pp_servo {
TimeInternal m_to_s_dly;
TimeInternal s_to_m_dly;
struct pp_time m_to_s_dly;
struct pp_time s_to_m_dly;
long long obs_drift;
struct pp_avg_fltr mpd_fltr;
};
......@@ -149,10 +149,10 @@ struct pp_instance {
uint16_t peer_vid; /* Our peer's VID (for PROTO_VLAN) */
/* Times, for the various offset computations */
TimeInternal t1, t2, t3, t4, t5, t6; /* *the* stamps */
struct pp_time t1, t2, t3, t4, t5, t6; /* *the* stamps */
Integer32 t4_cf, t6_cf; /* peer delay */
TimeInternal cField; /* transp. clocks */
TimeInternal last_rcv_time, last_snt_time; /* two temporaries */
struct pp_time cField; /* transp. clocks */
struct pp_time last_rcv_time, last_snt_time; /* two temporaries */
/* Page 85: each port shall maintain an implementation-specific
* foreignMasterDS data set for the purposes of qualifying Announce
......
#ifndef __PPSI_PP_TIME_H__
#define __PPSI_PP_TIME_H__
/* Everything internally uses this time format, *signed* */
struct pp_time {
int64_t secs;
int64_t scaled_nsecs;
};
/* The "correct" bit is hidden in the hight bits */
static inline int is_incorrect(const struct pp_time *t)
{
return (((t->secs >> 56) & 0xc0) == 0x80);
}
static inline void mark_incorrect(struct pp_time *t)
{
t->secs |= (0x8fLL << 56);
}
static inline void clear_time(struct pp_time *t)
{
memset(t, 0, sizeof(*t));
}
#endif /* __PPSI_PP_TIME_H__ */
......@@ -22,6 +22,7 @@
#include <arch/arch.h> /* ntohs and so on -- and wr-api.h for wr archs */
/* At this point in time, we need ARRAY_SIZE to conditionally build vlan code */
#undef ARRAY_SIZE
#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
......@@ -154,8 +155,8 @@ struct pp_ext_hooks {
void (*s1)(struct pp_instance *ppi, MsgHeader *hdr, MsgAnnounce *ann);
int (*execute_slave)(struct pp_instance *ppi);
int (*handle_announce)(struct pp_instance *ppi);
int (*handle_followup)(struct pp_instance *ppi, TimeInternal *orig,
TimeInternal *correction_field);
int (*handle_followup)(struct pp_instance *ppi, struct pp_time *orig,
struct pp_time *correction_field);
int (*handle_preq) (struct pp_instance * ppi);
int (*handle_presp) (struct pp_instance * ppi);
int (*pack_announce)(struct pp_instance *ppi);
......@@ -173,7 +174,7 @@ struct pp_network_operations {
int (*init)(struct pp_instance *ppi);
int (*exit)(struct pp_instance *ppi);
int (*recv)(struct pp_instance *ppi, void *pkt, int len,
TimeInternal *t);
struct pp_time *t);
int (*send)(struct pp_instance *ppi, void *pkt, int len, int msgtype);
int (*check_packet)(struct pp_globals *ppg, int delay_ms);
};
......@@ -192,8 +193,8 @@ extern struct pp_network_operations unix_net_ops;
* If "set" receives a NULL time value, it should update the TAI offset.
*/
struct pp_time_operations {
int (*get)(struct pp_instance *ppi, TimeInternal *t);
int (*set)(struct pp_instance *ppi, TimeInternal *t);
int (*get)(struct pp_instance *ppi, struct pp_time *t);
int (*set)(struct pp_instance *ppi, const struct pp_time *t);
/* freq_ppb is parts per billion */
int (*adjust)(struct pp_instance *ppi, long offset_ns, long freq_ppb);
int (*adjust_offset)(struct pp_instance *ppi, long offset_ns);
......@@ -380,19 +381,16 @@ extern void *msg_copy_header(MsgHeader *dest, MsgHeader *src); /* REMOVE ME!! */
extern int msg_issue_announce(struct pp_instance *ppi);
extern int msg_issue_sync_followup(struct pp_instance *ppi);
extern int msg_issue_request(struct pp_instance *ppi);
extern int msg_issue_delay_resp(struct pp_instance *ppi, TimeInternal *time);
extern int msg_issue_delay_resp(struct pp_instance *ppi,
struct pp_time *time);
extern int msg_issue_pdelay_resp_followup(struct pp_instance *ppi,
TimeInternal * time);
extern int msg_issue_pdelay_resp(struct pp_instance *ppi, TimeInternal * time);
/* Functions for timestamp handling (internal to protocol format conversion*/
/* FIXME: add prefix in function name? */
extern void cField_to_TimeInternal(TimeInternal *internal, Integer64 bigint);
extern int from_TimeInternal(TimeInternal *internal, Timestamp *external);
extern int to_TimeInternal(TimeInternal *internal, Timestamp *external);
extern void add_TimeInternal(TimeInternal *r, TimeInternal *x, TimeInternal *y);
extern void sub_TimeInternal(TimeInternal *r, TimeInternal *x, TimeInternal *y);
extern void div2_TimeInternal(TimeInternal *r);
struct pp_time *time);
extern int msg_issue_pdelay_resp(struct pp_instance *ppi, struct pp_time *time);
/* Functions for time math */
extern void pp_time_add(struct pp_time *t1, struct pp_time *t2);
extern void pp_time_sub(struct pp_time *t1, struct pp_time *t2);
extern void pp_time_div2(struct pp_time *t);
/*
* The state machine itself is an array of these structures.
......
......@@ -73,9 +73,9 @@ static int wr_listening(struct pp_instance *ppi, unsigned char *pkt, int plen)
static int wr_handle_preq(struct pp_instance *ppi)
{
ppi->received_ptp_header.correctionfield.msb = 0;
ppi->received_ptp_header.correctionfield.lsb =
phase_to_cf_units(ppi->last_rcv_time.phase);
/* FIXME: why do we save this fractional part? */
clear_time(&ppi->cField);
ppi->cField.scaled_nsecs = ppi->last_rcv_time.scaled_nsecs & 0xffff;
return 0;
}
......@@ -84,7 +84,7 @@ static int wr_master_msg(struct pp_instance *ppi, unsigned char *pkt, int plen,
{
MsgHeader *hdr = &ppi->received_ptp_header;
MsgSignaling wrsig_msg;
TimeInternal *time = &ppi->last_rcv_time;
struct pp_time *time = &ppi->last_rcv_time;
if (msgtype != PPM_NO_MESSAGE)
pp_diag(ppi, ext, 2, "hook: %s\n", __func__);
......@@ -93,9 +93,8 @@ static int wr_master_msg(struct pp_instance *ppi, unsigned char *pkt, int plen,
/* This case is modified from the default one */
case PPM_DELAY_REQ:
hdr->correctionfield.msb = 0;
hdr->correctionfield.lsb =
phase_to_cf_units(ppi->last_rcv_time.phase);
/* there is no cField now, we pp_time includes it all */
clear_time(&hdr->cField);
msg_issue_delay_resp(ppi, time); /* no error check */
msgtype = PPM_NO_MESSAGE;
break;
......@@ -131,15 +130,12 @@ static int wr_new_slave(struct pp_instance *ppi, unsigned char *pkt, int plen)
static int wr_handle_resp(struct pp_instance *ppi)
{
MsgHeader *hdr = &ppi->received_ptp_header;
TimeInternal correction_field;
TimeInternal *ofm = &DSCUR(ppi)->offsetFromMaster;
struct pp_time *ofm = &DSCUR(ppi)->offsetFromMaster;
struct wr_dsport *wrp = WR_DSPOR(ppi);
pp_diag(ppi, ext, 2, "hook: %s\n", __func__);
/* FIXME: check sub-nano relevance of correction filed */
cField_to_TimeInternal(&correction_field, hdr->correctionfield);
/* This correction_field we received is already part of t4 */
/*
* If no WR mode is on, run normal code, if T2/T3 are valid.
......@@ -147,7 +143,7 @@ static int wr_handle_resp(struct pp_instance *ppi)
* we'll have the Unix time instead, marked by "correct"
*/
if (!wrp->wrModeOn) {
if (!ppi->t2.correct || !ppi->t3.correct) {
if (is_incorrect(&ppi->t2) || is_incorrect(&ppi->t3)) {
pp_diag(ppi, servo, 1,
"T2 or T3 incorrect, discarding tuple\n");
return 0;
......@@ -156,13 +152,13 @@ static int wr_handle_resp(struct pp_instance *ppi)
/*
* pps always on if offset less than 1 second,
* until ve have a configurable threshold */
if (ofm->seconds)
if (ofm->secs)
wrp->ops->enable_timing_output(ppi, 0);
else
wrp->ops->enable_timing_output(ppi, 1);
}
wr_servo_got_delay(ppi, hdr->correctionfield.lsb);
wr_servo_got_delay(ppi);
wr_servo_update(ppi);
return 0;
}
......@@ -206,14 +202,13 @@ static int wr_handle_announce(struct pp_instance *ppi)
}
static int wr_handle_followup(struct pp_instance *ppi,
TimeInternal *precise_orig_timestamp,
TimeInternal *correction_field)
struct pp_time *precise_orig_timestamp,
struct pp_time *correction_field)
{
pp_diag(ppi, ext, 2, "hook: %s\n", __func__);
if (!WR_DSPOR(ppi)->wrModeOn)
return 0;
precise_orig_timestamp->phase = 0;
wr_servo_got_sync(ppi, precise_orig_timestamp,
&ppi->t2);
......@@ -225,13 +220,8 @@ static int wr_handle_followup(struct pp_instance *ppi,
static __attribute__((used)) int wr_handle_presp(struct pp_instance *ppi)
{
MsgHeader *hdr = &ppi->received_ptp_header;
TimeInternal correction_field;
struct wr_dsport *wrp = WR_DSPOR(ppi);
TimeInternal *ofm = &DSCUR(ppi)->offsetFromMaster;
/* FIXME: check sub-nano relevance of correction filed */
cField_to_TimeInternal(&correction_field, hdr->correctionfield);
struct pp_time *ofm = &DSCUR(ppi)->offsetFromMaster;
/*
* If no WR mode is on, run normal code, if T2/T3 are valid.
......@@ -240,7 +230,7 @@ static __attribute__((used)) int wr_handle_presp(struct pp_instance *ppi)
*/
if (!wrp->wrModeOn) {
if (!ppi->t3.correct || !ppi->t6.correct) {
if (is_incorrect(&ppi->t3) || is_incorrect(&ppi->t6)) {
pp_diag(ppi, servo, 1,
"T3 or T6 incorrect, discarding tuple\n");
return 0;
......@@ -249,7 +239,7 @@ static __attribute__((used)) int wr_handle_presp(struct pp_instance *ppi)
/*
* pps always on if offset less than 1 second,
* until ve have a configurable threshold */
if (ofm->seconds)
if (ofm->secs)
wrp->ops->enable_timing_output(ppi, 0);
else
wrp->ops->enable_timing_output(ppi, 1);
......@@ -257,8 +247,8 @@ static __attribute__((used)) int wr_handle_presp(struct pp_instance *ppi)
return 0;
}
ppi->t4_cf = hdr->correctionfield.lsb;
wr_servo_got_delay(ppi, ppi->t4_cf);
/* FIXME: verify that last-received cField is already accounted for */
wr_servo_got_delay(ppi);
return 0;
}
......
......@@ -12,8 +12,7 @@
#include <ppsi/lib.h>
#include "wr-constants.h"
#define WRS_PPSI_SHMEM_VERSION 17 /* Changed definition of field mcast_addr of
* struct pp_instance */
#define WRS_PPSI_SHMEM_VERSION 18 /* New pp_time everywhere */
/*
* This structure is used as extension-specific data in the DSPort
......@@ -133,9 +132,9 @@ enum {
int wr_servo_init(struct pp_instance *ppi);
void wr_servo_reset(struct pp_instance *ppi);
void wr_servo_enable_tracking(int enable);
int wr_servo_got_sync(struct pp_instance *ppi, TimeInternal *t1,
TimeInternal *t2);
int wr_servo_got_delay(struct pp_instance *ppi, Integer32 cf);
int wr_servo_got_sync(struct pp_instance *ppi, struct pp_time *t1,
struct pp_time *t2);
int wr_servo_got_delay(struct pp_instance *ppi);
int wr_servo_update(struct pp_instance *ppi);
struct wr_servo_state {
......@@ -156,12 +155,12 @@ struct wr_servo_state {
int32_t clock_period_ps;
/* These fields are used by servo code, across iterations */
TimeInternal t1, t2, t3, t4, t5, t6;
struct pp_time t1, t2, t3, t4, t5, t6;
int64_t delta_ms_prev;
int missed_iters;
/* Following fields are for monitoring/diagnostics (use w/ shmem) */
TimeInternal mu;
struct pp_time mu;
int64_t picos_mu;
int32_t cur_setpoint;
int64_t delta_ms;
......@@ -176,14 +175,14 @@ struct wr_servo_state {
uint32_t n_err_state;
uint32_t n_err_offset;
uint32_t n_err_delta_rtt;
TimeInternal update_time;
struct pp_time update_time;
};
int wr_p2p_delay(struct pp_instance *ppi, struct wr_servo_state *s);
int wr_e2e_offset(struct pp_instance *ppi,
struct wr_servo_state *s, TimeInternal *ts_offset_hw);
struct wr_servo_state *s, struct pp_time *ts_offset_hw);
int wr_p2p_offset(struct pp_instance *ppi,
struct wr_servo_state *s, TimeInternal *ts_offset_hw);
struct wr_servo_state *s, struct pp_time *ts_offset_hw);
/* All data used as extension ppsi-wr must be put here */
......
......@@ -36,105 +36,78 @@ void wr_servo_enable_tracking(int enable)
/* my own timestamp arithmetic functions */
static void dump_timestamp(struct pp_instance *ppi, char *what, TimeInternal ts)
static void dump_timestamp(struct pp_instance *ppi, char *what,
struct pp_time ts)
{
pp_diag(ppi, servo, 2, "%s = %d:%d:%d\n", what, (int32_t)ts.seconds,
ts.nanoseconds, ts.phase);
pp_diag(ppi, servo, 2, "%s = %ld:%ld:%ld\n", what, (long)ts.secs,
(long)(ts.scaled_nsecs >> 16),
/* unlikely what we had earlier, third field is not phase */
((long)(ts.scaled_nsecs & 0xffff) * 1000 + 0x8000) >> 16);
}
static int64_t ts_to_picos(TimeInternal ts)
static int64_t ts_to_picos(struct pp_time *ts)
{
return ts.seconds * 1000000000000LL
+ ts.nanoseconds * 1000LL
+ ts.phase;
return ts->secs * PP_NSEC_PER_SEC
+ ((ts->scaled_nsecs * 1000 + 0x8000) >> 16);
}
static TimeInternal picos_to_ts(int64_t picos)
static void picos_to_ts(int64_t picos, struct pp_time *ts)
{
uint64_t nsec, phase;
TimeInternal ts;
uint64_t sec, nsec;
int phase;
int sign = (picos < 0 ? -1 : 1);
picos *= sign;
nsec = picos;
phase = __div64_32(&nsec, 1000);
sec = nsec;
ts.nanoseconds = __div64_32(&nsec, PP_NSEC_PER_SEC);
ts.seconds = nsec; /* after the division */
ts.phase = phase;
return ts;
}
static TimeInternal ts_add(TimeInternal a, TimeInternal b)
{
TimeInternal c;
c.phase = a.phase + b.phase;
c.nanoseconds = a.nanoseconds + b.nanoseconds;
c.seconds = a.seconds + b.seconds;
while (c.phase >= 1000) {
c.phase -= 1000;
c.nanoseconds++;
}
while (c.nanoseconds >= PP_NSEC_PER_SEC) {
c.nanoseconds -= PP_NSEC_PER_SEC;
c.seconds++;
}
return c;
}
static TimeInternal ts_sub(TimeInternal a, TimeInternal b)
{
TimeInternal c;
c.phase = a.phase - b.phase;
c.nanoseconds = a.nanoseconds - b.nanoseconds;
c.seconds = a.seconds - b.seconds;
while(c.phase < 0) {
c.phase += 1000;
c.nanoseconds--;
}
while(c.nanoseconds < 0) {
c.nanoseconds += PP_NSEC_PER_SEC;
c.seconds--;
}
return c;
ts->scaled_nsecs = __div64_32(&sec, PP_NSEC_PER_SEC) << 16;
ts->scaled_nsecs += (phase << 16) / 1000;
ts->scaled_nsecs *= sign;
ts->secs = sec * sign;
}
/* "Hardwarizes" the timestamp - e.g. makes the nanosecond field a multiple
* of 8ns cycles and puts the extra nanoseconds in the phase field */
static TimeInternal ts_hardwarize(TimeInternal ts, int clock_period_ps)
* of 8/16ns cycles and puts the extra nanoseconds in the picos result */
static void ts_hardwarize(struct pp_time *time, int clock_period_ps,
int32_t *ticks, int32_t *picos)
{
int32_t q_threshold;
q_threshold = (clock_period_ps + 999) / 1000;
if (ts.nanoseconds > 0) {
int32_t extra_nsec = ts.nanoseconds % q_threshold;
if(extra_nsec) {
ts.nanoseconds -= extra_nsec;
ts.phase += extra_nsec * 1000;
}
int32_t s, ns, ps, clock_ns;
/* clock_period_ps *must* be a multiple of 1000 -- assert()? */
clock_ns = clock_period_ps / 1000;
/*
* In pp_time, both sec/nsec are positive, or both negative.
* Only 0 secs can have positive or negative nsecs.
*
* Here we need a positive count for both tick and picos. Or not.
* The code here replicates what found in original WR code.
*/
s = time->secs; /* a difference: known to fit 32 bits (really?) */
ps = time->scaled_nsecs & 0xffff; /* fractional nano */
ps = (ps * 1000) >> 16; /* now picoseconds 0..999 -- positive*/
ns = time->scaled_nsecs >> 16;
if (ns > 0) {
ps += (ns % clock_ns) * 1000;
ns -= (ns % clock_ns);
}
if (ts.nanoseconds < 0) {
ts.nanoseconds += PP_NSEC_PER_SEC;
ts.seconds--;
if (ns < 0) {
s--;
ns += PP_NSEC_PER_SEC;
}
if (ts.seconds == -1 && ts.nanoseconds > 0) {
ts.seconds++;
ts.nanoseconds -= PP_NSEC_PER_SEC;
if (s == -1 && ns > 0) {
s++;
ns -= PP_NSEC_PER_SEC;
}
if (ts.nanoseconds < 0 && ts.nanoseconds >= (-q_threshold)
&& ts.seconds == 0) {
ts.nanoseconds += q_threshold;
ts.phase -= q_threshold * 1000;
if (ns < 0 && s == 0 && ns >= -clock_ns) {
/* originally, ns was a multiple of clock_ns, code differs */
ps += ns * 1000;
ns = 0;
}
return ts;
*ticks = ns;
*picos = ps;
}
/* end my own timestamp arithmetic functions */
......@@ -219,22 +192,19 @@ int wr_servo_init(struct pp_instance *ppi)
return 0;
}
int wr_servo_got_sync(struct pp_instance *ppi, TimeInternal *t1,
TimeInternal *t2)
int wr_servo_got_sync(struct pp_instance *ppi, struct pp_time *t1,
struct pp_time *t2)
{
struct wr_servo_state *s =
&((struct wr_data *)ppi->ext_data)->servo_state;
s->t1 = *t1;
s->t1.correct = 1;
s->t2 = *t2;
got_sync = 1;
return 0;
}
int wr_servo_got_delay(struct pp_instance *ppi, Integer32 cf)
int wr_servo_got_delay(struct pp_instance *ppi)
{
struct wr_servo_state *s =
&((struct wr_data *)ppi->ext_data)->servo_state;
......@@ -244,15 +214,11 @@ int wr_servo_got_delay(struct pp_instance *ppi, Integer32 cf)
s->t3 = ppi->t3;
/* s->t3.phase = 0; */
s->t4 = ppi->t4;
s->t4.correct = 1; /* clock->delay_req_receive_time.correct; */
s->t4.phase = (int64_t) cf * 1000LL / 65536LL;
/* FIXME: verify that cField is already merged */
if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH) {
s->t5 = ppi->t5;
s->t5.correct = 1;
s->t5.phase = 0;
s->t6 = ppi->t6;
s->t6.phase = (int64_t) ppi->t6_cf * 1000LL / 65536LL;
wr_p2p_delay(ppi, s);
}
......@@ -266,20 +232,28 @@ int wr_p2p_delay(struct pp_instance *ppi, struct wr_servo_state *s)
uint64_t big_delta_fix;
static int errcount;
if (!s->t3.correct || !s->t4.correct ||
!s->t5.correct || !s->t6.correct) {
if (is_incorrect(&s->t3) || is_incorrect(&s->t4)
|| is_incorrect(&s->t5) || is_incorrect(&s->t6)) {
errcount++;
if (errcount > 5) /* a 2-3 in a row are expected */
pp_error("%s: TimestampsIncorrect: %d %d %d %d\n",
__func__, s->t3.correct, s->t4.correct,
s->t5.correct, s->t6.correct);
__func__, !is_incorrect(&s->t3),
!is_incorrect(&s->t4), !is_incorrect(&s->t5),
!is_incorrect(&s->t6));
return 0;
}
errcount = 0;
s->update_count++;
s->mu = ts_sub(ts_sub(s->t6, s->t3), ts_sub(s->t5, s->t4));
{ /* avoid modifying stamps in place */
struct pp_time mtime, stime;
stime = s->t6; pp_time_sub(&stime, &s->t3);
mtime = s->t5; pp_time_sub(&mtime, &s->t4);
s->mu = stime; pp_time_sub(&s->mu, &mtime);
}
if (__PP_DIAG_ALLOW(ppi, pp_dt_servo, 1)) {
dump_timestamp(ppi, "servo:t1", s->t1);
......@@ -291,30 +265,31 @@ int wr_p2p_delay(struct pp_instance *ppi, struct wr_servo_state *s)
dump_timestamp(ppi, "->mdelay", s->mu);
}
s->picos_mu = ts_to_picos(s->mu);
s->picos_mu = ts_to_picos(&s->mu);
big_delta_fix = s->delta_tx_m + s->delta_tx_s
+ s->delta_rx_m + s->delta_rx_s;
s->delta_ms =
(((int64_t) (ts_to_picos(s->mu) - big_delta_fix) *
(((int64_t) (ts_to_picos(&s->mu) - big_delta_fix) *
(int64_t) s->fiber_fix_alpha) >> FIX_ALPHA_FRACBITS)
+ ((ts_to_picos(s->mu) - big_delta_fix) >> 1)
+ ((ts_to_picos(&s->mu) - big_delta_fix) >> 1)
+ s->delta_tx_m + s->delta_rx_s;
return 1;
}
int wr_p2p_offset(struct pp_instance *ppi,
struct wr_servo_state *s, TimeInternal *ts_offset_hw)
struct wr_servo_state *s, struct pp_time *ts_offset)
{
TimeInternal ts_offset;
static int errcount;
struct pp_time time_ms;
if (!s->t1.correct || !s->t2.correct) {
if (is_incorrect(&s->t1) || is_incorrect(&s->t2)) {
errcount++;
if (errcount > 5) /* a 2-3 in a row are expected */
pp_error("%s: TimestampsIncorrect: %d %d \n",
__func__, s->t1.correct, s->t2.correct);
__func__, !is_incorrect(&s->t1),
!is_incorrect(&s->t2));
return 0;
}
errcount = 0;
......@@ -322,8 +297,10 @@ int wr_p2p_offset(struct pp_instance *ppi,
s->update_count++;
ts_offset = ts_add(ts_sub(s->t1, s->t2), picos_to_ts(s->delta_ms));
*ts_offset_hw = ts_hardwarize(ts_offset, s->clock_period_ps);
picos_to_ts(s->delta_ms, &time_ms);
*ts_offset = s->t1;
pp_time_sub(ts_offset, &s->t2);
pp_time_add(ts_offset, &time_ms);
/* is it possible to calculate it in client,
* but then t1 and t2 require shmem locks */
......@@ -335,35 +312,41 @@ int wr_p2p_offset(struct pp_instance *ppi,
}
int wr_e2e_offset(struct pp_instance *ppi,
struct wr_servo_state *s, TimeInternal *ts_offset_hw)
struct wr_servo_state *s, struct pp_time *ts_offset)
{
struct wr_dsport *wrp = WR_DSPOR(ppi);
uint64_t big_delta_fix;
uint64_t delay_ms_fix;
TimeInternal ts_offset;
static int errcount;
if(!s->t1.correct || !s->t2.correct ||
!s->t3.correct || !s->t4.correct) {
if (is_incorrect(&s->t1) || is_incorrect(&s->t2)
|| is_incorrect(&s->t3) || is_incorrect(&s->t4)) {
errcount++;
if (errcount > 5) /* a 2-3 in a row are expected */
pp_error("%s: TimestampsIncorrect: %d %d %d %d\n",
__func__, s->t1.correct, s->t2.correct,
s->t3.correct, s->t4.correct);
__func__, !is_incorrect(&s->t1),
!is_incorrect(&s->t2), !is_incorrect(&s->t3),
!is_incorrect(&s->t4));
return 0;
}
if (wrp->ops->servo_hook)
if (wrp->ops->servo_hook) /* FIXME: check this, missing in p2p */
wrp->ops->servo_hook(s, WR_SERVO_ENTER);
errcount = 0;
s->update_count++;
ppi->t_ops->get(ppi, &s->update_time);
ppi->t_ops->get(ppi, &s->update_time); /* FIXME: missing in p2p */
got_sync = 0;
s->mu = ts_sub(ts_sub(s->t4, s->t1), ts_sub(s->t3, s->t2));
{ /* avoid modifying stamps in place */
struct pp_time mtime, stime;
mtime = s->t4; pp_time_sub(&mtime, &s->t1);
stime = s->t3; pp_time_sub(&stime, &s->t2);
s->mu = mtime; pp_time_sub(&s->mu, &stime);
}
if (__PP_DIAG_ALLOW(ppi, pp_dt_servo, 1)) {
dump_timestamp(ppi, "servo:t1", s->t1);
......@@ -373,7 +356,7 @@ int wr_e2e_offset(struct pp_instance *ppi,
dump_timestamp(ppi, "->mdelay", s->mu);
}
s->picos_mu = ts_to_picos(s->mu);
s->picos_mu = ts_to_picos(&s->mu);
big_delta_fix = s->delta_tx_m + s->delta_tx_s
+ s->delta_rx_m + s->delta_rx_s;
......@@ -382,12 +365,20 @@ int wr_e2e_offset(struct pp_instance *ppi,
s->picos_mu = big_delta_fix;
}
delay_ms_fix = (((int64_t)(s->picos_mu - big_delta_fix) * (int64_t) s->fiber_fix_alpha) >> FIX_ALPHA_FRACBITS)
+ ((s->picos_mu - big_delta_fix) >> 1)
+ s->delta_tx_m + s->delta_rx_s;
delay_ms_fix = (((s->picos_mu - big_delta_fix)
* s->fiber_fix_alpha) >> FIX_ALPHA_FRACBITS)
+ ((s->picos_mu - big_delta_fix) >> 1)
+ s->delta_tx_m + s->delta_rx_s;
{ /* again, use temps to avoid modifying tx in place */
struct pp_time tmp = s->t1, tmp2;
ts_offset = ts_add(ts_sub(s->t1, s->t2), picos_to_ts(delay_ms_fix));
*ts_offset_hw = ts_hardwarize(ts_offset, s->clock_period_ps);
pp_time_sub(&tmp, &s->t2);
picos_to_ts(delay_ms_fix, &tmp2);
pp_time_add(&tmp, &tmp2);
*ts_offset = tmp;
}
/* is it possible to calculate it in client,
* but then t1 and t2 require shmem locks */
......@@ -408,7 +399,9 @@ int wr_servo_update(struct pp_instance *ppi)
int remaining_offset;
int64_t picos_mu_prev = 0;
TimeInternal ts_offset_hw /*, ts_phase_adjust */ ;
struct pp_time ts_offset;
int32_t ts_offset_ticks;
int32_t ts_offset_picos;
if (!got_sync)
return 0;
......@@ -418,13 +411,20 @@ int wr_servo_update(struct pp_instance *ppi)
picos_mu_prev = s->picos_mu;
if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH) {
if (!wr_p2p_offset(ppi, s, &ts_offset_hw))
if (!wr_p2p_offset(ppi, s, &ts_offset))
goto out;
} else {
if (!wr_e2e_offset(ppi, s, &ts_offset_hw))
if (!wr_e2e_offset(ppi, s, &ts_offset))
goto out;
}
ts_hardwarize(&ts_offset, s->clock_period_ps,
&ts_offset_ticks, &ts_offset_picos);
pp_diag(ppi, servo, 2, "offset_hw: %li.%09li (+%li)\n",
(long)ts_offset.secs, (long)ts_offset_ticks,
(long)ts_offset_picos);
if (wrp->ops->locking_poll(ppi, 0) != WR_SPLL_READY) {
pp_diag(ppi, servo, 1, "PLL OutOfLock, should restart sync\n");
wrp->ops->enable_timing_output(ppi, 0);
......@@ -441,16 +441,12 @@ int wr_servo_update(struct pp_instance *ppi)
}
/* So, we didn't return. Choose the right state */
if (ts_offset_hw.seconds) /* so bad... */
if (ts_offset.secs) /* so bad... */
s->state = WR_SYNC_TAI;
else if (ts_offset_hw.nanoseconds) /* not that bad */
else if (ts_offset_ticks) /* not that bad */
s->state = WR_SYNC_NSEC;
/* else, let the states below choose the sequence */
pp_diag(ppi, servo, 2, "offset_hw: %li.%09li (+%li)\n",
(long)ts_offset_hw.seconds, (long)ts_offset_hw.nanoseconds,
(long)ts_offset_hw.phase);
pp_diag(ppi, servo, 1, "wr_servo state: %s%s\n",
servo_name[s->state],
s->flags & WR_FLAG_WAIT_HW ? " (wait for hw)" : "");
......@@ -460,7 +456,7 @@ int wr_servo_update(struct pp_instance *ppi)
switch (s->state) {
case WR_SYNC_TAI:
wrp->ops->adjust_counters(ts_offset_hw.seconds, 0);
wrp->ops->adjust_counters(ts_offset.secs, 0);
s->flags |= WR_FLAG_WAIT_HW;
/*
* If nsec wrong, code above forces SYNC_NSEC,
......@@ -471,16 +467,16 @@ int wr_servo_update(struct pp_instance *ppi)
break;
case WR_SYNC_NSEC:
wrp->ops->adjust_counters(0, ts_offset_hw.nanoseconds);
wrp->ops->adjust_counters(0, ts_offset_ticks);
s->flags |= WR_FLAG_WAIT_HW;
s->state = WR_SYNC_PHASE;
break;
case WR_SYNC_PHASE:
pp_diag(ppi, servo, 2, "oldsetp %i, offset %i:%04i\n",
s->cur_setpoint, ts_offset_hw.nanoseconds,
ts_offset_hw.phase);
s->cur_setpoint += ts_offset_hw.phase;
s->cur_setpoint, ts_offset_ticks,
ts_offset_picos);
s->cur_setpoint += ts_offset_picos;
wrp->ops->adjust_phase(s->cur_setpoint);
s->flags |= WR_FLAG_WAIT_HW;
......@@ -496,14 +492,14 @@ int wr_servo_update(struct pp_instance *ppi)
*/
unix_time_ops.set(ppi, &ppi->t4);
pp_diag(ppi, time, 1, "system time set to %li TAI\n",
(long)ppi->t4.seconds);
(long)ppi->t4.secs);
}
break;
case WR_WAIT_OFFSET_STABLE:
/* ts_to_picos() below returns phase alone */
remaining_offset = abs(ts_to_picos(ts_offset_hw));
remaining_offset = abs(ts_offset_picos);
if(remaining_offset < WR_SERVO_OFFSET_STABILITY_THRESHOLD) {
wrp->ops->enable_timing_output(ppi, 1);
s->delta_ms_prev = s->delta_ms;
......@@ -522,14 +518,14 @@ int wr_servo_update(struct pp_instance *ppi)
/* Can be disabled for manually tweaking and testing */
if(tracking_enabled) {
if (abs(ts_offset_hw.phase) >
if (abs(ts_offset_picos) >
2 * WR_SERVO_OFFSET_STABILITY_THRESHOLD) {
s->state = WR_SYNC_PHASE;
break;
}
// adjust phase towards offset = 0 make ck0 0
s->cur_setpoint += (ts_offset_hw.phase / 4);
s->cur_setpoint += (ts_offset_picos / 4);
wrp->ops->adjust_phase(s->cur_setpoint);
pp_diag(ppi, time, 1, "adjust phase %i\n",
......
......@@ -9,108 +9,50 @@
#include <limits.h>
#include <ppsi/ppsi.h>
void cField_to_TimeInternal(TimeInternal *internal, Integer64 cField)
static void normalize_pp_time(struct pp_time *t)
{
uint64_t i64;
/* no 64b division please, we'll rather loop a few times */
#define SNS_PER_S ((1000LL * 1000 * 1000) << 16)
i64 = cField.lsb;
i64 |= ((int64_t)cField.msb) << 32;
int sign = (t->secs < 0 || (t->secs == 0 && t->scaled_nsecs < 0))
? -1 : 1;
if ((int32_t)cField.msb < 0)
pp_error("BUG: %s doesn't support negatives\n", __func__);
/* turn into positive, to make code shorter (don't replicate loops) */
t->secs *= sign;
t->scaled_nsecs *= sign;
/*
* the correctionField is nanoseconds scaled by 16 bits.
* It is updated by transparent clocks and may be used to count
* for asymmetry. Since we support no better than nanosecond with
* the standard protocol and WR (which is better than nanosecond)
* doesn't use this field, just approximate to nanoseconds.
* and the WR extension uses its own methods for asymmetry,
*/
i64 += 0x8000;
i64 >>= 16;
/* Use __div64_32 from library, to avoid libgcc on small targets */
internal->nanoseconds = __div64_32(&i64, PP_NSEC_PER_SEC);
internal->seconds = i64;
}
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_error("Negative value (%08x.%08x) cannot be converted "
"into timestamp\n",
internal->seconds,
internal->nanoseconds);
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_error("to_TimeInternal: "
"seconds field is higher than signed integer (32bits)\n");
return -1;
while (t->scaled_nsecs < 0) {
t->secs--;
t->scaled_nsecs += SNS_PER_S;
}
}
/* A negative TimeInternal has both secs and nsecs <= 0 */
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;
while (t->scaled_nsecs > SNS_PER_S) {
t->secs++;
t->scaled_nsecs -= SNS_PER_S;
}
t->secs *= sign;
t->scaled_nsecs *= sign;
}
void add_TimeInternal(TimeInternal *r, TimeInternal *x, TimeInternal *y)
void pp_time_add(struct pp_time *t1, struct pp_time *t2)
{
r->seconds = x->seconds + y->seconds;
r->nanoseconds = x->nanoseconds + y->nanoseconds;
normalize_TimeInternal(r);
t1->secs += t2->secs;
t1->scaled_nsecs += t2->scaled_nsecs;
normalize_pp_time(t1);
}
void sub_TimeInternal(TimeInternal *r, TimeInternal *x, TimeInternal *y)
void pp_time_sub(struct pp_time *t1, struct pp_time *t2)
{
r->seconds = x->seconds - y->seconds;
r->nanoseconds = x->nanoseconds - y->nanoseconds;
normalize_TimeInternal(r);
t1->secs -= t2->secs;
t1->scaled_nsecs -= t2->scaled_nsecs;
normalize_pp_time(t1);
}
void div2_TimeInternal(TimeInternal *r)
void pp_time_div2(struct pp_time *t)
{
r->nanoseconds += r->seconds % 2 * PP_NSEC_PER_SEC;
r->seconds /= 2;
r->nanoseconds /= 2;
int sign = (t->secs < 0) ? -1 : 1;
normalize_TimeInternal(r);
t->scaled_nsecs >>= 1;
if (t->secs &1)
t->scaled_nsecs += sign * SNS_PER_S / 2;
t->secs >>= 1;
}
......@@ -24,8 +24,8 @@ void m1(struct pp_instance *ppi)
/* Current data set update */
DSCUR(ppi)->stepsRemoved = 0;
clear_TimeInternal(&DSCUR(ppi)->offsetFromMaster);
clear_TimeInternal(&DSCUR(ppi)->meanPathDelay);
clear_time(&DSCUR(ppi)->offsetFromMaster);
clear_time(&DSCUR(ppi)->meanPathDelay);
/* Parent data set: we are the parent */
memset(parent, 0, sizeof(*parent));
......
......@@ -109,7 +109,8 @@ int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf,
/* t2 may be overriden by follow-up, cField is always valid */
ppi->t2 = ppi->last_rcv_time;
cField_to_TimeInternal(&ppi->cField, hdr->correctionfield);
/* FIXME: merge cField here? */
ppi->cField = hdr->cField;
if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0) {
ppi->flags |= PPI_FLAG_WAITING_FOR_F_UP;
......@@ -118,8 +119,7 @@ int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf,
}
msg_unpack_sync(buf, &sync);
ppi->flags &= ~PPI_FLAG_WAITING_FOR_F_UP;
to_TimeInternal(&ppi->t1,
&sync.originTimestamp);
ppi->t1 = sync.originTimestamp;
if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH)
pp_servo_got_psync(ppi);
else
......@@ -145,9 +145,9 @@ int st_com_peer_handle_pres(struct pp_instance *ppi, unsigned char *buf,
resp.requestingPortIdentity.portNumber) &&
(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) {
to_TimeInternal(&ppi->t4, &resp.requestReceiptTimestamp);
ppi->t4 = resp.requestReceiptTimestamp;
pp_time_add(&ppi->t4, &hdr->cField);
ppi->t6 = ppi->last_rcv_time;
ppi->t6_cf = phase_to_cf_units(ppi->last_rcv_time.phase);
if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0)
ppi->flags |= PPI_FLAG_WAITING_FOR_RF_UP;
else {
......@@ -159,9 +159,6 @@ int st_com_peer_handle_pres(struct pp_instance *ppi, unsigned char *buf,
memset(&ppi->t5, 0, sizeof(ppi->t5));
}
/* Save correctionField of pdelay_resp, see 11.4.3 d 3/4 */
cField_to_TimeInternal(&ppi->cField, hdr->correctionfield);
if (!(hdr->flagField[0] & PP_TWO_STEP_FLAG)) {
if (pp_hooks.handle_presp)
e = pp_hooks.handle_presp(ppi);
......@@ -182,7 +179,6 @@ int st_com_peer_handle_pres_followup(struct pp_instance *ppi,
MsgHeader *hdr = &ppi->received_ptp_header;
MsgPDelayRespFollowUp respFllw;
int e = 0;
TimeInternal tmp;
if (plen < PP_PDELAY_R_FUP_LENGTH)
/* Ignore */
......@@ -199,14 +195,12 @@ int st_com_peer_handle_pres_followup(struct pp_instance *ppi,
respFllw.requestingPortIdentity.portNumber) &&
(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) {
to_TimeInternal(&ppi->t5,
&respFllw.responseOriginTimestamp);
ppi->t5 = respFllw.responseOriginTimestamp;
/*
* Add correctionField of pdelay_resp_followup to
* cf of pdelay_resp (see 11.4.3 d 4)
*/
cField_to_TimeInternal(&tmp, hdr->correctionfield);
add_TimeInternal(&ppi->cField, &ppi->cField, &tmp);
pp_time_add(&ppi->t4, &hdr->cField);
if (pp_hooks.handle_presp)
e = pp_hooks.handle_presp(ppi);
......@@ -242,7 +236,6 @@ int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
{
MsgFollowUp follow;
int ret = 0;
TimeInternal cField;
MsgHeader *hdr = &ppi->received_ptp_header;
......@@ -266,11 +259,10 @@ int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
msg_unpack_follow_up(buf, &follow);
ppi->flags &= ~PPI_FLAG_WAITING_FOR_F_UP;
to_TimeInternal(&ppi->t1, &follow.preciseOriginTimestamp);
ppi->t1 = follow.preciseOriginTimestamp;
/* Add correctionField in follow-up to sync correctionField, see 11.2 */
cField_to_TimeInternal(&cField, hdr->correctionfield);
add_TimeInternal(&ppi->cField, &ppi->cField, &cField);
pp_time_add(&ppi->cField, &hdr->cField); /* FIXME: check this cField */
/* Call the extension; it may do it all and ask to return */
if (pp_hooks.handle_followup)
......@@ -311,10 +303,10 @@ int __send_and_log(struct pp_instance *ppi, int msglen, int chtype)
}
/* FIXME: diagnosticst should be looped back in the send method */
pp_diag(ppi, frames, 1, "SENT %02d bytes at %d.%09d (%s)\n", msglen,
(int)(ppi->last_snt_time.seconds),
(int)(ppi->last_snt_time.nanoseconds),
(int)(ppi->last_snt_time.secs),
(int)(ppi->last_snt_time.scaled_nsecs >> 16),
pp_msgtype_info[msgtype].name);
if (chtype == PP_NP_EVT && ppi->last_snt_time.correct == 0)
if (chtype == PP_NP_EVT && is_incorrect(&ppi->last_snt_time))
return PP_SEND_NO_STAMP;
/* count sent packets */
......
......@@ -42,7 +42,7 @@ int __send_and_log(struct pp_instance *ppi, int msglen, int chtype);
/* Count successfully received PTP packets */
static inline int __recv_and_count(struct pp_instance *ppi, void *pkt, int len,
TimeInternal *t)
struct pp_time *t)
{
int ret;
ret = ppi->n_ops->recv(ppi, pkt, len, t);
......
......@@ -13,7 +13,7 @@
int msg_unpack_header(struct pp_instance *ppi, void *buf, int plen)
{
MsgHeader *hdr = &ppi->received_ptp_header;
uint32_t lsb, msb;
hdr->transportSpecific = (*(Nibble *) (buf + 0)) >> 4;
hdr->messageType = (*(Enumeration4 *) (buf + 0)) & 0x0F;
hdr->versionPTP = (*(UInteger4 *) (buf + 1)) & 0x0F;
......@@ -24,10 +24,10 @@ int msg_unpack_header(struct pp_instance *ppi, void *buf, int plen)
memcpy(hdr->flagField, (buf + 6), PP_FLAG_FIELD_LENGTH);
memcpy(&hdr->correctionfield.msb, (buf + 8), 4);
memcpy(&hdr->correctionfield.lsb, (buf + 12), 4);
hdr->correctionfield.msb = htonl(hdr->correctionfield.msb);
hdr->correctionfield.lsb = htonl(hdr->correctionfield.lsb);
memcpy(&msb, (buf + 8), 4);
memcpy(&lsb, (buf + 12), 4);
hdr->cField.secs = 0LL;
hdr->cField.scaled_nsecs = (uint64_t)msb << 32 | (uint64_t)lsb;
memcpy(&hdr->sourcePortIdentity.clockIdentity, (buf + 20),
PP_CLOCK_IDENTITY_LENGTH);
hdr->sourcePortIdentity.portNumber =
......@@ -100,7 +100,7 @@ static int __msg_pack_header(struct pp_instance *ppi, unsigned msgtype)
}
/* Pack Sync message into out buffer of ppi */
static int msg_pack_sync(struct pp_instance *ppi, Timestamp *orig_tstamp)
static int msg_pack_sync(struct pp_instance *ppi, struct pp_time *orig_tstamp)
{
void *buf = ppi->tx_ptp;
UInteger8 *flags8 = buf + 6;;
......@@ -112,21 +112,24 @@ static int msg_pack_sync(struct pp_instance *ppi, Timestamp *orig_tstamp)
*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_SYNC]);
/* Sync message */
*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secondsField.msb);
*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secondsField.lsb);
*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->nanosecondsField);
*(UInteger16 *)(buf + 34) = htons(orig_tstamp->secs >> 32);
*(UInteger32 *)(buf + 36) = htonl(orig_tstamp->secs);
*(UInteger32 *)(buf + 40) = htonl(orig_tstamp->scaled_nsecs >> 16);
return len;
}
/* Unpack Sync message from in buffer */
void msg_unpack_sync(void *buf, MsgSync *sync)
{
sync->originTimestamp.secondsField.msb =
htons(*(UInteger16 *) (buf + 34));
sync->originTimestamp.secondsField.lsb =
htonl(*(UInteger32 *) (buf + 36));
sync->originTimestamp.nanosecondsField =
htonl(*(UInteger32 *) (buf + 40));
int64_t secs, nsecs;
secs = htons(*(UInteger16 *) (buf + 34));
secs <<= 32;
secs |= htonl(*(UInteger32 *) (buf + 36));
nsecs = htonl(*(UInteger32 *) (buf + 40));
sync->originTimestamp.secs = secs;
sync->originTimestamp.scaled_nsecs = nsecs << 16;
}
/*
......@@ -196,7 +199,7 @@ void msg_unpack_announce(void *buf, MsgAnnounce *ann)
ann->originTimestamp.secondsField.lsb =
htonl(*(UInteger32 *) (buf + 36));
ann->originTimestamp.nanosecondsField =
htonl(*(UInteger32 *) (buf + 40));
htonl(*(UInteger32 *) (buf + 40)) << 16;
ann->currentUtcOffset = htons(*(UInteger16 *) (buf + 44));
ann->grandmasterPriority1 = *(UInteger8 *) (buf + 47);
ann->grandmasterClockQuality.clockClass =
......@@ -216,7 +219,8 @@ void msg_unpack_announce(void *buf, MsgAnnounce *ann)
}
/* Pack Follow Up message into out buffer of ppi*/
static int msg_pack_follow_up(struct pp_instance *ppi, Timestamp *prec_orig_tstamp)
static int msg_pack_follow_up(struct pp_instance *ppi,
struct pp_time *prec_orig_tstamp)
{
void *buf = ppi->tx_ptp;
int len = __msg_pack_header(ppi, PPM_FOLLOW_UP);
......@@ -225,19 +229,16 @@ static int msg_pack_follow_up(struct pp_instance *ppi, Timestamp *prec_orig_tsta
*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_SYNC]);
/* Follow Up message */
*(UInteger16 *) (buf + 34) =
htons(prec_orig_tstamp->secondsField.msb);
*(UInteger32 *) (buf + 36) =
htonl(prec_orig_tstamp->secondsField.lsb);
*(UInteger32 *) (buf + 40) =
htonl(prec_orig_tstamp->nanosecondsField);
*(UInteger16 *)(buf + 34) = htons(prec_orig_tstamp->secs >> 32);
*(UInteger32 *)(buf + 36) = htonl(prec_orig_tstamp->secs);
*(UInteger32 *)(buf + 40) = htonl(prec_orig_tstamp->scaled_nsecs >> 16);
return len;
}
/* Pack PDelay Follow Up message into out buffer of ppi*/
static int msg_pack_pdelay_resp_follow_up(struct pp_instance *ppi,
MsgHeader * hdr,
Timestamp * prec_orig_tstamp)
MsgHeader * hdr,
struct pp_time *prec_orig_tstamp)
{
void *buf = ppi->tx_ptp;
int len = __msg_pack_header(ppi, PPM_PDELAY_R_FUP);
......@@ -245,14 +246,14 @@ static int msg_pack_pdelay_resp_follow_up(struct pp_instance *ppi,
/* Header */
*(UInteger8 *) (buf + 4) = hdr->domainNumber; /* FIXME: why? */
/* copy the correction field, 11.4.3 c.3) */
*(Integer32 *) (buf + 8) = htonl(hdr->correctionfield.msb);
*(Integer32 *) (buf + 12) = htonl(hdr->correctionfield.lsb);
*(Integer32 *) (buf + 8) = htonl(hdr->cField.scaled_nsecs >> 32);
*(Integer32 *) (buf + 12) = htonl((int)hdr->cField.scaled_nsecs);
*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
/* requestReceiptTimestamp */
*(UInteger16 *) (buf + 34) = htons(prec_orig_tstamp->secondsField.msb);
*(UInteger32 *) (buf + 36) = htonl(prec_orig_tstamp->secondsField.lsb);
*(UInteger32 *) (buf + 40) = htonl(prec_orig_tstamp->nanosecondsField);
*(UInteger16 *)(buf + 34) = htons(prec_orig_tstamp->secs >> 32);
*(UInteger32 *)(buf + 36) = htonl(prec_orig_tstamp->secs);
*(UInteger32 *)(buf + 40) = htonl(prec_orig_tstamp->scaled_nsecs >> 16);
/* requestingPortIdentity */
memcpy((buf + 44), &hdr->sourcePortIdentity.clockIdentity,
......@@ -264,24 +265,32 @@ static int msg_pack_pdelay_resp_follow_up(struct pp_instance *ppi,
/* Unpack FollowUp message from in buffer of ppi to internal structure */
void msg_unpack_follow_up(void *buf, MsgFollowUp *flwup)
{
flwup->preciseOriginTimestamp.secondsField.msb =
htons(*(UInteger16 *) (buf + 34));
flwup->preciseOriginTimestamp.secondsField.lsb =
htonl(*(UInteger32 *) (buf + 36));
flwup->preciseOriginTimestamp.nanosecondsField =
htonl(*(UInteger32 *) (buf + 40));
int64_t secs, nsecs;
secs = htons(*(UInteger16 *) (buf + 34));
secs <<= 32;
secs |= htonl(*(UInteger32 *) (buf + 36));
nsecs = htonl(*(UInteger32 *) (buf + 40));
flwup->preciseOriginTimestamp.secs = secs;
flwup->preciseOriginTimestamp.scaled_nsecs = nsecs << 16;
}
/* Unpack PDelayRespFollowUp message from in buffer of ppi to internal struct */
void msg_unpack_pdelay_resp_follow_up(void *buf,
MsgPDelayRespFollowUp * pdelay_resp_flwup)
{
pdelay_resp_flwup->responseOriginTimestamp.secondsField.msb =
htons(*(UInteger16 *) (buf + 34));
pdelay_resp_flwup->responseOriginTimestamp.secondsField.lsb =
htonl(*(UInteger32 *) (buf + 36));
pdelay_resp_flwup->responseOriginTimestamp.nanosecondsField =
htonl(*(UInteger32 *) (buf + 40));
int64_t secs, nsecs;
secs = htons(*(UInteger16 *) (buf + 34));
secs <<= 32;
secs |= htonl(*(UInteger32 *) (buf + 36));
nsecs = htonl(*(UInteger32 *) (buf + 40));
pdelay_resp_flwup->responseOriginTimestamp.secs = secs;
pdelay_resp_flwup->responseOriginTimestamp.scaled_nsecs =
nsecs << 16;
memcpy(&pdelay_resp_flwup->requestingPortIdentity.clockIdentity,
(buf + 44), PP_CLOCK_IDENTITY_LENGTH);
pdelay_resp_flwup->requestingPortIdentity.portNumber =
......@@ -289,7 +298,8 @@ void msg_unpack_pdelay_resp_follow_up(void *buf,
}
/* pack DelayReq message into out buffer of ppi */
static int msg_pack_delay_req(struct pp_instance *ppi, Timestamp *orig_tstamp)
static int msg_pack_delay_req(struct pp_instance *ppi,
struct pp_time *orig_tstamp)
{
void *buf = ppi->tx_ptp;
int len = __msg_pack_header(ppi, PPM_DELAY_REQ);
......@@ -299,15 +309,15 @@ static int msg_pack_delay_req(struct pp_instance *ppi, Timestamp *orig_tstamp)
*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_DELAY_REQ]);
/* Delay_req message */
*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secondsField.msb);
*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secondsField.lsb);
*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->nanosecondsField);
*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secs >> 32);
*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secs);
*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->scaled_nsecs >> 16);
return len;
}
/* pack DelayReq message into out buffer of ppi */
static int msg_pack_pdelay_req(struct pp_instance *ppi,
Timestamp * orig_tstamp)
struct pp_time *orig_tstamp)
{
void *buf = ppi->tx_ptp;
int len = __msg_pack_header(ppi, PPM_PDELAY_REQ);
......@@ -317,16 +327,16 @@ static int msg_pack_pdelay_req(struct pp_instance *ppi,
*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_PDELAY_REQ]);
/* PDelay_req message */
*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secondsField.msb);
*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secondsField.lsb);
*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->nanosecondsField);
*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secs >> 32);
*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secs);
*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->scaled_nsecs >> 16);
memset(buf + 44, 0, 10); /* reserved to match pdelay_resp length */
return len;
}
/* pack PDelayResp message into OUT buffer of ppi */
static int msg_pack_pdelay_resp(struct pp_instance *ppi,
MsgHeader * hdr, Timestamp * rcv_tstamp)
MsgHeader * hdr, struct pp_time *rcv_tstamp)
{
void *buf = ppi->tx_ptp;
UInteger8 *flags8 = buf + 6;;
......@@ -337,9 +347,9 @@ static int msg_pack_pdelay_resp(struct pp_instance *ppi,
*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
/* requestReceiptTimestamp */
*(UInteger16 *) (buf + 34) = htons(rcv_tstamp->secondsField.msb);
*(UInteger32 *) (buf + 36) = htonl(rcv_tstamp->secondsField.lsb);
*(UInteger32 *) (buf + 40) = htonl(rcv_tstamp->nanosecondsField);
*(UInteger16 *) (buf + 34) = htons(rcv_tstamp->secs >> 32);
*(UInteger32 *) (buf + 36) = htonl(rcv_tstamp->secs);
*(UInteger32 *) (buf + 40) = htonl(rcv_tstamp->scaled_nsecs >> 16);
/* requestingPortIdentity */
memcpy((buf + 44), &hdr->sourcePortIdentity.clockIdentity,
......@@ -350,22 +360,22 @@ static int msg_pack_pdelay_resp(struct pp_instance *ppi,
/* pack DelayResp message into OUT buffer of ppi */
static int msg_pack_delay_resp(struct pp_instance *ppi,
MsgHeader *hdr, Timestamp *rcv_tstamp)
MsgHeader *hdr, struct pp_time *rcv_tstamp)
{
void *buf = ppi->tx_ptp;
int len = __msg_pack_header(ppi, PPM_DELAY_RESP);
/* Header */
/* Copy correctionField of delayReqMessage */
*(Integer32 *) (buf + 8) = htonl(hdr->correctionfield.msb);
*(Integer32 *) (buf + 12) = htonl(hdr->correctionfield.lsb);
*(Integer32 *) (buf + 8) = 0;
*(Integer32 *) (buf + 12) = htonl(rcv_tstamp->scaled_nsecs & 0xffff);
*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
/* Delay_resp message */
*(UInteger16 *) (buf + 34) =
htons(rcv_tstamp->secondsField.msb);
*(UInteger32 *) (buf + 36) = htonl(rcv_tstamp->secondsField.lsb);
*(UInteger32 *) (buf + 40) = htonl(rcv_tstamp->nanosecondsField);
*(UInteger16 *)(buf + 34) = htons(rcv_tstamp->secs >> 32);
*(UInteger32 *)(buf + 36) = htonl(rcv_tstamp->secs);
*(UInteger32 *)(buf + 40) = htonl(rcv_tstamp->scaled_nsecs >> 16);
memcpy((buf + 44), &hdr->sourcePortIdentity.clockIdentity,
PP_CLOCK_IDENTITY_LENGTH);
*(UInteger16 *) (buf + 52) =
......@@ -376,34 +386,44 @@ static int msg_pack_delay_resp(struct pp_instance *ppi,
/* Unpack delayReq message from in buffer of ppi to internal structure */
void msg_unpack_delay_req(void *buf, MsgDelayReq *delay_req)
{
delay_req->originTimestamp.secondsField.msb =
htons(*(UInteger16 *) (buf + 34));
delay_req->originTimestamp.secondsField.lsb =
htonl(*(UInteger32 *) (buf + 36));
delay_req->originTimestamp.nanosecondsField =
htonl(*(UInteger32 *) (buf + 40));
int64_t secs, nsecs;
secs = htons(*(UInteger16 *) (buf + 34));
secs <<= 32;
secs |= htonl(*(UInteger32 *) (buf + 36));
nsecs = htonl(*(UInteger32 *) (buf + 40));
delay_req->originTimestamp.secs = secs;
delay_req->originTimestamp.scaled_nsecs = nsecs;
}
/* Unpack PDelayReq message from in buffer of ppi to internal structure */
void msg_unpack_pdelay_req(void *buf, MsgPDelayReq * pdelay_req)
{
pdelay_req->originTimestamp.secondsField.msb =
htons(*(UInteger16 *) (buf + 34));
pdelay_req->originTimestamp.secondsField.lsb =
htonl(*(UInteger32 *) (buf + 36));
pdelay_req->originTimestamp.nanosecondsField =
htonl(*(UInteger32 *) (buf + 40));
int64_t secs, nsecs;
secs = htons(*(UInteger16 *) (buf + 34));
secs <<= 32;
secs |= htonl(*(UInteger32 *) (buf + 36));
nsecs = htonl(*(UInteger32 *) (buf + 40));
pdelay_req->originTimestamp.secs = secs;
pdelay_req->originTimestamp.scaled_nsecs = nsecs << 16;
}
/* Unpack delayResp message from IN buffer of ppi to internal structure */
void msg_unpack_delay_resp(void *buf, MsgDelayResp *resp)
{
resp->receiveTimestamp.secondsField.msb =
htons(*(UInteger16 *) (buf + 34));
resp->receiveTimestamp.secondsField.lsb =
htonl(*(UInteger32 *) (buf + 36));
resp->receiveTimestamp.nanosecondsField =
htonl(*(UInteger32 *) (buf + 40));
int64_t secs, nsecs;
secs = htons(*(UInteger16 *) (buf + 34));
secs <<= 32;
secs |= htonl(*(UInteger32 *) (buf + 36));
nsecs = htonl(*(UInteger32 *) (buf + 40));
resp->receiveTimestamp.secs = secs;
resp->receiveTimestamp.scaled_nsecs = nsecs << 16;
memcpy(&resp->requestingPortIdentity.clockIdentity,
(buf + 44), PP_CLOCK_IDENTITY_LENGTH);
resp->requestingPortIdentity.portNumber =
......@@ -413,12 +433,16 @@ void msg_unpack_delay_resp(void *buf, MsgDelayResp *resp)
/* Unpack PDelayResp message from IN buffer of ppi to internal structure */
void msg_unpack_pdelay_resp(void *buf, MsgPDelayResp * presp)
{
presp->requestReceiptTimestamp.secondsField.msb =
htons(*(UInteger16 *) (buf + 34));
presp->requestReceiptTimestamp.secondsField.lsb =
htonl(*(UInteger32 *) (buf + 36));
presp->requestReceiptTimestamp.nanosecondsField =
htonl(*(UInteger32 *) (buf + 40));
int64_t secs, nsecs;
secs = htons(*(UInteger16 *) (buf + 34));
secs <<= 32;
secs |= htonl(*(UInteger32 *) (buf + 36));
nsecs = htonl(*(UInteger32 *) (buf + 40));
presp->requestReceiptTimestamp.secs = secs;
presp->requestReceiptTimestamp.scaled_nsecs = nsecs << 16;
memcpy(&presp->requestingPortIdentity.clockIdentity,
(buf + 44), PP_CLOCK_IDENTITY_LENGTH);
presp->requestingPortIdentity.portNumber =
......@@ -436,49 +460,37 @@ int msg_issue_announce(struct pp_instance *ppi)
/* Pack and send on event multicast ip adress a Sync message */
int msg_issue_sync_followup(struct pp_instance *ppi)
{
Timestamp tstamp;
TimeInternal now, *time_snt;
struct pp_time now;
int e, len;
/* Send sync on the event channel with the "current" timestamp */
ppi->t_ops->get(ppi, &now);
from_TimeInternal(&now, &tstamp);
len = msg_pack_sync(ppi, &tstamp);
len = msg_pack_sync(ppi, &now);
e = __send_and_log(ppi, len, PP_NP_EVT);
if (e) return e;
/* Send followup on general channel with sent-stamp of sync */
time_snt = &ppi->last_snt_time;
from_TimeInternal(time_snt, &tstamp);
len = msg_pack_follow_up(ppi, &tstamp);
len = msg_pack_follow_up(ppi, &ppi->last_snt_time);
return __send_and_log(ppi, len, PP_NP_GEN);
}
/* Pack and send on general multicast ip address a FollowUp message */
int msg_issue_pdelay_resp_followup(struct pp_instance *ppi, TimeInternal * time)
int msg_issue_pdelay_resp_followup(struct pp_instance *ppi, struct pp_time *t)
{
Timestamp prec_orig_tstamp;
int len;
from_TimeInternal(time, &prec_orig_tstamp);
len = msg_pack_pdelay_resp_follow_up(ppi, &ppi->received_ptp_header,
&prec_orig_tstamp);
len = msg_pack_pdelay_resp_follow_up(ppi, &ppi->received_ptp_header, t);
return __send_and_log(ppi, len, PP_NP_GEN);
}
/* Pack and send on event multicast ip adress a DelayReq message */
static int msg_issue_delay_req(struct pp_instance *ppi)
{
Timestamp orig_tstamp;
TimeInternal now;
struct pp_time now;
int len;
ppi->t_ops->get(ppi, &now);
from_TimeInternal(&now, &orig_tstamp);
len = msg_pack_delay_req(ppi, &orig_tstamp);
len = msg_pack_delay_req(ppi, &now);
return __send_and_log(ppi, len, PP_NP_EVT);
}
......@@ -486,15 +498,11 @@ static int msg_issue_delay_req(struct pp_instance *ppi)
/* Pack and send on event multicast ip adress a PDelayReq message */
static int msg_issue_pdelay_req(struct pp_instance *ppi)
{
Timestamp orig_tstamp;
TimeInternal now;
struct pp_time now;
int len;
ppi->t_ops->get(ppi, &now);
from_TimeInternal(&now, &orig_tstamp);
len = msg_pack_pdelay_req(ppi, &orig_tstamp);
len = msg_pack_pdelay_req(ppi, &now);
return __send_and_log(ppi, len, PP_NP_EVT);
}
......@@ -506,27 +514,19 @@ int msg_issue_request(struct pp_instance *ppi)
}
/* Pack and send on event multicast ip adress a DelayResp message */
int msg_issue_delay_resp(struct pp_instance *ppi, TimeInternal *time)
int msg_issue_delay_resp(struct pp_instance *ppi, struct pp_time *t)
{
Timestamp rcv_tstamp;
int len;
from_TimeInternal(time, &rcv_tstamp);
len = msg_pack_delay_resp(ppi, &ppi->received_ptp_header, &rcv_tstamp);
len = msg_pack_delay_resp(ppi, &ppi->received_ptp_header, t);
return __send_and_log(ppi, len, PP_NP_GEN);
}
/* Pack and send on event multicast ip adress a DelayResp message */
int msg_issue_pdelay_resp(struct pp_instance *ppi, TimeInternal * time)
int msg_issue_pdelay_resp(struct pp_instance *ppi, struct pp_time *t)
{
Timestamp rcv_tstamp;
int len;
from_TimeInternal(time, &rcv_tstamp);
len = msg_pack_pdelay_resp(ppi, &ppi->received_ptp_header, &rcv_tstamp);
len = msg_pack_pdelay_resp(ppi, &ppi->received_ptp_header, t);
return __send_and_log(ppi, len, PP_NP_EVT);
}
......@@ -9,10 +9,10 @@
#include <ppsi/ppsi.h>
static void pp_servo_mpd_fltr(struct pp_instance *, struct pp_avg_fltr *,
TimeInternal *);
static int pp_servo_offset_master(struct pp_instance *, TimeInternal *,
TimeInternal *, TimeInternal *);
static Integer32 pp_servo_pi_controller(struct pp_instance *, TimeInternal *);
struct pp_time *);
static int pp_servo_offset_master(struct pp_instance *, struct pp_time *,
struct pp_time *, struct pp_time *);
static int64_t pp_servo_pi_controller(struct pp_instance *, struct pp_time *);
void pp_servo_init(struct pp_instance *ppi)
......@@ -43,21 +43,22 @@ void pp_servo_init(struct pp_instance *ppi)
}
/* internal helper, returning static storage to be used immediately */
static char *fmt_TI(TimeInternal *t)
static char *fmt_ppt(struct pp_time *t)
{
static char s[24];
pp_sprintf(s, "%s%d.%09d",
(t->seconds < 0 || (t->seconds == 0 && t->nanoseconds < 0))
(t->secs < 0 || (t->secs == 0 && t->scaled_nsecs < 0))
? "-" : " ",
(int)abs(t->seconds), (int)abs(t->nanoseconds));
/* FIXME: this is wrong for some of the negatives */
(int)abs(t->secs), (int)abs(t->scaled_nsecs >> 16));
return s;
}
/* Called by slave and uncalib when we have t1 and t2 */
void pp_servo_got_sync(struct pp_instance *ppi)
{
TimeInternal *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
struct pp_time *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
/*
* calc 'master_to_slave_delay', removing the correction field
......@@ -65,46 +66,48 @@ void pp_servo_got_sync(struct pp_instance *ppi)
* cField contains the sum of sync and followup messages
* correctionField(s)
*/
sub_TimeInternal(m_to_s_dly, &ppi->t2, &ppi->t1);
sub_TimeInternal(m_to_s_dly, m_to_s_dly, &ppi->cField);
*m_to_s_dly = ppi->t2;
pp_time_sub(m_to_s_dly, &ppi->t1);
pp_time_sub(m_to_s_dly, &ppi->cField); /* FIXME: check cField */
pp_diag(ppi, servo, 3, "correction field 1: %s\n",
fmt_TI(&ppi->cField));
fmt_ppt(&ppi->cField));
}
/* Called by slave and uncalib when we have t1 and t2 */
void pp_servo_got_psync(struct pp_instance *ppi)
{
TimeInternal *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
TimeInternal *mpd = &DSCUR(ppi)->meanPathDelay;
TimeInternal *ofm = &DSCUR(ppi)->offsetFromMaster;
Integer32 adj;
struct pp_time *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
struct pp_time *mpd = &DSCUR(ppi)->meanPathDelay;
struct pp_time *ofm = &DSCUR(ppi)->offsetFromMaster;
int adj32;
pp_diag(ppi, servo, 2, "T1: %s\n", fmt_TI(&ppi->t1));
pp_diag(ppi, servo, 2, "T2: %s\n", fmt_TI(&ppi->t2));
pp_diag(ppi, servo, 2, "T1: %s\n", fmt_ppt(&ppi->t1));
pp_diag(ppi, servo, 2, "T2: %s\n", fmt_ppt(&ppi->t2));
/*
* calc 'master_to_slave_delay', removing the correction field
* added by transparent clocks in the path.
*/
sub_TimeInternal(m_to_s_dly, &ppi->t2, &ppi->t1);
sub_TimeInternal(m_to_s_dly, m_to_s_dly, &ppi->cField);
*m_to_s_dly = ppi->t2;
pp_time_sub(m_to_s_dly, &ppi->t1);
pp_time_sub(m_to_s_dly, &ppi->cField); /* FIXME: check cField */
pp_diag(ppi, servo, 3, "correction field 1: %s\n",
fmt_TI(&ppi->cField));
fmt_ppt(&ppi->cField));
/* update 'offsetFromMaster' and possibly jump in time */
if (pp_servo_offset_master(ppi, mpd, ofm, m_to_s_dly))
return;
/* PI controller */
adj = pp_servo_pi_controller(ppi, ofm);
/* PI controller returns a scaled_nsecs adjustment, so shift back */
adj32 = (int)(pp_servo_pi_controller(ppi, ofm) >> 16);
/* apply controller output as a clock tick rate adjustment, if
* provided by arch, or as a raw offset otherwise */
if (pp_can_adjust(ppi)) {
if (ppi->t_ops->adjust_freq)
ppi->t_ops->adjust_freq(ppi, -adj);
ppi->t_ops->adjust_freq(ppi, -adj32);
else
ppi->t_ops->adjust_offset(ppi, -adj);
ppi->t_ops->adjust_offset(ppi, -adj32);
}
pp_diag(ppi, servo, 2, "Observed drift: %9i\n",
......@@ -114,16 +117,16 @@ void pp_servo_got_psync(struct pp_instance *ppi)
/* called by slave states when delay_resp is received (all t1..t4 are valid) */
void pp_servo_got_resp(struct pp_instance *ppi)
{
TimeInternal *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
TimeInternal *s_to_m_dly = &SRV(ppi)->s_to_m_dly;
TimeInternal *mpd = &DSCUR(ppi)->meanPathDelay;
TimeInternal *ofm = &DSCUR(ppi)->offsetFromMaster;
struct pp_time *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
struct pp_time *s_to_m_dly = &SRV(ppi)->s_to_m_dly;
struct pp_time *mpd = &DSCUR(ppi)->meanPathDelay;
struct pp_time *ofm = &DSCUR(ppi)->offsetFromMaster;
struct pp_avg_fltr *mpd_fltr = &SRV(ppi)->mpd_fltr;
Integer32 adj;
int adj32;
/* We sometimes enter here before we got sync/f-up */
if (ppi->t1.seconds == 0 && ppi->t1.nanoseconds == 0) {
if (ppi->t1.secs == 0 && ppi->t1.scaled_nsecs == 0) {
pp_diag(ppi, servo, 2, "discard T3/T4: we miss T1/T2\n");
return;
}
......@@ -131,24 +134,26 @@ void pp_servo_got_resp(struct pp_instance *ppi)
* calc 'slave_to_master_delay', removing delay_resp correction field
* added by transparent clocks in the path.
*/
sub_TimeInternal(s_to_m_dly, &ppi->t4, &ppi->t3);
sub_TimeInternal(s_to_m_dly, s_to_m_dly, &ppi->cField);
*s_to_m_dly = ppi->t4;
pp_time_sub(s_to_m_dly, &ppi->t3);
pp_time_sub(s_to_m_dly, &ppi->cField); /* FIXME: cField check */
pp_diag(ppi, servo, 3, "correction field 2: %s\n",
fmt_TI(&ppi->cField));
fmt_ppt(&ppi->cField));
pp_diag(ppi, servo, 2, "T1: %s\n", fmt_TI(&ppi->t1));
pp_diag(ppi, servo, 2, "T2: %s\n", fmt_TI(&ppi->t2));
pp_diag(ppi, servo, 2, "T3: %s\n", fmt_TI(&ppi->t3));
pp_diag(ppi, servo, 2, "T4: %s\n", fmt_TI(&ppi->t4));
pp_diag(ppi, servo, 1, "Master to slave: %s\n", fmt_TI(m_to_s_dly));
pp_diag(ppi, servo, 1, "Slave to master: %s\n", fmt_TI(s_to_m_dly));
pp_diag(ppi, servo, 2, "T1: %s\n", fmt_ppt(&ppi->t1));
pp_diag(ppi, servo, 2, "T2: %s\n", fmt_ppt(&ppi->t2));
pp_diag(ppi, servo, 2, "T3: %s\n", fmt_ppt(&ppi->t3));
pp_diag(ppi, servo, 2, "T4: %s\n", fmt_ppt(&ppi->t4));
pp_diag(ppi, servo, 1, "Master to slave: %s\n", fmt_ppt(m_to_s_dly));
pp_diag(ppi, servo, 1, "Slave to master: %s\n", fmt_ppt(s_to_m_dly));
/* Calc mean path delay, used later to calc "offset from master" */
add_TimeInternal(mpd, &SRV(ppi)->m_to_s_dly, &SRV(ppi)->s_to_m_dly);
div2_TimeInternal(mpd);
pp_diag(ppi, servo, 1, "meanPathDelay: %s\n", fmt_TI(mpd));
*mpd = SRV(ppi)->m_to_s_dly;
pp_time_add(mpd, &SRV(ppi)->s_to_m_dly);
pp_time_div2(mpd);
pp_diag(ppi, servo, 1, "meanPathDelay: %s\n", fmt_ppt(mpd));
if (mpd->seconds) /* Hmm.... we called this "bad event" */
if (mpd->secs) /* Hmm.... we called this "bad event" */
return;
/* mean path delay filtering */
......@@ -159,15 +164,15 @@ void pp_servo_got_resp(struct pp_instance *ppi)
return;
/* PI controller */
adj = pp_servo_pi_controller(ppi, ofm);
adj32 = (int)(pp_servo_pi_controller(ppi, ofm) >> 16);
/* apply controller output as a clock tick rate adjustment, if
* provided by arch, or as a raw offset otherwise */
if (pp_can_adjust(ppi)) {
if (ppi->t_ops->adjust_freq)
ppi->t_ops->adjust_freq(ppi, -adj);
ppi->t_ops->adjust_freq(ppi, -adj32);
else
ppi->t_ops->adjust_offset(ppi, -adj);
ppi->t_ops->adjust_offset(ppi, -adj32);
}
pp_diag(ppi, servo, 2, "Observed drift: %9i\n",
......@@ -177,35 +182,38 @@ void pp_servo_got_resp(struct pp_instance *ppi)
/* called by slave states when delay_resp is received (all t1..t4 are valid) */
void pp_servo_got_presp(struct pp_instance *ppi)
{
TimeInternal *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
TimeInternal *s_to_m_dly = &SRV(ppi)->s_to_m_dly;
TimeInternal *mpd = &DSCUR(ppi)->meanPathDelay;
struct pp_time *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
struct pp_time *s_to_m_dly = &SRV(ppi)->s_to_m_dly;
struct pp_time *mpd = &DSCUR(ppi)->meanPathDelay;
struct pp_avg_fltr *mpd_fltr = &SRV(ppi)->mpd_fltr;
/*
* calc 'slave_to_master_delay', removing the correction field
* added by transparent clocks in the path.
*/
sub_TimeInternal(s_to_m_dly, &ppi->t6, &ppi->t5);
sub_TimeInternal(s_to_m_dly, s_to_m_dly, &ppi->cField);
*s_to_m_dly = ppi->t6;
pp_time_sub(s_to_m_dly, &ppi->t5);
pp_time_sub(s_to_m_dly, &ppi->cField); /* FIXME: check cField */
pp_diag(ppi, servo, 3, "correction field 2: %s\n",
fmt_TI(&ppi->cField));
fmt_ppt(&ppi->cField));
sub_TimeInternal(m_to_s_dly, &ppi->t4, &ppi->t3);
*m_to_s_dly = ppi->t4;
pp_time_sub(m_to_s_dly, &ppi->t3);
pp_diag(ppi, servo, 2, "T3: %s\n", fmt_TI(&ppi->t3));
pp_diag(ppi, servo, 2, "T4: %s\n", fmt_TI(&ppi->t4));
pp_diag(ppi, servo, 2, "T5: %s\n", fmt_TI(&ppi->t5));
pp_diag(ppi, servo, 2, "T6: %s\n", fmt_TI(&ppi->t6));
pp_diag(ppi, servo, 1, "Master to slave: %s\n", fmt_TI(m_to_s_dly));
pp_diag(ppi, servo, 1, "Slave to master: %s\n", fmt_TI(s_to_m_dly));
pp_diag(ppi, servo, 2, "T3: %s\n", fmt_ppt(&ppi->t3));
pp_diag(ppi, servo, 2, "T4: %s\n", fmt_ppt(&ppi->t4));
pp_diag(ppi, servo, 2, "T5: %s\n", fmt_ppt(&ppi->t5));
pp_diag(ppi, servo, 2, "T6: %s\n", fmt_ppt(&ppi->t6));
pp_diag(ppi, servo, 1, "Master to slave: %s\n", fmt_ppt(m_to_s_dly));
pp_diag(ppi, servo, 1, "Slave to master: %s\n", fmt_ppt(s_to_m_dly));
/* Calc mean path delay, used later to calc "offset from master" */
add_TimeInternal(mpd, &SRV(ppi)->m_to_s_dly, &SRV(ppi)->s_to_m_dly);
div2_TimeInternal(mpd);
pp_diag(ppi, servo, 1, "meanPathDelay: %s\n", fmt_TI(mpd));
*mpd = SRV(ppi)->m_to_s_dly;
pp_time_add(mpd, &SRV(ppi)->s_to_m_dly);
pp_time_div2(mpd);
pp_diag(ppi, servo, 1, "meanPathDelay: %s\n", fmt_ppt(mpd));
if (mpd->seconds) /* Hmm.... we called this "bad event" */
if (mpd->secs) /* Hmm.... we called this "bad event" */
return;
pp_servo_mpd_fltr(ppi, mpd_fltr, mpd);
......@@ -213,22 +221,25 @@ void pp_servo_got_presp(struct pp_instance *ppi)
static
void pp_servo_mpd_fltr(struct pp_instance *ppi, struct pp_avg_fltr *mpd_fltr,
TimeInternal * mpd)
struct pp_time *mpd)
{
int s;
uint64_t y;
if (mpd_fltr->s_exp < 1) {
/* First time, keep what we have */
mpd_fltr->y = mpd->nanoseconds;
mpd_fltr->y = mpd->scaled_nsecs;
if (mpd->scaled_nsecs > 0)
mpd_fltr->y = mpd->scaled_nsecs;
}
/* avoid overflowing filter */
/* avoid overflowing filter: calculate number of bits */
s = OPTS(ppi)->s;
while (abs(mpd_fltr->y) >> (31 - s))
while (mpd_fltr->y >> (63 - s))
--s;
if (mpd_fltr->s_exp > 1 << s)
mpd_fltr->s_exp = 1 << s;
if (mpd_fltr->s_exp > 1LL << s)
mpd_fltr->s_exp = 1LL << s;
/* crank down filter cutoff by increasing 's_exp' */
if (mpd_fltr->s_exp < 1 << s)
if (mpd_fltr->s_exp < 1LL << s)
++mpd_fltr->s_exp;
/*
......@@ -238,10 +249,10 @@ void pp_servo_mpd_fltr(struct pp_instance *ppi, struct pp_avg_fltr *mpd_fltr,
* measured in the master. Ignore such values, by keeping the
* current average instead.
*/
if (mpd->nanoseconds < 0)
mpd->nanoseconds = mpd_fltr->y;
if (mpd->nanoseconds < 0)
mpd->nanoseconds = 0;
if (mpd->scaled_nsecs < 0)
mpd->scaled_nsecs = mpd_fltr->y;
if (mpd->scaled_nsecs < 0)
mpd->scaled_nsecs = 0;
/*
* It may happen that mpd appears to be very big. This happens
......@@ -253,62 +264,61 @@ void pp_servo_mpd_fltr(struct pp_instance *ppi, struct pp_avg_fltr *mpd_fltr,
* The constant multipliers have been chosed arbitrarily, but
* they work well in testing environment.
*/
if (mpd->nanoseconds > 3 * mpd_fltr->y) {
if (mpd->scaled_nsecs > 3 * mpd_fltr->y) {
pp_diag(ppi, servo, 1, "Trim too-long mpd: %i\n",
mpd->nanoseconds);
(int)(mpd->scaled_nsecs >> 16));
/* add fltr->s_exp to ensure we are not trapped into 0 */
mpd->nanoseconds = mpd_fltr->y * 2 + mpd_fltr->s_exp + 1;
mpd->scaled_nsecs = mpd_fltr->y * 2 + mpd_fltr->s_exp + 1;
}
/* filter 'meanPathDelay' (running average) */
mpd_fltr->y = (mpd_fltr->y * (mpd_fltr->s_exp - 1) + mpd->nanoseconds)
/ mpd_fltr->s_exp;
mpd->nanoseconds = mpd_fltr->y;
/* filter 'meanPathDelay' (running average) -- use an unsigned "y" */
y = (mpd_fltr->y * (mpd_fltr->s_exp - 1) + mpd->scaled_nsecs);
__div64_32(&y, mpd_fltr->s_exp);
mpd->scaled_nsecs = mpd_fltr->y = y;
pp_diag(ppi, servo, 1, "After avg(%i), meanPathDelay: %i\n",
(int)mpd_fltr->s_exp, mpd->nanoseconds);
(int)mpd_fltr->s_exp, (int)(mpd->scaled_nsecs >> 16));
}
static
int pp_servo_offset_master(struct pp_instance *ppi, TimeInternal * mpd,
TimeInternal * ofm, TimeInternal * m_to_s_dly)
int pp_servo_offset_master(struct pp_instance *ppi, struct pp_time *mpd,
struct pp_time *ofm, struct pp_time *m_to_s_dly)
{
TimeInternal time_tmp;
sub_TimeInternal(ofm, m_to_s_dly, mpd);
pp_diag(ppi, servo, 1, "Offset from master: %s\n", fmt_TI(ofm));
struct pp_time time_tmp;
*ofm = *m_to_s_dly;
pp_time_sub(ofm, mpd);
pp_diag(ppi, servo, 1, "Offset from master: %s\n", fmt_ppt(ofm));
if (!ofm->seconds)
if (!ofm->secs)
return 0; /* proceeed with adjust */
if (!pp_can_adjust(ppi))
return 0; /* e.g., a loopback test run... "-t" on cmdline */
ppi->t_ops->get(ppi, &time_tmp);
sub_TimeInternal(&time_tmp, &time_tmp, ofm);
pp_time_sub(&time_tmp, ofm);
ppi->t_ops->set(ppi, &time_tmp);
pp_servo_init(ppi);
return 1; /* done */
}
static
Integer32 pp_servo_pi_controller(struct pp_instance * ppi, TimeInternal * ofm)
int64_t pp_servo_pi_controller(struct pp_instance * ppi, struct pp_time *ofm)
{
long long I_term;
long long P_term;
long long tmp;
int I_sign;
int P_sign;
Integer32 adj;
int64_t adj;
/* the accumulator for the I component, shift by 10 to avoid losing bits
* later in the division */
SRV(ppi)->obs_drift += ((long long)ofm->nanoseconds) << 10;
/* the accumulator for the I component */
SRV(ppi)->obs_drift += ofm->scaled_nsecs;
/* Anti-windup. The PP_ADJ_FREQ_MAX value is multiplied by OPTS(ppi)->ai
* (which is the reciprocal of the integral gain of the controller).
* Then it's scaled by 10 bits to match the bit shift used earlier to
* Then it's scaled by 16 bits to match our granularity and
* avoid bit losses */
tmp = (((long long)PP_ADJ_FREQ_MAX) * OPTS(ppi)->ai) << 10;
tmp = (((long long)PP_ADJ_FREQ_MAX) * OPTS(ppi)->ai) << 16;
if (SRV(ppi)->obs_drift > tmp)
SRV(ppi)->obs_drift = tmp;
else if (SRV(ppi)->obs_drift < -tmp)
......@@ -324,9 +334,9 @@ Integer32 pp_servo_pi_controller(struct pp_instance * ppi, TimeInternal * ofm)
I_term = -I_term;
/* calculation of the P component */
P_sign = (ofm->nanoseconds > 0) ? 0 : -1;
/* shift 10 bits again to avoid losses */
P_term = ((long long)ofm->nanoseconds) << 10;
P_sign = (ofm->scaled_nsecs > 0) ? 0 : -1;
/* alrady shifted 16 bits, so we avoid losses */
P_term = ofm->scaled_nsecs;
if (P_sign)
P_term = -P_term;
__div64_32((uint64_t *)&P_term, OPTS(ppi)->ap);
......@@ -334,13 +344,8 @@ Integer32 pp_servo_pi_controller(struct pp_instance * ppi, TimeInternal * ofm)
P_term = -P_term;
/* calculate the correction of applied by the controller */
tmp = P_term + I_term;
/* Now scale it before passing the argument to adjtimex
* Be careful with the signs */
if (tmp > 0)
adj = (tmp >> 10);
else
adj = -((-tmp) >> 10);
adj = P_term + I_term;
/* Return the scaled-nanos values; the caller is scaling back */
return adj;
}
......@@ -51,10 +51,9 @@ static int slave_handle_response(struct pp_instance *ppi, unsigned char *pkt,
return 0;
}
to_TimeInternal(&ppi->t4, &resp.receiveTimestamp);
/* Save delay resp cf in ppi->cField */
cField_to_TimeInternal(&ppi->cField,
hdr->correctionfield);
ppi->t4 = resp.receiveTimestamp;
pp_time_add(&ppi->t4, &hdr->cField);
/* NOTE: older code was adding cField later, in hooks.handle_resp */
if (pp_hooks.handle_resp)
e = pp_hooks.handle_resp(ppi);
......
......@@ -12,7 +12,7 @@
/* FIXME: which socket we receive and send with? */
static int bare_net_recv(struct pp_instance *ppi, void *pkt, int len,
TimeInternal *t)
struct pp_time *t)
{
int ret;
if (t)
......@@ -28,7 +28,7 @@ static int bare_net_send(struct pp_instance *ppi, void *pkt, int len,
int msgtype)
{
struct bare_ethhdr *hdr = pkt;
TimeInternal *t = &ppi->last_snt_time;
struct pp_time *t = &ppi->last_snt_time;
static const uint8_t macaddr[2][ETH_ALEN] = {
[PP_E2E_MECH] = PP_MCAST_MACADDRESS,
[PP_P2P_MECH] = PP_PDELAY_MACADDRESS,
......
......@@ -8,7 +8,7 @@
#include <ppsi/ppsi.h>
#include "bare-linux.h"
static int bare_time_get(struct pp_instance *ppi, TimeInternal *t)
static int bare_time_get(struct pp_instance *ppi, struct pp_time *t)
{
struct bare_timeval tv;
......@@ -17,15 +17,15 @@ static int bare_time_get(struct pp_instance *ppi, TimeInternal *t)
sys_exit(1);
}
/* TAI = UTC + 35 */
t->seconds = tv.tv_sec + DSPRO(ppi)->currentUtcOffset;
t->nanoseconds = tv.tv_usec * 1000;
t->secs = tv.tv_sec + DSPRO(ppi)->currentUtcOffset;
t->scaled_nsecs = (tv.tv_usec * 1000LL) << 16;
if (!(pp_global_d_flags & PP_FLAG_NOTIMELOG))
pp_diag(ppi, time, 2, "%s: %9li.%06li\n", __func__,
tv.tv_sec, tv.tv_usec);
return 0;
}
static int bare_time_set(struct pp_instance *ppi, TimeInternal *t)
static int bare_time_set(struct pp_instance *ppi, const struct pp_time *t)
{
struct bare_timeval tv;
......@@ -43,8 +43,8 @@ static int bare_time_set(struct pp_instance *ppi, TimeInternal *t)
}
/* UTC = TAI - 35 */
tv.tv_sec = t->seconds - DSPRO(ppi)->currentUtcOffset;
tv.tv_usec = t->nanoseconds / 1000;
tv.tv_sec = t->secs - DSPRO(ppi)->currentUtcOffset;
tv.tv_usec = (int)(t->scaled_nsecs >> 16) / 1000;
if (sys_settimeofday(&tv, NULL) < 0) {
pp_error("%s:", __func__);
......
......@@ -73,7 +73,7 @@ static int pending_received(struct sim_ppg_arch_data *data)
}
static int sim_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len,
TimeInternal *t)
struct pp_time *t)
{
ssize_t ret;
struct msghdr msg;
......@@ -119,7 +119,7 @@ static int sim_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len,
ppi->t_ops->get(ppi, t);
/* This is not really hw... */
pp_diag(ppi, time, 2, "recv stamp: %i.%09i (%s)\n",
(int)t->seconds, (int)t->nanoseconds, "user");
(int)t->secs, (int)(t->scaled_nsecs >> 16), "user");
/* If we got a DelayResponse print out the offset from master */
if (((*(Enumeration4 *) (pkt + 0)) & 0x0F) == PPM_DELAY_RESP) {
master_ns = SIM_PPI_ARCH(INST(ppg, SIM_MASTER))->time.current_ns;
......@@ -132,7 +132,7 @@ static int sim_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len,
}
static int sim_net_recv(struct pp_instance *ppi, void *pkt, int len,
TimeInternal *t)
struct pp_time *t)
{
struct sim_ppg_arch_data *data = SIM_PPG_ARCH(ppi->glbs);
struct pp_channel *ch;
......@@ -165,7 +165,7 @@ static int sim_net_send(struct pp_instance *ppi, void *pkt, int len,
int msgtype)
{
int chtype = pp_msgtype_info[msgtype].chtype;
TimeInternal *t = &ppi->last_snt_time;
struct pp_time *t = &ppi->last_snt_time;
struct sim_ppi_arch_data *data = SIM_PPI_ARCH(ppi);
struct sockaddr_in addr;
struct sim_pending_pkt pending;
......
......@@ -46,32 +46,31 @@ int sim_fast_forward_ns(struct pp_globals *ppg, int64_t ff_ns)
return 0;
}
static int sim_time_get(struct pp_instance *ppi, TimeInternal *t)
static int sim_time_get(struct pp_instance *ppi, struct pp_time *t)
{
t->nanoseconds = SIM_PPI_ARCH(ppi)->time.current_ns %
(long long)PP_NSEC_PER_SEC;
t->seconds = (SIM_PPI_ARCH(ppi)->time.current_ns - t->nanoseconds) /
(long long)PP_NSEC_PER_SEC;
t->correct = 1;
t->scaled_nsecs = (SIM_PPI_ARCH(ppi)->time.current_ns %
(long long)PP_NSEC_PER_SEC) << 16;
t->secs = SIM_PPI_ARCH(ppi)->time.current_ns /
(long long)PP_NSEC_PER_SEC;
if (!(pp_global_d_flags & PP_FLAG_NOTIMELOG))
pp_diag(ppi, time, 2, "%s: %9li.%09li\n", __func__,
(long)t->seconds, (long)t->nanoseconds);
(long)t->secs, (long)(t->scaled_nsecs >> 16));
return 0;
}
static int sim_time_set(struct pp_instance *ppi, TimeInternal *t)
static int sim_time_set(struct pp_instance *ppi, const struct pp_time *t)
{
if (!t) {
/* Change the network notion of utc/tai offset */
return 0;
}
SIM_PPI_ARCH(ppi)->time.current_ns = t->nanoseconds
+ t->seconds * (long long)PP_NSEC_PER_SEC;
SIM_PPI_ARCH(ppi)->time.current_ns = (t->scaled_nsecs >> 16)
+ t->secs * (long long)PP_NSEC_PER_SEC;
pp_diag(ppi, time, 1, "%s: %9i.%09i\n", __func__,
t->seconds, t->nanoseconds);
(int)t->secs, (int)(t->scaled_nsecs >> 16));
return 0;
}
......
......@@ -26,7 +26,7 @@
/* unix_recv_msg uses recvmsg for timestamp query */
static int unix_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len,
TimeInternal *t)
struct pp_time *t)
{
struct ethhdr *hdr = pkt;
ssize_t ret;
......@@ -88,9 +88,8 @@ static int unix_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len,
}
if (tv) {
t->seconds = tv->tv_sec + DSPRO(ppi)->currentUtcOffset;
t->nanoseconds = tv->tv_usec * 1000;
t->correct = 1;
t->secs = tv->tv_sec + DSPRO(ppi)->currentUtcOffset;
t->scaled_nsecs = (uint64_t)(tv->tv_usec * 1000) << 16;
} else {
/*
* get the recording time here, even though it may put a big
......@@ -122,13 +121,14 @@ static int unix_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len,
/* This is not really hw... */
pp_diag(ppi, time, 2, "recv stamp: %i.%09i (%s)\n",
(int)t->seconds, (int)t->nanoseconds, tv ? "kernel" : "user");
(int)t->secs, (int)(t->scaled_nsecs >> 16),
tv ? "kernel" : "user");
return ret;
}
/* Receive and send is *not* so trivial */
static int unix_net_recv(struct pp_instance *ppi, void *pkt, int len,
TimeInternal *t)
struct pp_time *t)
{
struct pp_channel *ch1, *ch2;
struct ethhdr *hdr = pkt;
......@@ -184,7 +184,7 @@ static int unix_net_send(struct pp_instance *ppi, void *pkt, int len,
struct ethhdr *hdr = pkt;
struct pp_vlanhdr *vhdr = pkt;
struct pp_channel *ch = ppi->ch + chtype;
TimeInternal *t = &ppi->last_snt_time;
struct pp_time *t = &ppi->last_snt_time;
int is_pdelay = pp_msgtype_info[msgtype].is_pdelay;
static const uint16_t udpport[] = {
[PP_NP_GEN] = PP_GEN_PORT,
......@@ -653,4 +653,3 @@ struct pp_network_operations unix_net_ops = {
.check_packet = unix_net_check_packet,
};
......@@ -23,22 +23,21 @@ static void clock_fatal_error(char *context)
exit(1);
}
static int unix_time_get(struct pp_instance *ppi, TimeInternal *t)
static int unix_time_get(struct pp_instance *ppi, struct pp_time *t)
{
struct timespec tp;
if (clock_gettime(CLOCK_REALTIME, &tp) < 0)
clock_fatal_error("clock_gettime");
/* TAI = UTC + 35 */
t->seconds = tp.tv_sec + DSPRO(ppi)->currentUtcOffset;
t->nanoseconds = tp.tv_nsec;
t->correct = 1;
t->secs = tp.tv_sec + DSPRO(ppi)->currentUtcOffset;
t->scaled_nsecs = ((int64_t)tp.tv_nsec) << 16;
if (!(pp_global_d_flags & PP_FLAG_NOTIMELOG))
pp_diag(ppi, time, 2, "%s: %9li.%09li\n", __func__,
tp.tv_sec, tp.tv_nsec);
return 0;
}
static int unix_time_set(struct pp_instance *ppi, TimeInternal *t)
static int unix_time_set(struct pp_instance *ppi, const struct pp_time *t)
{
struct timespec tp;
......@@ -55,8 +54,8 @@ static int unix_time_set(struct pp_instance *ppi, TimeInternal *t)
}
/* UTC = TAI - 35 */
tp.tv_sec = t->seconds - DSPRO(ppi)->currentUtcOffset;
tp.tv_nsec = t->nanoseconds;
tp.tv_sec = t->secs - DSPRO(ppi)->currentUtcOffset;
tp.tv_nsec = t->scaled_nsecs >> 16;
if (clock_settime(CLOCK_REALTIME, &tp) < 0)
clock_fatal_error("clock_settime");
pp_diag(ppi, time, 1, "%s: %9li.%09li\n", __func__,
......
......@@ -49,7 +49,7 @@ static int wrpc_open_ch(struct pp_instance *ppi)
/* To receive and send packets, we call the minic low-level stuff */
static int wrpc_net_recv(struct pp_instance *ppi, void *pkt, int len,
TimeInternal *t)
struct pp_time *t)
{
int got;
struct wrpc_socket *sock;
......@@ -59,15 +59,11 @@ static int wrpc_net_recv(struct pp_instance *ppi, void *pkt, int len,
got = ptpd_netif_recvfrom(sock, &addr, pkt, len, &wr_ts);
if (t) {
t->seconds = wr_ts.sec;
t->nanoseconds = wr_ts.nsec;
t->phase = wr_ts.phase;
t->correct = wr_ts.correct;
#if 0 /* I disabled the fields, for space: they were only used here */
t->raw_nsec = wr_ts.raw_nsec;
t->raw_phase = wr_ts.raw_phase;
#endif
t->raw_ahead = wr_ts.raw_ahead;
t->secs = wr_ts.sec;
t->scaled_nsecs = (int64_t)wr_ts.nsec << 16;
t->scaled_nsecs += wr_ts.phase * (1 << 16) / 1000;
if (!wr_ts.correct)
mark_incorrect(t);
}
/* wrpc-sw may pass this in USER_CFLAGS, to remove footprint */
......@@ -87,7 +83,7 @@ static int wrpc_net_send(struct pp_instance *ppi, void *pkt, int len,
struct wrpc_socket *sock;
struct wr_timestamp wr_ts;
struct wr_sockaddr addr;
TimeInternal *t = &ppi->last_snt_time;
struct pp_time *t = &ppi->last_snt_time;
int is_pdelay = pp_msgtype_info[msgtype].is_pdelay;
static const uint8_t macaddr[2][ETH_ALEN] = {
[PP_E2E_MECH] = PP_MCAST_MACADDRESS,
......@@ -102,13 +98,14 @@ static int wrpc_net_send(struct pp_instance *ppi, void *pkt, int len,
snt = ptpd_netif_sendto(sock, &addr, pkt, len, &wr_ts);
if (t) {
t->seconds = wr_ts.sec;
t->nanoseconds = wr_ts.nsec;
t->phase = 0;
t->correct = wr_ts.correct;
pp_diag(ppi, frames, 2, "%s: snt=%d, sec=%d, nsec=%d\n",
__func__, snt, t->seconds, t->nanoseconds);
t->secs = wr_ts.sec;
t->scaled_nsecs = (int64_t)wr_ts.nsec << 16;
if (!wr_ts.correct)
mark_incorrect(t);
pp_diag(ppi, frames, 2, "%s: snt=%d, sec=%ld, nsec=%ld\n",
__func__, snt, (long)t->secs,
(long)(t->scaled_nsecs >> 16));
}
/* wrpc-sw may pass this in USER_CFLAGS, to remove footprint */
#ifndef CONFIG_NO_PTPDUMP
......
......@@ -9,22 +9,22 @@
#include "pps_gen.h" /* in wrpc-sw */
#include "syscon.h" /* in wrpc-sw */
static int wrpc_time_get(struct pp_instance *ppi, TimeInternal *t)
static int wrpc_time_get(struct pp_instance *ppi, struct pp_time *t)
{
uint64_t sec;
uint32_t nsec;
shw_pps_gen_get_time(&sec, &nsec);
t->seconds = sec;
t->nanoseconds = nsec;
t->secs = sec;
t->scaled_nsecs = (int64_t)nsec << 16;
if (!(pp_global_d_flags & PP_FLAG_NOTIMELOG))
pp_diag(ppi, time, 2, "%s: %9lu.%09li\n", __func__,
(long)sec, (long)nsec);
return 0;
}
static int wrpc_time_set(struct pp_instance *ppi, TimeInternal *t)
static int wrpc_time_set(struct pp_instance *ppi, const struct pp_time *t)
{
uint64_t sec;
unsigned long nsec;
......@@ -32,8 +32,8 @@ static int wrpc_time_set(struct pp_instance *ppi, TimeInternal *t)
if (!t) /* tai offset changed. We don't have it in wrpc-sw */
return 0;
sec = t->seconds;
nsec = t->nanoseconds;
sec = t->secs;
nsec = t->scaled_nsecs >> 16;
shw_pps_gen_set_time(sec, nsec, PPSG_SET_ALL);
pp_diag(ppi, time, 1, "%s: %9lu.%09li\n", __func__,
......
......@@ -100,6 +100,15 @@ static inline int inside_range(int min, int max, int x)
return (x<=max || x>=min);
}
static char *fmt_time(struct pp_time *t)
{
static char buf[64];
sprintf(buf, "(correct %i) %9li.%09li",
!is_incorrect(t), (long)t->secs,
(long)(t->scaled_nsecs >> 16));
return buf;
}
static void update_dmtd(struct wrs_socket *s, struct pp_instance *ppi)
{
struct hal_port_state *p;
......@@ -116,14 +125,19 @@ static void update_dmtd(struct wrs_socket *s, struct pp_instance *ppi)
}
}
static void wrs_linearize_rx_timestamp(TimeInternal *ts,
/*
* Note: it looks like transition_point is always zero.
* Also, all calculations are ps here, but the timestamp is scaled_ns
* -- ARub 2016-10
*/
static void wrs_linearize_rx_timestamp(struct pp_time *ts,
int32_t dmtd_phase, int cntr_ahead, int transition_point,
int clock_period)
{
int trip_lo, trip_hi;
int phase;
phase = clock_period -1 -dmtd_phase;
phase = clock_period - 1 - dmtd_phase;
/* calculate the range within which falling edge timestamp is stable
* (no possible transitions) */
......@@ -140,25 +154,28 @@ static void wrs_linearize_rx_timestamp(TimeInternal *ts,
* "reliable" one. cntr_ahead will be 1 when the rising edge
* counter is 1 tick ahead of the falling edge counter */
ts->nanoseconds -= cntr_ahead ? (clock_period / 1000) : 0;
if (cntr_ahead)
ts->scaled_nsecs -= (clock_period / 1000LL) << 16;
/* check if the phase is before the counter transition value
* and eventually increase the counter by 1 to simulate a
* timestamp transition exactly at s->phase_transition
* DMTD phase value */
if(inside_range(trip_lo, transition_point, phase))
ts->nanoseconds += clock_period / 1000;
ts->scaled_nsecs += (clock_period / 1000LL) << 16;
}
ts->phase = phase - transition_point - 1;
if(ts->phase < 0) ts->phase += clock_period;
ts->phase = clock_period - 1 -ts->phase;
phase = phase - transition_point - 1;
if (phase < 0)
phase += clock_period;
phase = clock_period - 1 - phase;
ts->scaled_nsecs += (phase << 16) / 1000;
}
static int wrs_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len,
TimeInternal *t)
struct pp_time *t)
{
struct ethhdr *hdr = pkt;
struct wrs_socket *s;
......@@ -198,9 +215,6 @@ static int wrs_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len,
/* FIXME Check ptp-noposix, commit d34f56f: if sender mac check
* is required. Should be added here */
if (t)
t->correct = 0;
for (cmsg = CMSG_FIRSTHDR(&msg);
cmsg;
cmsg = CMSG_NXTHDR(&msg, cmsg)) {
......@@ -219,23 +233,18 @@ static int wrs_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len,
if(sts && t)
{
int cntr_ahead = sts->hwtimeraw.tv_sec & 0x80000000 ? 1: 0;
t->nanoseconds = sts->hwtimeraw.tv_nsec;
t->seconds =
(uint64_t) sts->hwtimeraw.tv_sec & 0x7fffffff;
t->raw_ahead = cntr_ahead;
t->scaled_nsecs = (long long)sts->hwtimeraw.tv_nsec << 16;
t->secs = sts->hwtimeraw.tv_sec & 0x7fffffff;
update_dmtd(s, ppi);
if (!WR_DSPOR(ppi)->wrModeOn) {
/* for non-wr-mode any reported stamp is correct */
t->correct = 1;
goto drop;
}
if (s->dmtd_phase_valid)
{
if (s->dmtd_phase_valid) {
wrs_linearize_rx_timestamp(t, s->dmtd_phase,
cntr_ahead, s->phase_transition, s->clock_period);
t->correct = 1;
} else {
mark_incorrect(t);
}
}
......@@ -278,7 +287,7 @@ out:
}
static int wrs_net_recv(struct pp_instance *ppi, void *pkt, int len,
TimeInternal *t)
struct pp_time *t)
{
struct pp_channel *ch1, *ch2;
struct ethhdr *hdr = pkt;
......@@ -321,15 +330,13 @@ static int wrs_net_recv(struct pp_instance *ppi, void *pkt, int len,
if (ret < 0)
return ret;
pp_diag(ppi, time, 1, "recv stamp: (correct %i) %9li.%09li\n",
t->correct, (long)t->seconds,
(long)t->nanoseconds);
pp_diag(ppi, time, 1, "recv stamp: %s\n", fmt_time(t));
return ret;
}
/* Waits for the transmission timestamp and stores it in t (if not null). */
static void poll_tx_timestamp(struct pp_instance *ppi, void *pkt, int len,
struct wrs_socket *s, int fd, TimeInternal *t)
struct wrs_socket *s, int fd, struct pp_time *t)
{
char data[16384], *dataptr;
struct msghdr msg;
......@@ -357,7 +364,7 @@ static void poll_tx_timestamp(struct pp_instance *ppi, void *pkt, int len,
msg.msg_controllen = sizeof(control);
if (t) /* poison the stamp */
t->seconds = t->correct = 0;
mark_incorrect(t);
pfd.fd = fd;
pfd.events = POLLERR;
......@@ -415,23 +422,21 @@ static void poll_tx_timestamp(struct pp_instance *ppi, void *pkt, int len,
cmsg;
cmsg = CMSG_NXTHDR(&msg, cmsg)) {
void *dp = CMSG_DATA(cmsg);
void *dp = CMSG_DATA(cmsg);
if(cmsg->cmsg_level == SOL_PACKET
&& cmsg->cmsg_type == PACKET_TX_TIMESTAMP)
serr = (struct sock_extended_err *) dp;
if(cmsg->cmsg_level == SOL_PACKET
&& cmsg->cmsg_type == PACKET_TX_TIMESTAMP)
serr = (struct sock_extended_err *) dp;
if(cmsg->cmsg_level == SOL_SOCKET
&& cmsg->cmsg_type == SO_TIMESTAMPING)
sts = (struct scm_timestamping *) dp;
if(cmsg->cmsg_level == SOL_SOCKET
&& cmsg->cmsg_type == SO_TIMESTAMPING)
sts = (struct scm_timestamping *) dp;
if(sts && serr)
{
t->correct = 1;
t->phase = 0;
t->nanoseconds = sts->hwtimeraw.tv_nsec;
t->seconds = (uint64_t) sts->hwtimeraw.tv_sec & 0x7fffffff;
}
if(sts && serr) {
t->scaled_nsecs =
(long long)sts->hwtimeraw.tv_nsec << 16;
t->secs = sts->hwtimeraw.tv_sec & 0x7fffffff;
}
}
}
......@@ -443,7 +448,7 @@ static int wrs_net_send(struct pp_instance *ppi, void *pkt, int len,
struct ethhdr *hdr = pkt;
struct pp_vlanhdr *vhdr = pkt;
struct pp_channel *ch = ppi->ch + chtype;
TimeInternal *t = &ppi->last_snt_time;
struct pp_time *t = &ppi->last_snt_time;
int is_pdelay = pp_msgtype_info[msgtype].is_pdelay;
static uint16_t udpport[] = {
[PP_NP_GEN] = PP_GEN_PORT,
......@@ -492,9 +497,7 @@ static int wrs_net_send(struct pp_instance *ppi, void *pkt, int len,
if (pp_diag_allow(ppi, frames, 2))
dump_1588pkt("send: ", pkt, len, t, -1);
pp_diag(ppi, time, 1, "send stamp: (correct %i) %9li.%09li\n",
t->correct, (long)t->seconds,
(long)t->nanoseconds);
pp_diag(ppi, time, 1, "send stamp: %s\n", fmt_time(t));
return ret;
case PPSI_PROTO_VLAN:
......@@ -527,9 +530,7 @@ static int wrs_net_send(struct pp_instance *ppi, void *pkt, int len,
if (pp_diag_allow(ppi, frames, 2))
dump_1588pkt("send: ", pkt, len, t, ppi->peer_vid);
pp_diag(ppi, time, 1, "send stamp: (correct %i) %9li.%09li\n",
t->correct, (long)t->seconds,
(long)t->nanoseconds);
pp_diag(ppi, time, 1, "send stamp: %s\n", fmt_time(t));
return ret;
case PPSI_PROTO_UDP:
......@@ -553,9 +554,7 @@ static int wrs_net_send(struct pp_instance *ppi, void *pkt, int len,
if (pp_diag_allow(ppi, frames, 2))
dump_payloadpkt("send: ", pkt, len, t);
pp_diag(ppi, time, 1, "send stamp: (correct %i) %9li.%09li\n",
t->correct, (long)t->seconds,
(long)t->nanoseconds);
pp_diag(ppi, time, 1, "send stamp: %s\n", fmt_time(t));
return ret;
default:
......
......@@ -153,10 +153,10 @@ int wrs_locking_poll(struct pp_instance *ppi, int grandmaster)
return WR_SPLL_READY;
}
/* This is a hack: we don't even have taih (struct pp_time come and save me) */
static int wrdate_get(TimeInternal *t)
/* This is a hack, but at least the year is 640bit clean */
static int wrdate_get(struct pp_time *t)
{
unsigned long tail, nsec, tmp;
unsigned long tail, taih, nsec, tmp1, tmp2;
static volatile uint32_t *pps;
int fd;
......@@ -173,18 +173,22 @@ static int wrdate_get(TimeInternal *t)
pps = mapaddr + 0x500; /* pps: 0x10010500 */
}
memset(t, 0, sizeof(*t));
do {
taih = pps[3];
tail = pps[2];
nsec = pps[1] * 16; /* we count a 62.5MHz */
tmp = pps[2];
} while(tmp != tail);
t->seconds = tail;
t->nanoseconds = nsec;
nsec = pps[1] * 16; /* we count at 62.5MHz */
tmp1 = pps[3];
tmp2 = pps[2];
} while((tmp1 != taih) || (tmp2 != tail));
t->secs = tail | ((uint64_t)taih << 32);
t->scaled_nsecs = nsec << 16;
return 0;
}
/* This is only used when the wrs is slave to a non-WR master */
static int wrs_time_get(struct pp_instance *ppi, TimeInternal *t)
static int wrs_time_get(struct pp_instance *ppi, struct pp_time *t)
{
hexp_pps_params_t p;
int cmd;
......@@ -204,9 +208,8 @@ static int wrs_time_get(struct pp_instance *ppi, TimeInternal *t)
/* FIXME Don't know whether p.current_phase_shift is to be assigned
* to t->phase or t->raw_phase. I ignore it, it's not useful here. */
t->seconds = p.current_sec;
t->nanoseconds = p.current_nsec;
t->correct = p.pps_valid;
t->secs = p.current_sec;
t->scaled_nsecs = (long long)p.current_nsec << 16;
if (!(pp_global_d_flags & PP_FLAG_NOTIMELOG))
pp_diag(ppi, time, 2, "%s: (valid %x) %9li.%09li\n", __func__,
......@@ -215,9 +218,9 @@ static int wrs_time_get(struct pp_instance *ppi, TimeInternal *t)
return rval;
}
static int wrs_time_set(struct pp_instance *ppi, TimeInternal *t)
static int wrs_time_set(struct pp_instance *ppi, const struct pp_time *t)
{
TimeInternal diff, now;
struct pp_time diff, now;
struct timex tx;
int tai_offset = 0;
int msec;
......@@ -238,11 +241,12 @@ static int wrs_time_set(struct pp_instance *ppi, TimeInternal *t)
* normal servo drives us). So get time to calc a rough difference.
*/
wrdate_get(&now);
sub_TimeInternal(&diff, t, &now);
diff = *t;
pp_time_sub(&diff, &now);
pp_diag(ppi, time, 1, "%s: (weird) %9li.%09li - delta %9li.%09li\n",
__func__,
(long)t->seconds, (long)t->nanoseconds,
(long)diff.seconds, (long)diff.nanoseconds);
(long)t->secs, (long)(t->scaled_nsecs >> 16),
(long)diff.secs, (long)(diff.scaled_nsecs >> 16));
/*
* We can adjust nanoseconds or seconds, but not both at the
......@@ -251,28 +255,28 @@ static int wrs_time_set(struct pp_instance *ppi, TimeInternal *t)
* servo will call us again later for the seconds part.
* Thus, we fall near, and can then trim frequency (hopefully).
*/
msec = diff.nanoseconds / 1000 / 1000;;
msec = (diff.scaled_nsecs >> 16) / 1000 / 1000;;
#define THRESHOLD_MS 20
if ((msec > THRESHOLD_MS && msec < (1000 - THRESHOLD_MS))
|| (msec < -THRESHOLD_MS && msec > (-1000 + THRESHOLD_MS))) {
pp_diag(ppi, time, 1, "%s: adjusting nanoseconds: %li\n",
__func__, (long)diff.nanoseconds);
diff.seconds = 0;
__func__, (long)(diff.scaled_nsecs >> 16));
diff.secs = 0;
} else {
diff.nanoseconds = 0;
diff.scaled_nsecs = 0;
if (msec > 500)
diff.seconds++;
diff.secs++;
if (msec < -500)
diff.seconds--;
diff.secs--;
pp_diag(ppi, time, 1, "%s: adjusting seconds: %li\n",
__func__, (long)diff.seconds);
__func__, (long)diff.secs);
}
wrs_adjust_counters(diff.seconds, diff.nanoseconds);
wrs_adjust_counters(diff.secs, diff.scaled_nsecs >> 16);
/* If WR time is unrelated to real-world time, we are done. */
if (t->seconds < 1420730822 /* "now" as I write this */)
if (t->secs < 1420730822 /* "now" as I write this */)
return 0;
/*
......@@ -288,8 +292,13 @@ static int wrs_time_set(struct pp_instance *ppi, TimeInternal *t)
*/
tai_offset = *((int *)(&tx.stbcnt) + 1);
}
t->seconds -= tai_offset;
unix_time_ops.set(ppi, t);
{
struct pp_time utc = *t; /* t is "const". uff.... */
utc.secs -= tai_offset;
unix_time_ops.set(ppi, &utc);
}
return 0;
}
......
......@@ -31,25 +31,26 @@ static int dumpstruct(char *p1, char *p2, char *name, void *ptr, int size)
}
#if __STDC_HOSTED__
static void dump_time(char *prefix, struct TimeInternal *ti)
static void dump_time(char *prefix, const struct pp_time *t)
{
struct timeval tv;
struct tm tm;
tv.tv_sec = ti->seconds;
tv.tv_usec = ti->nanoseconds / 1000;
tv.tv_sec = t->secs;
tv.tv_usec = (t->scaled_nsecs >> 16) / 1000;
localtime_r(&tv.tv_sec, &tm);
printf("%sTIME: (%li - 0x%lx) %02i:%02i:%02i.%06li%s\n", prefix,
tv.tv_sec, tv.tv_sec,
tm.tm_hour, tm.tm_min, tm.tm_sec, (long)tv.tv_usec,
!ti->correct ? " invalid" : "");
is_incorrect(t) ? " invalid" : "");
}
#else
static void dump_time(char *prefix, struct TimeInternal *ti)
static void dump_time(char *prefix, const struct pp_time *t)
{
printf("%sTIME: (%li - 0x%lx) %li.%06li%s\n", prefix, (long)ti->seconds,
(long)ti->seconds, (long)ti->seconds, (long)ti->nanoseconds,
!ti->correct ? " invalid" : "");
printf("%sTIME: (%li - 0x%lx) %li.%06li%s\n", prefix,
(long)t->secs, (long)t->secs, (long)t->secs,
(long)(t->scaled_nsecs >> 16) / 1000,
is_incorrect(t) ? " invalid" : "");
}
#endif
......@@ -259,7 +260,7 @@ out:
}
/* This dumps a complete udp frame, starting from the eth header */
int dump_udppkt(char *prefix, void *buf, int len, struct TimeInternal *ti,
int dump_udppkt(char *prefix, void *buf, int len, const struct pp_time *t,
int vlan)
{
struct ethhdr *eth = buf;
......@@ -267,8 +268,8 @@ int dump_udppkt(char *prefix, void *buf, int len, struct TimeInternal *ti,
struct udphdr *udp;
void *payload;
if (ti)
dump_time(prefix, ti);
if (t)
dump_time(prefix, t);
dump_vlan(prefix, vlan);
......@@ -285,23 +286,23 @@ int dump_udppkt(char *prefix, void *buf, int len, struct TimeInternal *ti,
}
/* This dumps the payload only, used for udp frames without headers */
int dump_payloadpkt(char *prefix, void *buf, int len, struct TimeInternal *ti)
int dump_payloadpkt(char *prefix, void *buf, int len, const struct pp_time *t)
{
if (ti)
dump_time(prefix, ti);
if (t)
dump_time(prefix, t);
dump_payload(prefix, buf, len);
return 0;
}
/* This dumps everything, used for raw frames with headers and ptp payload */
int dump_1588pkt(char *prefix, void *buf, int len, struct TimeInternal *ti,
int dump_1588pkt(char *prefix, void *buf, int len, const struct pp_time *t,
int vlan)
{
struct ethhdr *eth = buf;
void *payload;
if (ti)
dump_time(prefix, ti);
if (t)
dump_time(prefix, t);
dump_vlan(prefix, vlan);
payload = buf + dump_eth(prefix, eth);
dump_payload(prefix, payload, len - (payload - buf));
......
......@@ -33,16 +33,16 @@
#define ETH_P_1588 0x88F7
#endif
void print_spaces(struct TimeInternal *ti)
void print_spaces(struct pp_time *ti)
{
static struct TimeInternal prev_ti;
static struct pp_time prev_ti;
int i, diffms;
if (prev_ti.seconds) {
if (prev_ti.secs) {
diffms = (ti->seconds - prev_ti.seconds) * 1000
+ (ti->nanoseconds / 1000 / 1000)
- (prev_ti.nanoseconds / 1000 / 1000);
diffms = (ti->secs - prev_ti.secs) * 1000
+ ((ti->scaled_nsecs >> 16) / 1000 / 1000)
- ((prev_ti.scaled_nsecs) / 1000 / 1000);
/* empty lines, one every .25 seconds, at most 10 of them */
for (i = 250; i < 2500 && i < diffms; i += 250)
printf("\n");
......@@ -117,7 +117,7 @@ int main(int argc, char **argv)
struct pp_vlanhdr *vhdr;
struct iphdr *ip;
unsigned char buf[1500];
struct TimeInternal ti;
struct pp_time ti;
struct timeval tv;
struct msghdr msg;
struct iovec entry;
......@@ -146,8 +146,8 @@ int main(int argc, char **argv)
/* Get the receive time, copy it to TimeInternal */
gettimeofday(&tv, NULL);
ti.seconds = tv.tv_sec;
ti.nanoseconds = tv.tv_usec * 1000;
ti.secs = tv.tv_sec;
ti.scaled_nsecs = (tv.tv_usec * 1000LL) << 16;
if (len > sizeof(buf))
len = sizeof(buf);
......
......@@ -14,10 +14,11 @@
#define printf pp_printf
#endif
int dump_udppkt(char *prefix, void *buf, int len, struct TimeInternal *ti,
int vlan);
int dump_payloadpkt(char *prefix, void *buf, int len, struct TimeInternal *ti);
int dump_1588pkt(char *prefix, void *buf, int len, struct TimeInternal *ti,
int dump_udppkt(char *prefix, void *buf, int len,
const struct pp_time *t, int vlan);
int dump_payloadpkt(char *prefix, void *buf, int len,
const struct pp_time *t);
int dump_1588pkt(char *prefix, void *buf, int len, const struct pp_time *t,
int vlan);
#endif /* __PTPDUMP_H__ */
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