Skip to content

P
PPSi
Commit 35eef679 authored by Alessandro Rubini's avatar Alessandro Rubini
Browse files
Options

time: introduce a new unified data structure 

Commit only build for arch-unix (and no tools either)
This simplifies a lot the calculations.

  - all times are now living in a single structure
  - all fields are signed and 64-bit long
  - the 2038 bug is over (we still had 32-bit seconds)
  - no overflow in math or unexplained "unsigned" appearing
Signed-off-by: Alessandro Rubini's avatarAlessandro Rubini <rubini@gnudd.com>
parent 828f7350
Hide whitespace changes
Inline Side-by-side
Showing with 404 additions and 453 deletions
fsm.c
View file @ 35eef679
...@@ -16,7 +16,7 @@ unsigned long pp_global_d_flags; /* This is the only "global" file in ppsi */ ...@@ -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, ...) static void pp_fsm_printf(struct pp_instance *ppi, char *fmt, ...)
{ {
va_list args; va_list args;
TimeInternal t; struct pp_time t;
unsigned long oflags = pp_global_d_flags; unsigned long oflags = pp_global_d_flags;
if (!pp_diag_allow(ppi, fsm, 1)) if (!pp_diag_allow(ppi, fsm, 1))
...@@ -28,7 +28,7 @@ static void pp_fsm_printf(struct pp_instance *ppi, char *fmt, ...) ...@@ -28,7 +28,7 @@ static void pp_fsm_printf(struct pp_instance *ppi, char *fmt, ...)
pp_global_d_flags = oflags; pp_global_d_flags = oflags;
pp_printf("diag-fsm-1-%s: %09d.%03d: ", ppi->port_name, 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); va_start(args, fmt);
pp_vprintf(fmt, args); pp_vprintf(fmt, args);
va_end(args); va_end(args);
...@@ -203,10 +203,10 @@ int pp_state_machine(struct pp_instance *ppi, uint8_t *packet, int plen) ...@@ -203,10 +203,10 @@ int pp_state_machine(struct pp_instance *ppi, uint8_t *packet, int plen)
if (plen) { if (plen) {
msgtype = packet[0] & 0xf; msgtype = packet[0] & 0xf;
pp_diag(ppi, frames, 1, pp_diag(ppi, frames, 1,
"RECV %02d bytes at %d.%09d (type %x, %s)\n", plen, "RECV %02d bytes at %9d.%09d (type %x, %s)\n", plen,
(int)ppi->last_rcv_time.seconds, (int)ppi->last_rcv_time.secs,
(int)ppi->last_rcv_time.nanoseconds, msgtype, (int)(ppi->last_rcv_time.scaled_nsecs >> 16),
pp_msgtype_info[msgtype].name); msgtype, pp_msgtype_info[msgtype].name);
} }
/* /*
......
include/ppsi/ieee1588_types.h
View file @ 35eef679
...@@ -11,6 +11,7 @@ ...@@ -11,6 +11,7 @@
#define __PPSI_IEEE_1588_TYPES_H__ #define __PPSI_IEEE_1588_TYPES_H__
#include <stdint.h> #include <stdint.h>
#include <ppsi/pp-time.h>
/* See F.2, pag.223 */ /* See F.2, pag.223 */
#define PP_ETHERTYPE 0x88f7 #define PP_ETHERTYPE 0x88f7
...@@ -61,30 +62,6 @@ typedef struct Timestamp { /* page 13 (33) -- no typedef expected */ ...@@ -61,30 +62,6 @@ typedef struct Timestamp { /* page 13 (33) -- no typedef expected */
UInteger32 nanosecondsField; UInteger32 nanosecondsField;
} Timestamp; } 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) */ typedef struct ClockIdentity { /* page 13 (33) */
Octet id[8]; Octet id[8];
} ClockIdentity; } ClockIdentity;
...@@ -136,7 +113,7 @@ typedef struct MsgHeader { ...@@ -136,7 +113,7 @@ typedef struct MsgHeader {
UInteger16 messageLength; UInteger16 messageLength;
UInteger8 domainNumber; UInteger8 domainNumber;
Octet flagField[2]; Octet flagField[2];
Integer64 correctionfield; struct pp_time cField;;
PortIdentity sourcePortIdentity; PortIdentity sourcePortIdentity;
UInteger16 sequenceId; UInteger16 sequenceId;
UInteger8 controlField; UInteger8 controlField;
...@@ -158,40 +135,40 @@ typedef struct MsgAnnounce { ...@@ -158,40 +135,40 @@ typedef struct MsgAnnounce {
/* Sync Message (table 26, page 129) */ /* Sync Message (table 26, page 129) */
typedef struct MsgSync { typedef struct MsgSync {
Timestamp originTimestamp; struct pp_time originTimestamp;
} MsgSync; } MsgSync;
/* DelayReq Message (table 26, page 129) */ /* DelayReq Message (table 26, page 129) */
typedef struct MsgDelayReq { typedef struct MsgDelayReq {
Timestamp originTimestamp; struct pp_time originTimestamp;
} MsgDelayReq; } MsgDelayReq;
/* DelayResp Message (table 27, page 130) */ /* DelayResp Message (table 27, page 130) */
typedef struct MsgFollowUp { typedef struct MsgFollowUp {
Timestamp preciseOriginTimestamp; struct pp_time preciseOriginTimestamp;
} MsgFollowUp; } MsgFollowUp;
/* DelayResp Message (table 28, page 130) */ /* DelayResp Message (table 28, page 130) */
typedef struct MsgDelayResp { typedef struct MsgDelayResp {
Timestamp receiveTimestamp; struct pp_time receiveTimestamp;
PortIdentity requestingPortIdentity; PortIdentity requestingPortIdentity;
} MsgDelayResp; } MsgDelayResp;
/* PdelayReq Message (table 29, page 131) */ /* PdelayReq Message (table 29, page 131) */
typedef struct MsgPDelayReq { typedef struct MsgPDelayReq {
Timestamp originTimestamp; struct pp_time originTimestamp;
} MsgPDelayReq; } MsgPDelayReq;
/* PdelayResp Message (table 30, page 131) */ /* PdelayResp Message (table 30, page 131) */
typedef struct MsgPDelayResp { typedef struct MsgPDelayResp {
Timestamp requestReceiptTimestamp; struct pp_time requestReceiptTimestamp;
PortIdentity requestingPortIdentity; PortIdentity requestingPortIdentity;
} MsgPDelayResp; } MsgPDelayResp;
/* PdelayRespFollowUp Message (table 31, page 132) */ /* PdelayRespFollowUp Message (table 31, page 132) */
typedef struct MsgPDelayRespFollowUp { typedef struct MsgPDelayRespFollowUp {
Timestamp responseOriginTimestamp; struct pp_time responseOriginTimestamp;
PortIdentity requestingPortIdentity; PortIdentity requestingPortIdentity;
} MsgPDelayRespFollowUp; } MsgPDelayRespFollowUp;
...@@ -229,8 +206,8 @@ typedef struct DSDefault { /* page 65 */ ...@@ -229,8 +206,8 @@ typedef struct DSDefault { /* page 65 */
typedef struct DSCurrent { /* page 67 */ typedef struct DSCurrent { /* page 67 */
/* Dynamic */ /* Dynamic */
UInteger16 stepsRemoved; UInteger16 stepsRemoved;
TimeInternal offsetFromMaster; struct pp_time offsetFromMaster;
TimeInternal meanPathDelay; /* oneWayDelay */ struct pp_time meanPathDelay; /* oneWayDelay */
/* White Rabbit extension begin */ /* White Rabbit extension begin */
UInteger16 primarySlavePortNumber; UInteger16 primarySlavePortNumber;
/* White Rabbit extension end */ /* White Rabbit extension end */
...@@ -256,7 +233,6 @@ typedef struct DSPort { /* page 72 */ ...@@ -256,7 +233,6 @@ typedef struct DSPort { /* page 72 */
/* Dynamic */ /* Dynamic */
/* Enumeration8 portState; -- not used */ /* Enumeration8 portState; -- not used */
Integer8 logMinDelayReqInterval; /* -- same as pdelay one */ Integer8 logMinDelayReqInterval; /* -- same as pdelay one */
/* TimeInternal peerMeanPathDelay; -- not used */
/* Configurable */ /* Configurable */
Integer8 logAnnounceInterval; Integer8 logAnnounceInterval;
UInteger8 announceReceiptTimeout; UInteger8 announceReceiptTimeout;
......
include/ppsi/pp-instance.h
View file @ 35eef679
...@@ -82,14 +82,14 @@ struct pp_frgn_master { ...@@ -82,14 +82,14 @@ struct pp_frgn_master {
* where increasing the stiffness (s) lowers the cutoff and increases the delay. * where increasing the stiffness (s) lowers the cutoff and increases the delay.
*/ */
struct pp_avg_fltr { struct pp_avg_fltr {
Integer32 m; /* magnitude */ int64_t m; /* magnitude */
Integer32 y; int64_t y;
Integer32 s_exp; int64_t s_exp;
}; };
struct pp_servo { struct pp_servo {
TimeInternal m_to_s_dly; struct pp_time m_to_s_dly;
TimeInternal s_to_m_dly; struct pp_time s_to_m_dly;
long long obs_drift; long long obs_drift;
struct pp_avg_fltr mpd_fltr; struct pp_avg_fltr mpd_fltr;
}; };
...@@ -149,10 +149,10 @@ struct pp_instance { ...@@ -149,10 +149,10 @@ struct pp_instance {
uint16_t peer_vid; /* Our peer's VID (for PROTO_VLAN) */ uint16_t peer_vid; /* Our peer's VID (for PROTO_VLAN) */
/* Times, for the various offset computations */ /* 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 */ Integer32 t4_cf, t6_cf; /* peer delay */
TimeInternal cField; /* transp. clocks */ struct pp_time cField; /* transp. clocks */
TimeInternal last_rcv_time, last_snt_time; /* two temporaries */ struct pp_time last_rcv_time, last_snt_time; /* two temporaries */
/* Page 85: each port shall maintain an implementation-specific /* Page 85: each port shall maintain an implementation-specific
* foreignMasterDS data set for the purposes of qualifying Announce * foreignMasterDS data set for the purposes of qualifying Announce
......
include/ppsi/pp-time.h 0 → 100644
View file @ 35eef679
#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__ */
include/ppsi/ppsi.h
View file @ 35eef679
...@@ -22,6 +22,7 @@ ...@@ -22,6 +22,7 @@
#include <arch/arch.h> /* ntohs and so on -- and wr-api.h for wr archs */ #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 */ /* At this point in time, we need ARRAY_SIZE to conditionally build vlan code */
#undef ARRAY_SIZE #undef ARRAY_SIZE
#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0])) #define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
...@@ -154,8 +155,8 @@ struct pp_ext_hooks { ...@@ -154,8 +155,8 @@ struct pp_ext_hooks {
void (*s1)(struct pp_instance *ppi, MsgHeader *hdr, MsgAnnounce *ann); void (*s1)(struct pp_instance *ppi, MsgHeader *hdr, MsgAnnounce *ann);
int (*execute_slave)(struct pp_instance *ppi); int (*execute_slave)(struct pp_instance *ppi);
int (*handle_announce)(struct pp_instance *ppi); int (*handle_announce)(struct pp_instance *ppi);
int (*handle_followup)(struct pp_instance *ppi, TimeInternal *orig, int (*handle_followup)(struct pp_instance *ppi, struct pp_time *orig,
TimeInternal *correction_field); struct pp_time *correction_field);
int (*handle_preq) (struct pp_instance * ppi); int (*handle_preq) (struct pp_instance * ppi);
int (*handle_presp) (struct pp_instance * ppi); int (*handle_presp) (struct pp_instance * ppi);
int (*pack_announce)(struct pp_instance *ppi); int (*pack_announce)(struct pp_instance *ppi);
...@@ -173,7 +174,7 @@ struct pp_network_operations { ...@@ -173,7 +174,7 @@ struct pp_network_operations {
int (*init)(struct pp_instance *ppi); int (*init)(struct pp_instance *ppi);
int (*exit)(struct pp_instance *ppi); int (*exit)(struct pp_instance *ppi);
int (*recv)(struct pp_instance *ppi, void *pkt, int len, 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 (*send)(struct pp_instance *ppi, void *pkt, int len, int msgtype);
int (*check_packet)(struct pp_globals *ppg, int delay_ms); int (*check_packet)(struct pp_globals *ppg, int delay_ms);
}; };
...@@ -192,8 +193,8 @@ extern struct pp_network_operations unix_net_ops; ...@@ -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. * If "set" receives a NULL time value, it should update the TAI offset.
*/ */
struct pp_time_operations { struct pp_time_operations {
int (*get)(struct pp_instance *ppi, TimeInternal *t); int (*get)(struct pp_instance *ppi, struct pp_time *t);
int (*set)(struct pp_instance *ppi, TimeInternal *t); int (*set)(struct pp_instance *ppi, const struct pp_time *t);
/* freq_ppb is parts per billion */ /* freq_ppb is parts per billion */
int (*adjust)(struct pp_instance *ppi, long offset_ns, long freq_ppb); int (*adjust)(struct pp_instance *ppi, long offset_ns, long freq_ppb);
int (*adjust_offset)(struct pp_instance *ppi, long offset_ns); 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!! */ ...@@ -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_announce(struct pp_instance *ppi);
extern int msg_issue_sync_followup(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_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, extern int msg_issue_pdelay_resp_followup(struct pp_instance *ppi,
TimeInternal * time); struct pp_time *time);
extern int msg_issue_pdelay_resp(struct pp_instance *ppi, TimeInternal * time); extern int msg_issue_pdelay_resp(struct pp_instance *ppi, struct pp_time *time);
/* Functions for timestamp handling (internal to protocol format conversion*/ /* Functions for time math */
/* FIXME: add prefix in function name? */ extern void pp_time_add(struct pp_time *t1, struct pp_time *t2);
extern void cField_to_TimeInternal(TimeInternal *internal, Integer64 bigint); extern void pp_time_sub(struct pp_time *t1, struct pp_time *t2);
extern int from_TimeInternal(TimeInternal *internal, Timestamp *external); extern void pp_time_div2(struct pp_time *t);
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);
/* /*
* The state machine itself is an array of these structures. * The state machine itself is an array of these structures.
......
proto-ext-whiterabbit/hooks.c
View file @ 35eef679
...@@ -257,8 +257,8 @@ static __attribute__((used)) int wr_handle_presp(struct pp_instance *ppi) ...@@ -257,8 +257,8 @@ static __attribute__((used)) int wr_handle_presp(struct pp_instance *ppi)
return 0; return 0;
} }
ppi->t4_cf = hdr->correctionfield.lsb; /* FIXME: verify that last-received cField is already accounted for */
wr_servo_got_delay(ppi, ppi->t4_cf); wr_servo_got_delay(ppi);
return 0; return 0;
} }
......
proto-ext-whiterabbit/wr-api.h
View file @ 35eef679
...@@ -133,8 +133,8 @@ enum { ...@@ -133,8 +133,8 @@ enum {
int wr_servo_init(struct pp_instance *ppi); int wr_servo_init(struct pp_instance *ppi);
void wr_servo_reset(struct pp_instance *ppi); void wr_servo_reset(struct pp_instance *ppi);
void wr_servo_enable_tracking(int enable); void wr_servo_enable_tracking(int enable);
int wr_servo_got_sync(struct pp_instance *ppi, TimeInternal *t1, int wr_servo_got_sync(struct pp_instance *ppi, struct pp_time *t1,
TimeInternal *t2); struct pp_time *t2);
int wr_servo_got_delay(struct pp_instance *ppi, Integer32 cf); int wr_servo_got_delay(struct pp_instance *ppi, Integer32 cf);
int wr_servo_update(struct pp_instance *ppi); int wr_servo_update(struct pp_instance *ppi);
...@@ -156,12 +156,12 @@ struct wr_servo_state { ...@@ -156,12 +156,12 @@ struct wr_servo_state {
int32_t clock_period_ps; int32_t clock_period_ps;
/* These fields are used by servo code, across iterations */ /* 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; int64_t delta_ms_prev;
int missed_iters; int missed_iters;
/* Following fields are for monitoring/diagnostics (use w/ shmem) */ /* Following fields are for monitoring/diagnostics (use w/ shmem) */
TimeInternal mu; struct pp_time mu;
int64_t picos_mu; int64_t picos_mu;
int32_t cur_setpoint; int32_t cur_setpoint;
int64_t delta_ms; int64_t delta_ms;
...@@ -176,14 +176,14 @@ struct wr_servo_state { ...@@ -176,14 +176,14 @@ struct wr_servo_state {
uint32_t n_err_state; uint32_t n_err_state;
uint32_t n_err_offset; uint32_t n_err_offset;
uint32_t n_err_delta_rtt; 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_p2p_delay(struct pp_instance *ppi, struct wr_servo_state *s);
int wr_e2e_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_hw);
int wr_p2p_offset(struct pp_instance *ppi, 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 */ /* All data used as extension ppsi-wr must be put here */
......
proto-ext-whiterabbit/wr-servo.c
View file @ 35eef679
...@@ -234,7 +234,7 @@ int wr_servo_got_sync(struct pp_instance *ppi, TimeInternal *t1, ...@@ -234,7 +234,7 @@ int wr_servo_got_sync(struct pp_instance *ppi, TimeInternal *t1,
return 0; 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_servo_state *s =
&((struct wr_data *)ppi->ext_data)->servo_state; &((struct wr_data *)ppi->ext_data)->servo_state;
...@@ -244,15 +244,13 @@ int wr_servo_got_delay(struct pp_instance *ppi, Integer32 cf) ...@@ -244,15 +244,13 @@ int wr_servo_got_delay(struct pp_instance *ppi, Integer32 cf)
s->t3 = ppi->t3; s->t3 = ppi->t3;
/* s->t3.phase = 0; */ /* s->t3.phase = 0; */
s->t4 = ppi->t4; s->t4 = ppi->t4;
s->t4.correct = 1; /* clock->delay_req_receive_time.correct; */ /* FIXME: verify that cField is already merged */
s->t4.phase = (int64_t) cf * 1000LL / 65536LL;
if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH) { if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH) {
s->t5 = ppi->t5; s->t5 = ppi->t5;
s->t5.correct = 1; s->t5.correct = 1;
s->t5.phase = 0; s->t5.phase = 0;
s->t6 = ppi->t6; s->t6 = ppi->t6;
s->t6.phase = (int64_t) ppi->t6_cf * 1000LL / 65536LL;
wr_p2p_delay(ppi, s); wr_p2p_delay(ppi, s);
} }
......
proto-standard/arith.c
View file @ 35eef679
...@@ -9,108 +9,68 @@ ...@@ -9,108 +9,68 @@
#include <limits.h> #include <limits.h>
#include <ppsi/ppsi.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; /* 0 is special: it can have both negative and positive fractions */
if (t->secs == 0LL) {
if ((int32_t)cField.msb < 0) while (t->scaled_nsecs > SNS_PER_S) {
pp_error("BUG: %s doesn't support negatives\n", __func__); t->secs++;
t->scaled_nsecs -= SNS_PER_S;
/* }
* the correctionField is nanoseconds scaled by 16 bits. while (t->scaled_nsecs < -SNS_PER_S) {
* It is updated by transparent clocks and may be used to count t->secs--;
* for asymmetry. Since we support no better than nanosecond with t->scaled_nsecs += SNS_PER_S;
* the standard protocol and WR (which is better than nanosecond) }
* doesn't use this field, just approximate to nanoseconds. return;
* 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) if (t->secs > 0LL) {
{ while (t->scaled_nsecs < 0) {
/* Program will not run after 2038... */ t->secs--;
if (external->secondsField.lsb < INT_MAX) { t->scaled_nsecs += SNS_PER_S;
internal->seconds = external->secondsField.lsb; }
internal->nanoseconds = external->nanosecondsField; while (t->scaled_nsecs > SNS_PER_S) {
return 0; t->secs++;
} else { t->scaled_nsecs -= SNS_PER_S;
pp_error("to_TimeInternal: " }
"seconds field is higher than signed integer (32bits)\n"); return;
return -1;
} }
}
/* 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) { /* A negative pp_time has both secs and scaled_nsecs <= 0 */
r->seconds -= 1; while (t->scaled_nsecs < -SNS_PER_S) {
r->nanoseconds += PP_NSEC_PER_SEC; t->secs--;
} else if (r->seconds < 0 && r->nanoseconds > 0) { t->scaled_nsecs += SNS_PER_S;
r->seconds += 1; }
r->nanoseconds -= PP_NSEC_PER_SEC; while (t->scaled_nsecs > 0) {
t->secs++;
t->scaled_nsecs -= SNS_PER_S;
} }
} }
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; t1->secs += t2->secs;
r->nanoseconds = x->nanoseconds + y->nanoseconds; t1->scaled_nsecs += t2->scaled_nsecs;
normalize_pp_time(t1);
normalize_TimeInternal(r);
} }
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; t1->secs -= t2->secs;
r->nanoseconds = x->nanoseconds - y->nanoseconds; t1->scaled_nsecs -= t2->scaled_nsecs;
normalize_pp_time(t1);
normalize_TimeInternal(r);
} }
void div2_TimeInternal(TimeInternal *r) void pp_time_div2(struct pp_time *t)
{ {
r->nanoseconds += r->seconds % 2 * PP_NSEC_PER_SEC; int sign = (t->secs < 0) ? -1 : 1;
r->seconds /= 2;
r->nanoseconds /= 2;
normalize_TimeInternal(r); t->scaled_nsecs >>= 1;
if (t->secs &1)
t->scaled_nsecs += sign * SNS_PER_S / 2;
t->secs >>= 1;
} }
proto-standard/bmc.c
View file @ 35eef679
...@@ -24,8 +24,8 @@ void m1(struct pp_instance *ppi) ...@@ -24,8 +24,8 @@ void m1(struct pp_instance *ppi)
/* Current data set update */ /* Current data set update */
DSCUR(ppi)->stepsRemoved = 0; DSCUR(ppi)->stepsRemoved = 0;
clear_TimeInternal(&DSCUR(ppi)->offsetFromMaster); clear_time(&DSCUR(ppi)->offsetFromMaster);
clear_TimeInternal(&DSCUR(ppi)->meanPathDelay); clear_time(&DSCUR(ppi)->meanPathDelay);
/* Parent data set: we are the parent */ /* Parent data set: we are the parent */
memset(parent, 0, sizeof(*parent)); memset(parent, 0, sizeof(*parent));
......
proto-standard/common-fun.c
View file @ 35eef679
...@@ -109,7 +109,8 @@ int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf, ...@@ -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 */ /* t2 may be overriden by follow-up, cField is always valid */
ppi->t2 = ppi->last_rcv_time; 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) { if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0) {
ppi->flags |= PPI_FLAG_WAITING_FOR_F_UP; 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, ...@@ -118,8 +119,7 @@ int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf,
} }
msg_unpack_sync(buf, &sync); msg_unpack_sync(buf, &sync);
ppi->flags &= ~PPI_FLAG_WAITING_FOR_F_UP; ppi->flags &= ~PPI_FLAG_WAITING_FOR_F_UP;
to_TimeInternal(&ppi->t1, ppi->t1 = sync.originTimestamp;
&sync.originTimestamp);
if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH) if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH)
pp_servo_got_psync(ppi); pp_servo_got_psync(ppi);
else else
...@@ -145,9 +145,9 @@ int st_com_peer_handle_pres(struct pp_instance *ppi, unsigned char *buf, ...@@ -145,9 +145,9 @@ int st_com_peer_handle_pres(struct pp_instance *ppi, unsigned char *buf,
resp.requestingPortIdentity.portNumber) && resp.requestingPortIdentity.portNumber) &&
(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) { (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 = 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) if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0)
ppi->flags |= PPI_FLAG_WAITING_FOR_RF_UP; ppi->flags |= PPI_FLAG_WAITING_FOR_RF_UP;
else { else {
...@@ -159,9 +159,6 @@ int st_com_peer_handle_pres(struct pp_instance *ppi, unsigned char *buf, ...@@ -159,9 +159,6 @@ int st_com_peer_handle_pres(struct pp_instance *ppi, unsigned char *buf,
memset(&ppi->t5, 0, sizeof(ppi->t5)); 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 (!(hdr->flagField[0] & PP_TWO_STEP_FLAG)) {
if (pp_hooks.handle_presp) if (pp_hooks.handle_presp)
e = pp_hooks.handle_presp(ppi); e = pp_hooks.handle_presp(ppi);
...@@ -182,7 +179,6 @@ int st_com_peer_handle_pres_followup(struct pp_instance *ppi, ...@@ -182,7 +179,6 @@ int st_com_peer_handle_pres_followup(struct pp_instance *ppi,
MsgHeader *hdr = &ppi->received_ptp_header; MsgHeader *hdr = &ppi->received_ptp_header;
MsgPDelayRespFollowUp respFllw; MsgPDelayRespFollowUp respFllw;
int e = 0; int e = 0;
TimeInternal tmp;
if (plen < PP_PDELAY_R_FUP_LENGTH) if (plen < PP_PDELAY_R_FUP_LENGTH)
/* Ignore */ /* Ignore */
...@@ -199,14 +195,12 @@ int st_com_peer_handle_pres_followup(struct pp_instance *ppi, ...@@ -199,14 +195,12 @@ int st_com_peer_handle_pres_followup(struct pp_instance *ppi,
respFllw.requestingPortIdentity.portNumber) && respFllw.requestingPortIdentity.portNumber) &&
(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) { (ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) {
to_TimeInternal(&ppi->t5, ppi->t5 = respFllw.responseOriginTimestamp;
&respFllw.responseOriginTimestamp);
/* /*
* Add correctionField of pdelay_resp_followup to * Add correctionField of pdelay_resp_followup to
* cf of pdelay_resp (see 11.4.3 d 4) * cf of pdelay_resp (see 11.4.3 d 4)
*/ */
cField_to_TimeInternal(&tmp, hdr->correctionfield); pp_time_add(&ppi->t4, &hdr->cField);
add_TimeInternal(&ppi->cField, &ppi->cField, &tmp);
if (pp_hooks.handle_presp) if (pp_hooks.handle_presp)
e = pp_hooks.handle_presp(ppi); e = pp_hooks.handle_presp(ppi);
...@@ -242,7 +236,6 @@ int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf, ...@@ -242,7 +236,6 @@ int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
{ {
MsgFollowUp follow; MsgFollowUp follow;
int ret = 0; int ret = 0;
TimeInternal cField;
MsgHeader *hdr = &ppi->received_ptp_header; MsgHeader *hdr = &ppi->received_ptp_header;
...@@ -266,11 +259,10 @@ int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf, ...@@ -266,11 +259,10 @@ int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
msg_unpack_follow_up(buf, &follow); msg_unpack_follow_up(buf, &follow);
ppi->flags &= ~PPI_FLAG_WAITING_FOR_F_UP; 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 */ /* Add correctionField in follow-up to sync correctionField, see 11.2 */
cField_to_TimeInternal(&cField, hdr->correctionfield); pp_time_add(&ppi->cField, &hdr->cField); /* FIXME: check this cField */
add_TimeInternal(&ppi->cField, &ppi->cField, &cField);
/* Call the extension; it may do it all and ask to return */ /* Call the extension; it may do it all and ask to return */
if (pp_hooks.handle_followup) if (pp_hooks.handle_followup)
...@@ -311,10 +303,10 @@ int __send_and_log(struct pp_instance *ppi, int msglen, int chtype) ...@@ -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 */ /* FIXME: diagnosticst should be looped back in the send method */
pp_diag(ppi, frames, 1, "SENT %02d bytes at %d.%09d (%s)\n", msglen, 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.secs),
(int)(ppi->last_snt_time.nanoseconds), (int)(ppi->last_snt_time.scaled_nsecs >> 16),
pp_msgtype_info[msgtype].name); 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; return PP_SEND_NO_STAMP;
/* count sent packets */ /* count sent packets */
......
proto-standard/common-fun.h
View file @ 35eef679
...@@ -42,7 +42,7 @@ int __send_and_log(struct pp_instance *ppi, int msglen, int chtype); ...@@ -42,7 +42,7 @@ int __send_and_log(struct pp_instance *ppi, int msglen, int chtype);
/* Count successfully received PTP packets */ /* Count successfully received PTP packets */
static inline int __recv_and_count(struct pp_instance *ppi, void *pkt, int len, static inline int __recv_and_count(struct pp_instance *ppi, void *pkt, int len,
TimeInternal *t) struct pp_time *t)
{ {
int ret; int ret;
ret = ppi->n_ops->recv(ppi, pkt, len, t); ret = ppi->n_ops->recv(ppi, pkt, len, t);
......
proto-standard/msg.c
View file @ 35eef679
...@@ -13,7 +13,7 @@ ...@@ -13,7 +13,7 @@
int msg_unpack_header(struct pp_instance *ppi, void *buf, int plen) int msg_unpack_header(struct pp_instance *ppi, void *buf, int plen)
{ {
MsgHeader *hdr = &ppi->received_ptp_header; MsgHeader *hdr = &ppi->received_ptp_header;
uint32_t lsb, msb;
hdr->transportSpecific = (*(Nibble *) (buf + 0)) >> 4; hdr->transportSpecific = (*(Nibble *) (buf + 0)) >> 4;
hdr->messageType = (*(Enumeration4 *) (buf + 0)) & 0x0F; hdr->messageType = (*(Enumeration4 *) (buf + 0)) & 0x0F;
hdr->versionPTP = (*(UInteger4 *) (buf + 1)) & 0x0F; hdr->versionPTP = (*(UInteger4 *) (buf + 1)) & 0x0F;
...@@ -24,10 +24,10 @@ int msg_unpack_header(struct pp_instance *ppi, void *buf, int plen) ...@@ -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->flagField, (buf + 6), PP_FLAG_FIELD_LENGTH);
memcpy(&hdr->correctionfield.msb, (buf + 8), 4); memcpy(&msb, (buf + 8), 4);
memcpy(&hdr->correctionfield.lsb, (buf + 12), 4); memcpy(&lsb, (buf + 12), 4);
hdr->correctionfield.msb = htonl(hdr->correctionfield.msb); hdr->cField.secs = 0LL;
hdr->correctionfield.lsb = htonl(hdr->correctionfield.lsb); hdr->cField.scaled_nsecs = (uint64_t)msb << 32 | (uint64_t)lsb;
memcpy(&hdr->sourcePortIdentity.clockIdentity, (buf + 20), memcpy(&hdr->sourcePortIdentity.clockIdentity, (buf + 20),
PP_CLOCK_IDENTITY_LENGTH); PP_CLOCK_IDENTITY_LENGTH);
hdr->sourcePortIdentity.portNumber = hdr->sourcePortIdentity.portNumber =
...@@ -100,7 +100,7 @@ static int __msg_pack_header(struct pp_instance *ppi, unsigned msgtype) ...@@ -100,7 +100,7 @@ static int __msg_pack_header(struct pp_instance *ppi, unsigned msgtype)
} }
/* Pack Sync message into out buffer of ppi */ /* 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; void *buf = ppi->tx_ptp;
UInteger8 *flags8 = buf + 6;; UInteger8 *flags8 = buf + 6;;
...@@ -112,21 +112,24 @@ static int msg_pack_sync(struct pp_instance *ppi, Timestamp *orig_tstamp) ...@@ -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]); *(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_SYNC]);
/* Sync message */ /* Sync message */
*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secondsField.msb); *(UInteger16 *)(buf + 34) = htons(orig_tstamp->secs >> 32);
*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secondsField.lsb); *(UInteger32 *)(buf + 36) = htonl(orig_tstamp->secs);
*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->nanosecondsField); *(UInteger32 *)(buf + 40) = htonl(orig_tstamp->scaled_nsecs >> 16);
return len; return len;
} }
/* Unpack Sync message from in buffer */ /* Unpack Sync message from in buffer */
void msg_unpack_sync(void *buf, MsgSync *sync) void msg_unpack_sync(void *buf, MsgSync *sync)
{ {
sync->originTimestamp.secondsField.msb = int64_t secs, nsecs;
htons(*(UInteger16 *) (buf + 34));
sync->originTimestamp.secondsField.lsb = secs = htons(*(UInteger16 *) (buf + 34));
htonl(*(UInteger32 *) (buf + 36)); secs <<= 32;
sync->originTimestamp.nanosecondsField = secs |= htonl(*(UInteger32 *) (buf + 36));
htonl(*(UInteger32 *) (buf + 40)); 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) ...@@ -196,7 +199,7 @@ void msg_unpack_announce(void *buf, MsgAnnounce *ann)
ann->originTimestamp.secondsField.lsb = ann->originTimestamp.secondsField.lsb =
htonl(*(UInteger32 *) (buf + 36)); htonl(*(UInteger32 *) (buf + 36));
ann->originTimestamp.nanosecondsField = ann->originTimestamp.nanosecondsField =
htonl(*(UInteger32 *) (buf + 40)); htonl(*(UInteger32 *) (buf + 40)) << 16;
ann->currentUtcOffset = htons(*(UInteger16 *) (buf + 44)); ann->currentUtcOffset = htons(*(UInteger16 *) (buf + 44));
ann->grandmasterPriority1 = *(UInteger8 *) (buf + 47); ann->grandmasterPriority1 = *(UInteger8 *) (buf + 47);
ann->grandmasterClockQuality.clockClass = ann->grandmasterClockQuality.clockClass =
...@@ -216,7 +219,8 @@ void msg_unpack_announce(void *buf, MsgAnnounce *ann) ...@@ -216,7 +219,8 @@ void msg_unpack_announce(void *buf, MsgAnnounce *ann)
} }
/* Pack Follow Up message into out buffer of ppi*/ /* 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; void *buf = ppi->tx_ptp;
int len = __msg_pack_header(ppi, PPM_FOLLOW_UP); 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 ...@@ -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]); *(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_SYNC]);
/* Follow Up message */ /* Follow Up message */
*(UInteger16 *) (buf + 34) = *(UInteger16 *)(buf + 34) = htons(prec_orig_tstamp->secs >> 32);
htons(prec_orig_tstamp->secondsField.msb); *(UInteger32 *)(buf + 36) = htonl(prec_orig_tstamp->secs);
*(UInteger32 *) (buf + 36) = *(UInteger32 *)(buf + 40) = htonl(prec_orig_tstamp->scaled_nsecs >> 16);
htonl(prec_orig_tstamp->secondsField.lsb);
*(UInteger32 *) (buf + 40) =
htonl(prec_orig_tstamp->nanosecondsField);
return len; return len;
} }
/* Pack PDelay Follow Up message into out buffer of ppi*/ /* Pack PDelay Follow Up message into out buffer of ppi*/
static int msg_pack_pdelay_resp_follow_up(struct pp_instance *ppi, static int msg_pack_pdelay_resp_follow_up(struct pp_instance *ppi,
MsgHeader * hdr, MsgHeader * hdr,
Timestamp * prec_orig_tstamp) struct pp_time *prec_orig_tstamp)
{ {
void *buf = ppi->tx_ptp; void *buf = ppi->tx_ptp;
int len = __msg_pack_header(ppi, PPM_PDELAY_R_FUP); 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, ...@@ -245,14 +246,14 @@ static int msg_pack_pdelay_resp_follow_up(struct pp_instance *ppi,
/* Header */ /* Header */
*(UInteger8 *) (buf + 4) = hdr->domainNumber; /* FIXME: why? */ *(UInteger8 *) (buf + 4) = hdr->domainNumber; /* FIXME: why? */
/* copy the correction field, 11.4.3 c.3) */ /* copy the correction field, 11.4.3 c.3) */
*(Integer32 *) (buf + 8) = htonl(hdr->correctionfield.msb); *(Integer32 *) (buf + 8) = htonl(hdr->cField.scaled_nsecs >> 32);
*(Integer32 *) (buf + 12) = htonl(hdr->correctionfield.lsb); *(Integer32 *) (buf + 12) = htonl((int)hdr->cField.scaled_nsecs);
*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId); *(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
/* requestReceiptTimestamp */ /* requestReceiptTimestamp */
*(UInteger16 *) (buf + 34) = htons(prec_orig_tstamp->secondsField.msb); *(UInteger16 *)(buf + 34) = htons(prec_orig_tstamp->secs >> 32);
*(UInteger32 *) (buf + 36) = htonl(prec_orig_tstamp->secondsField.lsb); *(UInteger32 *)(buf + 36) = htonl(prec_orig_tstamp->secs);
*(UInteger32 *) (buf + 40) = htonl(prec_orig_tstamp->nanosecondsField); *(UInteger32 *)(buf + 40) = htonl(prec_orig_tstamp->scaled_nsecs >> 16);
/* requestingPortIdentity */ /* requestingPortIdentity */
memcpy((buf + 44), &hdr->sourcePortIdentity.clockIdentity, memcpy((buf + 44), &hdr->sourcePortIdentity.clockIdentity,
...@@ -264,24 +265,32 @@ static int msg_pack_pdelay_resp_follow_up(struct pp_instance *ppi, ...@@ -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 */ /* Unpack FollowUp message from in buffer of ppi to internal structure */
void msg_unpack_follow_up(void *buf, MsgFollowUp *flwup) void msg_unpack_follow_up(void *buf, MsgFollowUp *flwup)
{ {
flwup->preciseOriginTimestamp.secondsField.msb = int64_t secs, nsecs;
htons(*(UInteger16 *) (buf + 34));
flwup->preciseOriginTimestamp.secondsField.lsb = secs = htons(*(UInteger16 *) (buf + 34));
htonl(*(UInteger32 *) (buf + 36)); secs <<= 32;
flwup->preciseOriginTimestamp.nanosecondsField = secs |= htonl(*(UInteger32 *) (buf + 36));
htonl(*(UInteger32 *) (buf + 40)); 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 */ /* Unpack PDelayRespFollowUp message from in buffer of ppi to internal struct */
void msg_unpack_pdelay_resp_follow_up(void *buf, void msg_unpack_pdelay_resp_follow_up(void *buf,
MsgPDelayRespFollowUp * pdelay_resp_flwup) MsgPDelayRespFollowUp * pdelay_resp_flwup)
{ {
pdelay_resp_flwup->responseOriginTimestamp.secondsField.msb = int64_t secs, nsecs;
htons(*(UInteger16 *) (buf + 34));
pdelay_resp_flwup->responseOriginTimestamp.secondsField.lsb = secs = htons(*(UInteger16 *) (buf + 34));
htonl(*(UInteger32 *) (buf + 36)); secs <<= 32;
pdelay_resp_flwup->responseOriginTimestamp.nanosecondsField = secs |= htonl(*(UInteger32 *) (buf + 36));
htonl(*(UInteger32 *) (buf + 40)); 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, memcpy(&pdelay_resp_flwup->requestingPortIdentity.clockIdentity,
(buf + 44), PP_CLOCK_IDENTITY_LENGTH); (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
pdelay_resp_flwup->requestingPortIdentity.portNumber = pdelay_resp_flwup->requestingPortIdentity.portNumber =
...@@ -289,7 +298,8 @@ void msg_unpack_pdelay_resp_follow_up(void *buf, ...@@ -289,7 +298,8 @@ void msg_unpack_pdelay_resp_follow_up(void *buf,
} }
/* pack DelayReq message into out buffer of ppi */ /* 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; void *buf = ppi->tx_ptp;
int len = __msg_pack_header(ppi, PPM_DELAY_REQ); 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) ...@@ -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]); *(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_DELAY_REQ]);
/* Delay_req message */ /* Delay_req message */
*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secondsField.msb); *(UInteger16 *) (buf + 34) = htons(orig_tstamp->secs >> 32);
*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secondsField.lsb); *(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secs);
*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->nanosecondsField); *(UInteger32 *) (buf + 40) = htonl(orig_tstamp->scaled_nsecs >> 16);
return len; return len;
} }
/* pack DelayReq message into out buffer of ppi */ /* pack DelayReq message into out buffer of ppi */
static int msg_pack_pdelay_req(struct pp_instance *ppi, static int msg_pack_pdelay_req(struct pp_instance *ppi,
Timestamp * orig_tstamp) struct pp_time *orig_tstamp)
{ {
void *buf = ppi->tx_ptp; void *buf = ppi->tx_ptp;
int len = __msg_pack_header(ppi, PPM_PDELAY_REQ); int len = __msg_pack_header(ppi, PPM_PDELAY_REQ);
...@@ -317,16 +327,16 @@ static int msg_pack_pdelay_req(struct pp_instance *ppi, ...@@ -317,16 +327,16 @@ static int msg_pack_pdelay_req(struct pp_instance *ppi,
*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_PDELAY_REQ]); *(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_PDELAY_REQ]);
/* PDelay_req message */ /* PDelay_req message */
*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secondsField.msb); *(UInteger16 *) (buf + 34) = htons(orig_tstamp->secs >> 32);
*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secondsField.lsb); *(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secs);
*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->nanosecondsField); *(UInteger32 *) (buf + 40) = htonl(orig_tstamp->scaled_nsecs >> 16);
memset(buf + 44, 0, 10); /* reserved to match pdelay_resp length */ memset(buf + 44, 0, 10); /* reserved to match pdelay_resp length */
return len; return len;
} }
/* pack PDelayResp message into OUT buffer of ppi */ /* pack PDelayResp message into OUT buffer of ppi */
static int msg_pack_pdelay_resp(struct pp_instance *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; void *buf = ppi->tx_ptp;
UInteger8 *flags8 = buf + 6;; UInteger8 *flags8 = buf + 6;;
...@@ -337,9 +347,9 @@ static int msg_pack_pdelay_resp(struct pp_instance *ppi, ...@@ -337,9 +347,9 @@ static int msg_pack_pdelay_resp(struct pp_instance *ppi,
*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId); *(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
/* requestReceiptTimestamp */ /* requestReceiptTimestamp */
*(UInteger16 *) (buf + 34) = htons(rcv_tstamp->secondsField.msb); *(UInteger16 *) (buf + 34) = htons(rcv_tstamp->secs >> 32);
*(UInteger32 *) (buf + 36) = htonl(rcv_tstamp->secondsField.lsb); *(UInteger32 *) (buf + 36) = htonl(rcv_tstamp->secs);
*(UInteger32 *) (buf + 40) = htonl(rcv_tstamp->nanosecondsField); *(UInteger32 *) (buf + 40) = htonl(rcv_tstamp->scaled_nsecs >> 16);
/* requestingPortIdentity */ /* requestingPortIdentity */
memcpy((buf + 44), &hdr->sourcePortIdentity.clockIdentity, memcpy((buf + 44), &hdr->sourcePortIdentity.clockIdentity,
...@@ -350,22 +360,22 @@ static int msg_pack_pdelay_resp(struct pp_instance *ppi, ...@@ -350,22 +360,22 @@ static int msg_pack_pdelay_resp(struct pp_instance *ppi,
/* pack DelayResp message into OUT buffer of ppi */ /* pack DelayResp message into OUT buffer of ppi */
static int msg_pack_delay_resp(struct pp_instance *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; void *buf = ppi->tx_ptp;
int len = __msg_pack_header(ppi, PPM_DELAY_RESP); int len = __msg_pack_header(ppi, PPM_DELAY_RESP);
/* Header */ /* Header */
/* Copy correctionField of delayReqMessage */ /* Copy correctionField of delayReqMessage */
*(Integer32 *) (buf + 8) = htonl(hdr->correctionfield.msb); *(Integer32 *) (buf + 8) = 0;
*(Integer32 *) (buf + 12) = htonl(hdr->correctionfield.lsb); *(Integer32 *) (buf + 12) = htonl(rcv_tstamp->scaled_nsecs & 0xffff);
*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId); *(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
/* Delay_resp message */ /* Delay_resp message */
*(UInteger16 *) (buf + 34) = *(UInteger16 *)(buf + 34) = htons(rcv_tstamp->secs >> 32);
htons(rcv_tstamp->secondsField.msb); *(UInteger32 *)(buf + 36) = htonl(rcv_tstamp->secs);
*(UInteger32 *) (buf + 36) = htonl(rcv_tstamp->secondsField.lsb); *(UInteger32 *)(buf + 40) = htonl(rcv_tstamp->scaled_nsecs >> 16);
*(UInteger32 *) (buf + 40) = htonl(rcv_tstamp->nanosecondsField);
memcpy((buf + 44), &hdr->sourcePortIdentity.clockIdentity, memcpy((buf + 44), &hdr->sourcePortIdentity.clockIdentity,
PP_CLOCK_IDENTITY_LENGTH); PP_CLOCK_IDENTITY_LENGTH);
*(UInteger16 *) (buf + 52) = *(UInteger16 *) (buf + 52) =
...@@ -376,34 +386,44 @@ static int msg_pack_delay_resp(struct pp_instance *ppi, ...@@ -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 */ /* Unpack delayReq message from in buffer of ppi to internal structure */
void msg_unpack_delay_req(void *buf, MsgDelayReq *delay_req) void msg_unpack_delay_req(void *buf, MsgDelayReq *delay_req)
{ {
delay_req->originTimestamp.secondsField.msb = int64_t secs, nsecs;
htons(*(UInteger16 *) (buf + 34));
delay_req->originTimestamp.secondsField.lsb = secs = htons(*(UInteger16 *) (buf + 34));
htonl(*(UInteger32 *) (buf + 36)); secs <<= 32;
delay_req->originTimestamp.nanosecondsField = secs |= htonl(*(UInteger32 *) (buf + 36));
htonl(*(UInteger32 *) (buf + 40)); 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 */ /* Unpack PDelayReq message from in buffer of ppi to internal structure */
void msg_unpack_pdelay_req(void *buf, MsgPDelayReq * pdelay_req) void msg_unpack_pdelay_req(void *buf, MsgPDelayReq * pdelay_req)
{ {
pdelay_req->originTimestamp.secondsField.msb = int64_t secs, nsecs;
htons(*(UInteger16 *) (buf + 34));
pdelay_req->originTimestamp.secondsField.lsb = secs = htons(*(UInteger16 *) (buf + 34));
htonl(*(UInteger32 *) (buf + 36)); secs <<= 32;
pdelay_req->originTimestamp.nanosecondsField = secs |= htonl(*(UInteger32 *) (buf + 36));
htonl(*(UInteger32 *) (buf + 40)); 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 */ /* Unpack delayResp message from IN buffer of ppi to internal structure */
void msg_unpack_delay_resp(void *buf, MsgDelayResp *resp) void msg_unpack_delay_resp(void *buf, MsgDelayResp *resp)
{ {
resp->receiveTimestamp.secondsField.msb = int64_t secs, nsecs;
htons(*(UInteger16 *) (buf + 34));
resp->receiveTimestamp.secondsField.lsb = secs = htons(*(UInteger16 *) (buf + 34));
htonl(*(UInteger32 *) (buf + 36)); secs <<= 32;
resp->receiveTimestamp.nanosecondsField = secs |= htonl(*(UInteger32 *) (buf + 36));
htonl(*(UInteger32 *) (buf + 40)); nsecs = htonl(*(UInteger32 *) (buf + 40));
resp->receiveTimestamp.secs = secs;
resp->receiveTimestamp.scaled_nsecs = nsecs << 16;
memcpy(&resp->requestingPortIdentity.clockIdentity, memcpy(&resp->requestingPortIdentity.clockIdentity,
(buf + 44), PP_CLOCK_IDENTITY_LENGTH); (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
resp->requestingPortIdentity.portNumber = resp->requestingPortIdentity.portNumber =
...@@ -413,12 +433,16 @@ void msg_unpack_delay_resp(void *buf, MsgDelayResp *resp) ...@@ -413,12 +433,16 @@ void msg_unpack_delay_resp(void *buf, MsgDelayResp *resp)
/* Unpack PDelayResp message from IN buffer of ppi to internal structure */ /* Unpack PDelayResp message from IN buffer of ppi to internal structure */
void msg_unpack_pdelay_resp(void *buf, MsgPDelayResp * presp) void msg_unpack_pdelay_resp(void *buf, MsgPDelayResp * presp)
{ {
presp->requestReceiptTimestamp.secondsField.msb = int64_t secs, nsecs;
htons(*(UInteger16 *) (buf + 34));
presp->requestReceiptTimestamp.secondsField.lsb = secs = htons(*(UInteger16 *) (buf + 34));
htonl(*(UInteger32 *) (buf + 36)); secs <<= 32;
presp->requestReceiptTimestamp.nanosecondsField = secs |= htonl(*(UInteger32 *) (buf + 36));
htonl(*(UInteger32 *) (buf + 40)); nsecs = htonl(*(UInteger32 *) (buf + 40));
presp->requestReceiptTimestamp.secs = secs;
presp->requestReceiptTimestamp.scaled_nsecs = nsecs << 16;
memcpy(&presp->requestingPortIdentity.clockIdentity, memcpy(&presp->requestingPortIdentity.clockIdentity,
(buf + 44), PP_CLOCK_IDENTITY_LENGTH); (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
presp->requestingPortIdentity.portNumber = presp->requestingPortIdentity.portNumber =
...@@ -436,49 +460,37 @@ int msg_issue_announce(struct pp_instance *ppi) ...@@ -436,49 +460,37 @@ int msg_issue_announce(struct pp_instance *ppi)
/* Pack and send on event multicast ip adress a Sync message */ /* Pack and send on event multicast ip adress a Sync message */
int msg_issue_sync_followup(struct pp_instance *ppi) int msg_issue_sync_followup(struct pp_instance *ppi)
{ {
Timestamp tstamp; struct pp_time now;
TimeInternal now, *time_snt;
int e, len; int e, len;
/* Send sync on the event channel with the "current" timestamp */ /* Send sync on the event channel with the "current" timestamp */
ppi->t_ops->get(ppi, &now); ppi->t_ops->get(ppi, &now);
from_TimeInternal(&now, &tstamp); len = msg_pack_sync(ppi, &now);
len = msg_pack_sync(ppi, &tstamp);
e = __send_and_log(ppi, len, PP_NP_EVT); e = __send_and_log(ppi, len, PP_NP_EVT);
if (e) return e; if (e) return e;
/* Send followup on general channel with sent-stamp of sync */ /* Send followup on general channel with sent-stamp of sync */
time_snt = &ppi->last_snt_time; len = msg_pack_follow_up(ppi, &ppi->last_snt_time);
from_TimeInternal(time_snt, &tstamp);
len = msg_pack_follow_up(ppi, &tstamp);
return __send_and_log(ppi, len, PP_NP_GEN); return __send_and_log(ppi, len, PP_NP_GEN);
} }
/* Pack and send on general multicast ip address a FollowUp message */ /* 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; int len;
from_TimeInternal(time, &prec_orig_tstamp); len = msg_pack_pdelay_resp_follow_up(ppi, &ppi->received_ptp_header, t);
len = msg_pack_pdelay_resp_follow_up(ppi, &ppi->received_ptp_header,
&prec_orig_tstamp);
return __send_and_log(ppi, len, PP_NP_GEN); return __send_and_log(ppi, len, PP_NP_GEN);
} }
/* Pack and send on event multicast ip adress a DelayReq message */ /* Pack and send on event multicast ip adress a DelayReq message */
static int msg_issue_delay_req(struct pp_instance *ppi) static int msg_issue_delay_req(struct pp_instance *ppi)
{ {
Timestamp orig_tstamp; struct pp_time now;
TimeInternal now;
int len; int len;
ppi->t_ops->get(ppi, &now); ppi->t_ops->get(ppi, &now);
from_TimeInternal(&now, &orig_tstamp); len = msg_pack_delay_req(ppi, &now);
len = msg_pack_delay_req(ppi, &orig_tstamp);
return __send_and_log(ppi, len, PP_NP_EVT); return __send_and_log(ppi, len, PP_NP_EVT);
} }
...@@ -486,15 +498,11 @@ static int msg_issue_delay_req(struct pp_instance *ppi) ...@@ -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 */ /* Pack and send on event multicast ip adress a PDelayReq message */
static int msg_issue_pdelay_req(struct pp_instance *ppi) static int msg_issue_pdelay_req(struct pp_instance *ppi)
{ {
Timestamp orig_tstamp; struct pp_time now;
TimeInternal now;
int len; int len;
ppi->t_ops->get(ppi, &now); ppi->t_ops->get(ppi, &now);
from_TimeInternal(&now, &orig_tstamp); len = msg_pack_pdelay_req(ppi, &now);
len = msg_pack_pdelay_req(ppi, &orig_tstamp);
return __send_and_log(ppi, len, PP_NP_EVT); return __send_and_log(ppi, len, PP_NP_EVT);
} }
...@@ -506,27 +514,19 @@ int msg_issue_request(struct pp_instance *ppi) ...@@ -506,27 +514,19 @@ int msg_issue_request(struct pp_instance *ppi)
} }
/* Pack and send on event multicast ip adress a DelayResp message */ /* 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; int len;
from_TimeInternal(time, &rcv_tstamp); len = msg_pack_delay_resp(ppi, &ppi->received_ptp_header, t);
len = msg_pack_delay_resp(ppi, &ppi->received_ptp_header, &rcv_tstamp);
return __send_and_log(ppi, len, PP_NP_GEN); return __send_and_log(ppi, len, PP_NP_GEN);
} }
/* Pack and send on event multicast ip adress a DelayResp message */ /* 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; int len;
from_TimeInternal(time, &rcv_tstamp); len = msg_pack_pdelay_resp(ppi, &ppi->received_ptp_header, t);
len = msg_pack_pdelay_resp(ppi, &ppi->received_ptp_header, &rcv_tstamp);
return __send_and_log(ppi, len, PP_NP_EVT); return __send_and_log(ppi, len, PP_NP_EVT);
} }
proto-standard/servo.c
View file @ 35eef679
...@@ -9,10 +9,10 @@ ...@@ -9,10 +9,10 @@
#include <ppsi/ppsi.h> #include <ppsi/ppsi.h>
static void pp_servo_mpd_fltr(struct pp_instance *, struct pp_avg_fltr *, static void pp_servo_mpd_fltr(struct pp_instance *, struct pp_avg_fltr *,
TimeInternal *); struct pp_time *);
static int pp_servo_offset_master(struct pp_instance *, TimeInternal *, static int pp_servo_offset_master(struct pp_instance *, struct pp_time *,
TimeInternal *, TimeInternal *); struct pp_time *, struct pp_time *);
static Integer32 pp_servo_pi_controller(struct pp_instance *, TimeInternal *); static int64_t pp_servo_pi_controller(struct pp_instance *, struct pp_time *);
void pp_servo_init(struct pp_instance *ppi) void pp_servo_init(struct pp_instance *ppi)
...@@ -43,21 +43,22 @@ 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 */ /* 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]; static char s[24];
pp_sprintf(s, "%s%d.%09d", 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; return s;
} }
/* Called by slave and uncalib when we have t1 and t2 */ /* Called by slave and uncalib when we have t1 and t2 */
void pp_servo_got_sync(struct pp_instance *ppi) 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 * calc 'master_to_slave_delay', removing the correction field
...@@ -65,46 +66,48 @@ void pp_servo_got_sync(struct pp_instance *ppi) ...@@ -65,46 +66,48 @@ void pp_servo_got_sync(struct pp_instance *ppi)
* cField contains the sum of sync and followup messages * cField contains the sum of sync and followup messages
* correctionField(s) * correctionField(s)
*/ */
sub_TimeInternal(m_to_s_dly, &ppi->t2, &ppi->t1); *m_to_s_dly = ppi->t2;
sub_TimeInternal(m_to_s_dly, m_to_s_dly, &ppi->cField); 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", 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 */ /* Called by slave and uncalib when we have t1 and t2 */
void pp_servo_got_psync(struct pp_instance *ppi) void pp_servo_got_psync(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;
TimeInternal *mpd = &DSCUR(ppi)->meanPathDelay; struct pp_time *mpd = &DSCUR(ppi)->meanPathDelay;
TimeInternal *ofm = &DSCUR(ppi)->offsetFromMaster; struct pp_time *ofm = &DSCUR(ppi)->offsetFromMaster;
Integer32 adj; int adj32;
pp_diag(ppi, servo, 2, "T1: %s\n", fmt_TI(&ppi->t1)); pp_diag(ppi, servo, 2, "T1: %s\n", fmt_ppt(&ppi->t1));
pp_diag(ppi, servo, 2, "T2: %s\n", fmt_TI(&ppi->t2)); pp_diag(ppi, servo, 2, "T2: %s\n", fmt_ppt(&ppi->t2));
/* /*
* calc 'master_to_slave_delay', removing the correction field * calc 'master_to_slave_delay', removing the correction field
* added by transparent clocks in the path. * added by transparent clocks in the path.
*/ */
sub_TimeInternal(m_to_s_dly, &ppi->t2, &ppi->t1); *m_to_s_dly = ppi->t2;
sub_TimeInternal(m_to_s_dly, m_to_s_dly, &ppi->cField); 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", 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 */ /* update 'offsetFromMaster' and possibly jump in time */
if (pp_servo_offset_master(ppi, mpd, ofm, m_to_s_dly)) if (pp_servo_offset_master(ppi, mpd, ofm, m_to_s_dly))
return; return;
/* PI controller */ /* PI controller returns a scaled_nsecs adjustment, so shift back */
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 /* apply controller output as a clock tick rate adjustment, if
* provided by arch, or as a raw offset otherwise */ * provided by arch, or as a raw offset otherwise */
if (pp_can_adjust(ppi)) { if (pp_can_adjust(ppi)) {
if (ppi->t_ops->adjust_freq) if (ppi->t_ops->adjust_freq)
ppi->t_ops->adjust_freq(ppi, -adj); ppi->t_ops->adjust_freq(ppi, -adj32);
else else
ppi->t_ops->adjust_offset(ppi, -adj); ppi->t_ops->adjust_offset(ppi, -adj32);
} }
pp_diag(ppi, servo, 2, "Observed drift: %9i\n", pp_diag(ppi, servo, 2, "Observed drift: %9i\n",
...@@ -114,16 +117,16 @@ void pp_servo_got_psync(struct pp_instance *ppi) ...@@ -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) */ /* called by slave states when delay_resp is received (all t1..t4 are valid) */
void pp_servo_got_resp(struct pp_instance *ppi) void pp_servo_got_resp(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;
TimeInternal *s_to_m_dly = &SRV(ppi)->s_to_m_dly; struct pp_time *s_to_m_dly = &SRV(ppi)->s_to_m_dly;
TimeInternal *mpd = &DSCUR(ppi)->meanPathDelay; struct pp_time *mpd = &DSCUR(ppi)->meanPathDelay;
TimeInternal *ofm = &DSCUR(ppi)->offsetFromMaster; struct pp_time *ofm = &DSCUR(ppi)->offsetFromMaster;
struct pp_avg_fltr *mpd_fltr = &SRV(ppi)->mpd_fltr; struct pp_avg_fltr *mpd_fltr = &SRV(ppi)->mpd_fltr;
Integer32 adj; int adj32;
/* We sometimes enter here before we got sync/f-up */ /* 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"); pp_diag(ppi, servo, 2, "discard T3/T4: we miss T1/T2\n");
return; return;
} }
...@@ -131,24 +134,26 @@ void pp_servo_got_resp(struct pp_instance *ppi) ...@@ -131,24 +134,26 @@ void pp_servo_got_resp(struct pp_instance *ppi)
* calc 'slave_to_master_delay', removing delay_resp correction field * calc 'slave_to_master_delay', removing delay_resp correction field
* added by transparent clocks in the path. * added by transparent clocks in the path.
*/ */
sub_TimeInternal(s_to_m_dly, &ppi->t4, &ppi->t3); *s_to_m_dly = ppi->t4;
sub_TimeInternal(s_to_m_dly, s_to_m_dly, &ppi->cField); 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", 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, "T1: %s\n", fmt_ppt(&ppi->t1));
pp_diag(ppi, servo, 2, "T2: %s\n", fmt_TI(&ppi->t2)); pp_diag(ppi, servo, 2, "T2: %s\n", fmt_ppt(&ppi->t2));
pp_diag(ppi, servo, 2, "T3: %s\n", fmt_TI(&ppi->t3)); pp_diag(ppi, servo, 2, "T3: %s\n", fmt_ppt(&ppi->t3));
pp_diag(ppi, servo, 2, "T4: %s\n", fmt_TI(&ppi->t4)); pp_diag(ppi, servo, 2, "T4: %s\n", fmt_ppt(&ppi->t4));
pp_diag(ppi, servo, 1, "Master to slave: %s\n", fmt_TI(m_to_s_dly)); 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_TI(s_to_m_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" */ /* 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); *mpd = SRV(ppi)->m_to_s_dly;
div2_TimeInternal(mpd); pp_time_add(mpd, &SRV(ppi)->s_to_m_dly);
pp_diag(ppi, servo, 1, "meanPathDelay: %s\n", fmt_TI(mpd)); 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; return;
/* mean path delay filtering */ /* mean path delay filtering */
...@@ -159,15 +164,15 @@ void pp_servo_got_resp(struct pp_instance *ppi) ...@@ -159,15 +164,15 @@ void pp_servo_got_resp(struct pp_instance *ppi)
return; return;
/* PI controller */ /* 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 /* apply controller output as a clock tick rate adjustment, if
* provided by arch, or as a raw offset otherwise */ * provided by arch, or as a raw offset otherwise */
if (pp_can_adjust(ppi)) { if (pp_can_adjust(ppi)) {
if (ppi->t_ops->adjust_freq) if (ppi->t_ops->adjust_freq)
ppi->t_ops->adjust_freq(ppi, -adj); ppi->t_ops->adjust_freq(ppi, -adj32);
else else
ppi->t_ops->adjust_offset(ppi, -adj); ppi->t_ops->adjust_offset(ppi, -adj32);
} }
pp_diag(ppi, servo, 2, "Observed drift: %9i\n", pp_diag(ppi, servo, 2, "Observed drift: %9i\n",
...@@ -177,35 +182,38 @@ void pp_servo_got_resp(struct pp_instance *ppi) ...@@ -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) */ /* called by slave states when delay_resp is received (all t1..t4 are valid) */
void pp_servo_got_presp(struct pp_instance *ppi) void pp_servo_got_presp(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;
TimeInternal *s_to_m_dly = &SRV(ppi)->s_to_m_dly; struct pp_time *s_to_m_dly = &SRV(ppi)->s_to_m_dly;
TimeInternal *mpd = &DSCUR(ppi)->meanPathDelay; struct pp_time *mpd = &DSCUR(ppi)->meanPathDelay;
struct pp_avg_fltr *mpd_fltr = &SRV(ppi)->mpd_fltr; struct pp_avg_fltr *mpd_fltr = &SRV(ppi)->mpd_fltr;
/* /*
* calc 'slave_to_master_delay', removing the correction field * calc 'slave_to_master_delay', removing the correction field
* added by transparent clocks in the path. * added by transparent clocks in the path.
*/ */
sub_TimeInternal(s_to_m_dly, &ppi->t6, &ppi->t5); *s_to_m_dly = ppi->t6;
sub_TimeInternal(s_to_m_dly, s_to_m_dly, &ppi->cField); 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", 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, "T3: %s\n", fmt_ppt(&ppi->t3));
pp_diag(ppi, servo, 2, "T4: %s\n", fmt_TI(&ppi->t4)); pp_diag(ppi, servo, 2, "T4: %s\n", fmt_ppt(&ppi->t4));
pp_diag(ppi, servo, 2, "T5: %s\n", fmt_TI(&ppi->t5)); pp_diag(ppi, servo, 2, "T5: %s\n", fmt_ppt(&ppi->t5));
pp_diag(ppi, servo, 2, "T6: %s\n", fmt_TI(&ppi->t6)); pp_diag(ppi, servo, 2, "T6: %s\n", fmt_ppt(&ppi->t6));
pp_diag(ppi, servo, 1, "Master to slave: %s\n", fmt_TI(m_to_s_dly)); 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_TI(s_to_m_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" */ /* 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); *mpd = SRV(ppi)->m_to_s_dly;
div2_TimeInternal(mpd); pp_time_add(mpd, &SRV(ppi)->s_to_m_dly);
pp_diag(ppi, servo, 1, "meanPathDelay: %s\n", fmt_TI(mpd)); 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; return;
pp_servo_mpd_fltr(ppi, mpd_fltr, mpd); pp_servo_mpd_fltr(ppi, mpd_fltr, mpd);
...@@ -213,22 +221,25 @@ void pp_servo_got_presp(struct pp_instance *ppi) ...@@ -213,22 +221,25 @@ void pp_servo_got_presp(struct pp_instance *ppi)
static static
void pp_servo_mpd_fltr(struct pp_instance *ppi, struct pp_avg_fltr *mpd_fltr, void pp_servo_mpd_fltr(struct pp_instance *ppi, struct pp_avg_fltr *mpd_fltr,
TimeInternal * mpd) struct pp_time *mpd)
{ {
int s; int s;
uint64_t y;
if (mpd_fltr->s_exp < 1) { if (mpd_fltr->s_exp < 1) {
/* First time, keep what we have */ /* 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; s = OPTS(ppi)->s;
while (abs(mpd_fltr->y) >> (31 - s)) while (mpd_fltr->y >> (63 - s))
--s; --s;
if (mpd_fltr->s_exp > 1 << s) if (mpd_fltr->s_exp > 1LL << s)
mpd_fltr->s_exp = 1 << s; mpd_fltr->s_exp = 1LL << s;
/* crank down filter cutoff by increasing 's_exp' */ /* 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; ++mpd_fltr->s_exp;
/* /*
...@@ -238,10 +249,10 @@ void pp_servo_mpd_fltr(struct pp_instance *ppi, struct pp_avg_fltr *mpd_fltr, ...@@ -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 * measured in the master. Ignore such values, by keeping the
* current average instead. * current average instead.
*/ */
if (mpd->nanoseconds < 0) if (mpd->scaled_nsecs < 0)
mpd->nanoseconds = mpd_fltr->y; mpd->scaled_nsecs = mpd_fltr->y;
if (mpd->nanoseconds < 0) if (mpd->scaled_nsecs < 0)
mpd->nanoseconds = 0; mpd->scaled_nsecs = 0;
/* /*
* It may happen that mpd appears to be very big. This happens * 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, ...@@ -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 * The constant multipliers have been chosed arbitrarily, but
* they work well in testing environment. * 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", 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 */ /* 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) */ /* filter 'meanPathDelay' (running average) -- use an unsigned "y" */
mpd_fltr->y = (mpd_fltr->y * (mpd_fltr->s_exp - 1) + mpd->nanoseconds) y = (mpd_fltr->y * (mpd_fltr->s_exp - 1) + mpd->scaled_nsecs);
/ mpd_fltr->s_exp; __div64_32(&y, mpd_fltr->s_exp);
mpd->nanoseconds = mpd_fltr->y; mpd->scaled_nsecs = mpd_fltr->y = y;
pp_diag(ppi, servo, 1, "After avg(%i), meanPathDelay: %i\n", 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 static
int pp_servo_offset_master(struct pp_instance *ppi, TimeInternal * mpd, int pp_servo_offset_master(struct pp_instance *ppi, struct pp_time *mpd,
TimeInternal * ofm, TimeInternal * m_to_s_dly) struct pp_time *ofm, struct pp_time *m_to_s_dly)
{ {
TimeInternal time_tmp; struct pp_time time_tmp;
*ofm = *m_to_s_dly;
sub_TimeInternal(ofm, m_to_s_dly, mpd); pp_time_sub(ofm, mpd);
pp_diag(ppi, servo, 1, "Offset from master: %s\n", fmt_TI(ofm)); pp_diag(ppi, servo, 1, "Offset from master: %s\n", fmt_ppt(ofm));
if (!ofm->seconds) if (!ofm->secs)
return 0; /* proceeed with adjust */ return 0; /* proceeed with adjust */
if (!pp_can_adjust(ppi)) if (!pp_can_adjust(ppi))
return 0; /* e.g., a loopback test run... "-t" on cmdline */ return 0; /* e.g., a loopback test run... "-t" on cmdline */
ppi->t_ops->get(ppi, &time_tmp); 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); ppi->t_ops->set(ppi, &time_tmp);
pp_servo_init(ppi); pp_servo_init(ppi);
return 1; /* done */ return 1; /* done */
} }
static 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 I_term;
long long P_term; long long P_term;
long long tmp; long long tmp;
int I_sign; int I_sign;
int P_sign; int P_sign;
Integer32 adj; int64_t adj;
/* the accumulator for the I component, shift by 10 to avoid losing bits /* the accumulator for the I component */
* later in the division */ SRV(ppi)->obs_drift += ofm->scaled_nsecs;
SRV(ppi)->obs_drift += ((long long)ofm->nanoseconds) << 10;
/* Anti-windup. The PP_ADJ_FREQ_MAX value is multiplied by OPTS(ppi)->ai /* 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). * (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 */ * 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) if (SRV(ppi)->obs_drift > tmp)
SRV(ppi)->obs_drift = tmp; SRV(ppi)->obs_drift = tmp;
else if (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) ...@@ -324,9 +334,9 @@ Integer32 pp_servo_pi_controller(struct pp_instance * ppi, TimeInternal * ofm)
I_term = -I_term; I_term = -I_term;
/* calculation of the P component */ /* calculation of the P component */
P_sign = (ofm->nanoseconds > 0) ? 0 : -1; P_sign = (ofm->scaled_nsecs > 0) ? 0 : -1;
/* shift 10 bits again to avoid losses */ /* alrady shifted 16 bits, so we avoid losses */
P_term = ((long long)ofm->nanoseconds) << 10; P_term = ofm->scaled_nsecs;
if (P_sign) if (P_sign)
P_term = -P_term; P_term = -P_term;
__div64_32((uint64_t *)&P_term, OPTS(ppi)->ap); __div64_32((uint64_t *)&P_term, OPTS(ppi)->ap);
...@@ -334,13 +344,8 @@ Integer32 pp_servo_pi_controller(struct pp_instance * ppi, TimeInternal * ofm) ...@@ -334,13 +344,8 @@ Integer32 pp_servo_pi_controller(struct pp_instance * ppi, TimeInternal * ofm)
P_term = -P_term; P_term = -P_term;
/* calculate the correction of applied by the controller */ /* calculate the correction of applied by the controller */
tmp = P_term + I_term; adj = P_term + I_term;
/* Now scale it before passing the argument to adjtimex /* Return the scaled-nanos values; the caller is scaling back */
* Be careful with the signs */
if (tmp > 0)
adj = (tmp >> 10);
else
adj = -((-tmp) >> 10);
return adj; return adj;
} }
proto-standard/state-slave.c
View file @ 35eef679
...@@ -51,10 +51,9 @@ static int slave_handle_response(struct pp_instance *ppi, unsigned char *pkt, ...@@ -51,10 +51,9 @@ static int slave_handle_response(struct pp_instance *ppi, unsigned char *pkt,
return 0; return 0;
} }
to_TimeInternal(&ppi->t4, &resp.receiveTimestamp); ppi->t4 = resp.receiveTimestamp;
/* Save delay resp cf in ppi->cField */ /* Save delay resp cf in ppi->cField */
cField_to_TimeInternal(&ppi->cField, ppi->cField = hdr->cField; /* FIXME: merge cField here? */
hdr->correctionfield);
if (pp_hooks.handle_resp) if (pp_hooks.handle_resp)
e = pp_hooks.handle_resp(ppi); e = pp_hooks.handle_resp(ppi);
......
time-unix/unix-socket.c
View file @ 35eef679
...@@ -26,7 +26,7 @@ ...@@ -26,7 +26,7 @@
/* unix_recv_msg uses recvmsg for timestamp query */ /* unix_recv_msg uses recvmsg for timestamp query */
static int unix_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len, 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; struct ethhdr *hdr = pkt;
ssize_t ret; ssize_t ret;
...@@ -88,9 +88,8 @@ static int unix_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len, ...@@ -88,9 +88,8 @@ static int unix_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len,
} }
if (tv) { if (tv) {
t->seconds = tv->tv_sec + DSPRO(ppi)->currentUtcOffset; t->secs = tv->tv_sec + DSPRO(ppi)->currentUtcOffset;
t->nanoseconds = tv->tv_usec * 1000; t->scaled_nsecs = (uint64_t)(tv->tv_usec * 1000) << 16;
t->correct = 1;
} else { } else {
/* /*
* get the recording time here, even though it may put a big * 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, ...@@ -122,13 +121,14 @@ static int unix_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len,
/* This is not really hw... */ /* This is not really hw... */
pp_diag(ppi, time, 2, "recv stamp: %i.%09i (%s)\n", 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; return ret;
} }
/* Receive and send is *not* so trivial */ /* Receive and send is *not* so trivial */
static int unix_net_recv(struct pp_instance *ppi, void *pkt, int len, 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 pp_channel *ch1, *ch2;
struct ethhdr *hdr = pkt; struct ethhdr *hdr = pkt;
...@@ -184,7 +184,7 @@ static int unix_net_send(struct pp_instance *ppi, void *pkt, int len, ...@@ -184,7 +184,7 @@ static int unix_net_send(struct pp_instance *ppi, void *pkt, int len,
struct ethhdr *hdr = pkt; struct ethhdr *hdr = pkt;
struct pp_vlanhdr *vhdr = pkt; struct pp_vlanhdr *vhdr = pkt;
struct pp_channel *ch = ppi->ch + chtype; 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; int is_pdelay = pp_msgtype_info[msgtype].is_pdelay;
static const uint16_t udpport[] = { static const uint16_t udpport[] = {
[PP_NP_GEN] = PP_GEN_PORT, [PP_NP_GEN] = PP_GEN_PORT,
...@@ -653,4 +653,3 @@ struct pp_network_operations unix_net_ops = { ...@@ -653,4 +653,3 @@ struct pp_network_operations unix_net_ops = {
.check_packet = unix_net_check_packet, .check_packet = unix_net_check_packet,
}; };
time-unix/unix-time.c
View file @ 35eef679
...@@ -23,22 +23,21 @@ static void clock_fatal_error(char *context) ...@@ -23,22 +23,21 @@ static void clock_fatal_error(char *context)
exit(1); 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; struct timespec tp;
if (clock_gettime(CLOCK_REALTIME, &tp) < 0) if (clock_gettime(CLOCK_REALTIME, &tp) < 0)
clock_fatal_error("clock_gettime"); clock_fatal_error("clock_gettime");
/* TAI = UTC + 35 */ /* TAI = UTC + 35 */
t->seconds = tp.tv_sec + DSPRO(ppi)->currentUtcOffset; t->secs = tp.tv_sec + DSPRO(ppi)->currentUtcOffset;
t->nanoseconds = tp.tv_nsec; t->scaled_nsecs = ((int64_t)tp.tv_nsec) << 16;
t->correct = 1;
if (!(pp_global_d_flags & PP_FLAG_NOTIMELOG)) if (!(pp_global_d_flags & PP_FLAG_NOTIMELOG))
pp_diag(ppi, time, 2, "%s: %9li.%09li\n", __func__, pp_diag(ppi, time, 2, "%s: %9li.%09li\n", __func__,
tp.tv_sec, tp.tv_nsec); tp.tv_sec, tp.tv_nsec);
return 0; 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; struct timespec tp;
...@@ -55,8 +54,8 @@ static int unix_time_set(struct pp_instance *ppi, TimeInternal *t) ...@@ -55,8 +54,8 @@ static int unix_time_set(struct pp_instance *ppi, TimeInternal *t)
} }
/* UTC = TAI - 35 */ /* UTC = TAI - 35 */
tp.tv_sec = t->seconds - DSPRO(ppi)->currentUtcOffset; tp.tv_sec = t->secs - DSPRO(ppi)->currentUtcOffset;
tp.tv_nsec = t->nanoseconds; tp.tv_nsec = t->scaled_nsecs >> 16;
if (clock_settime(CLOCK_REALTIME, &tp) < 0) if (clock_settime(CLOCK_REALTIME, &tp) < 0)
clock_fatal_error("clock_settime"); clock_fatal_error("clock_settime");
pp_diag(ppi, time, 1, "%s: %9li.%09li\n", __func__, pp_diag(ppi, time, 1, "%s: %9li.%09li\n", __func__,
......
tools/dump-funcs.c
View file @ 35eef679
...@@ -31,18 +31,18 @@ static int dumpstruct(char *p1, char *p2, char *name, void *ptr, int size) ...@@ -31,18 +31,18 @@ static int dumpstruct(char *p1, char *p2, char *name, void *ptr, int size)
} }
#if __STDC_HOSTED__ #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 timeval tv;
struct tm tm; struct tm tm;
tv.tv_sec = ti->seconds; tv.tv_sec = t->secs;
tv.tv_usec = ti->nanoseconds / 1000; tv.tv_usec = (t->scaled_nsecs >> 16) / 1000;
localtime_r(&tv.tv_sec, &tm); localtime_r(&tv.tv_sec, &tm);
printf("%sTIME: (%li - 0x%lx) %02i:%02i:%02i.%06li%s\n", prefix, printf("%sTIME: (%li - 0x%lx) %02i:%02i:%02i.%06li%s\n", prefix,
tv.tv_sec, tv.tv_sec, tv.tv_sec, tv.tv_sec,
tm.tm_hour, tm.tm_min, tm.tm_sec, (long)tv.tv_usec, tm.tm_hour, tm.tm_min, tm.tm_sec, (long)tv.tv_usec,
!ti->correct ? " invalid" : ""); is_incorrect(t) ? " invalid" : "");
} }
#else #else
static void dump_time(char *prefix, struct TimeInternal *ti) static void dump_time(char *prefix, struct TimeInternal *ti)
...@@ -259,7 +259,7 @@ out: ...@@ -259,7 +259,7 @@ out:
} }
/* This dumps a complete udp frame, starting from the eth header */ /* 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) int vlan)
{ {
struct ethhdr *eth = buf; struct ethhdr *eth = buf;
...@@ -267,8 +267,8 @@ int dump_udppkt(char *prefix, void *buf, int len, struct TimeInternal *ti, ...@@ -267,8 +267,8 @@ int dump_udppkt(char *prefix, void *buf, int len, struct TimeInternal *ti,
struct udphdr *udp; struct udphdr *udp;
void *payload; void *payload;
if (ti) if (t)
dump_time(prefix, ti); dump_time(prefix, t);
dump_vlan(prefix, vlan); dump_vlan(prefix, vlan);
...@@ -285,23 +285,23 @@ int dump_udppkt(char *prefix, void *buf, int len, struct TimeInternal *ti, ...@@ -285,23 +285,23 @@ int dump_udppkt(char *prefix, void *buf, int len, struct TimeInternal *ti,
} }
/* This dumps the payload only, used for udp frames without headers */ /* 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) if (t)
dump_time(prefix, ti); dump_time(prefix, t);
dump_payload(prefix, buf, len); dump_payload(prefix, buf, len);
return 0; return 0;
} }
/* This dumps everything, used for raw frames with headers and ptp payload */ /* 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) int vlan)
{ {
struct ethhdr *eth = buf; struct ethhdr *eth = buf;
void *payload; void *payload;
if (ti) if (t)
dump_time(prefix, ti); dump_time(prefix, t);
dump_vlan(prefix, vlan); dump_vlan(prefix, vlan);
payload = buf + dump_eth(prefix, eth); payload = buf + dump_eth(prefix, eth);
dump_payload(prefix, payload, len - (payload - buf)); dump_payload(prefix, payload, len - (payload - buf));
......
tools/ptpdump.h
View file @ 35eef679
...@@ -14,10 +14,11 @@ ...@@ -14,10 +14,11 @@
#define printf pp_printf #define printf pp_printf
#endif #endif
int dump_udppkt(char *prefix, void *buf, int len, struct TimeInternal *ti, int dump_udppkt(char *prefix, void *buf, int len,
int vlan); const struct pp_time *t, int vlan);
int dump_payloadpkt(char *prefix, void *buf, int len, struct TimeInternal *ti); int dump_payloadpkt(char *prefix, void *buf, int len,
int dump_1588pkt(char *prefix, void *buf, int len, struct TimeInternal *ti, const struct pp_time *t);
int dump_1588pkt(char *prefix, void *buf, int len, const struct pp_time *t,
int vlan); int vlan);
#endif /* __PTPDUMP_H__ */ #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