Commit c81db09e authored by baujc's avatar baujc

Big update ...

- New L1Sync and PTP servo calculation (delayAsym,...)
- Make function calls more uniforms between extensions
- refactoring
- Add a new state machine to enable/disable an extension and provide
only PTP support: New timer, new hooks, ...
- Fix bug in time format conversion
- Code removed when an extension is not required (#ifdef XXX )
- Align the PTP and L1Sync servo calculation (shared fct, same
calculation,...)
- Force only one servo running at a given time for a given instance
- New servo reset hook : called by main loop
- Add function to converte a time into a string
- Force to stay in FAULTY state during 60s
- Optimise function wrs_enable_timing_output()
parent d456d2fd
......@@ -69,7 +69,8 @@ static int run_all_state_machines(struct pp_globals *ppg)
ppi->frgn_rec_num = 0;
ppi->frgn_rec_best = -1;
if (ppg->ebest_idx == ppi->port_idx)
wr_servo_reset(ppi);
if( ppi->ext_hooks->servo_reset)
(*ppi->ext_hooks->servo_reset)(ppi);
}
}
......
......@@ -35,9 +35,13 @@ int wrs_read_calibration_data(struct pp_instance *ppi,
* way as the HAL itself was doing to fill the RPC structure.
* Formulas copied from libwr/hal_shmem.c (get_exported_state).
*/
#if CONFIG_PROFILE_WR == 1
port_fix_alpha = FIX_ALPHA_TWO_POW_FRACBITS *
((p->calib.sfp.alpha + 1.0) / (p->calib.sfp.alpha + 2.0)
- 0.5);
#else
port_fix_alpha =0;
#endif
if ( delta_tx || delta_rx) {
port_delta_tx = p->calib.delta_tx_phy
......@@ -64,15 +68,20 @@ int wrs_read_calibration_data(struct pp_instance *ppi,
int wrs_read_correction_data(struct pp_instance *ppi, int64_t *fiber_fix_alpha,
int32_t *clock_period_ps, uint32_t *bit_slide_ps) {
double alpha;
int32_t port_cP;
wrs_read_calibration_data(ppi, NULL, NULL,NULL, &port_cP, bit_slide_ps);
if(fiber_fix_alpha) {
#if CONFIG_PROFILE_WR == 1
double alpha;
alpha = ((double) ppi->asymmetryCorrectionPortDS.scaledDelayCoefficient)/REL_DIFF_TWO_POW_FRACBITS;
*fiber_fix_alpha = FIX_ALPHA_TWO_POW_FRACBITS * ((alpha + 1.0) / (alpha + 2.0) - 0.5);
#else
*fiber_fix_alpha=0;
#endif
}
if(clock_period_ps)
*clock_period_ps = port_cP; /* REF_CLOCK_PERIOD_PS */
return WRH_HW_CALIB_OK;
......
......@@ -102,15 +102,18 @@ static __inline__ double calculateDelayAsymCoefficient(double delayCoefficient
static void enable_asymmetryCorrection(struct pp_instance *ppi, Boolean enable ) {
if ( (ppi->asymmetryCorrectionPortDS.enable=enable)==TRUE ) {
/* Enabled: The delay asymmetry will be calculated */
ppi->asymmetryCorrectionPortDS.scaledDelayCoefficient=
ppi->cfg.scaledDelayCoefficient != 0 ?
ppi->cfg.scaledDelayCoefficient :
(RelativeDifference)(ppi->cfg.delayCoefficient * REL_DIFF_TWO_POW_FRACBITS);
ppi->portDS->delayAsymCoeff=(RelativeDifference)(calculateDelayAsymCoefficient(ppi->cfg.delayCoefficient) * REL_DIFF_TWO_POW_FRACBITS);
} else {
/* Disabled: The delay asymmetry will be provided by the config (constantAsymmetry) */
ppi->asymmetryCorrectionPortDS.constantAsymmetry=picos_to_interval(ppi->cfg.constantAsymmetry_ps);
double delayCoefficient;
if ( ppi->cfg.scaledDelayCoefficient != 0) {
ppi->asymmetryCorrectionPortDS.scaledDelayCoefficient=ppi->cfg.scaledDelayCoefficient;
delayCoefficient=ppi->cfg.scaledDelayCoefficient/REL_DIFF_TWO_POW_FRACBITS;
} else {
ppi->asymmetryCorrectionPortDS.scaledDelayCoefficient=(RelativeDifference)(ppi->cfg.delayCoefficient * REL_DIFF_TWO_POW_FRACBITS);
delayCoefficient=ppi->cfg.delayCoefficient;
}
ppi->portDS->delayAsymCoeff=(RelativeDifference)(calculateDelayAsymCoefficient(delayCoefficient) * REL_DIFF_TWO_POW_FRACBITS);
}
ppi->asymmetryCorrectionPortDS.constantAsymmetry=picos_to_interval(ppi->cfg.constantAsymmetry_ps);
}
int main(int argc, char **argv)
......@@ -266,9 +269,9 @@ int main(int argc, char **argv)
Boolean configured=FALSE;
#if CONFIG_PROFILE_HA == 1
sprintf(s, "port %i; iface wri%i; proto raw; profile ha", i + 1, i + 1);
configured=TRUE;
#endif
#if CONFIG_PROFILE_WR == 1
configured=TRUE;
if ( ! configured )
sprintf(s, "port %i; iface wri%i; proto raw; profile wr", i + 1, i + 1);
#endif
......@@ -288,6 +291,7 @@ int main(int argc, char **argv)
ppi->portDS = wrs_shm_alloc(ppsi_head, sizeof(*ppi->portDS));
ppi->servo = wrs_shm_alloc(ppsi_head, sizeof(*ppi->servo));
ppi->ext_hooks=&pp_hooks; /* Default value. Can be overwritten by an extension */
ppi->ptp_support=FALSE;
if (ppi->portDS) {
switch (ppi->cfg.profile) {
#if CONFIG_PROFILE_WR == 1
......@@ -324,9 +328,11 @@ int main(int argc, char **argv)
case PPSI_PROFILE_PTP :
/* Do not take care of L1SYNC */
enable_asymmetryCorrection(ppi,ppi->cfg.asymmetryCorrectionEnable);
ppi->protocol_extension=PPSI_EXT_NONE;
break;
#if CONFIG_PROFILE_CUSTOM == 1
case PPSI_PROFILE_CUSTOM :
ppi->protocol_extension=PPSI_EXT_NONE; /* can be changed ...*/
#if CONFIG_EXT_L1SYNC
if (ppi->cfg.l1SyncEnabled ) {
if ( !enable_l1Sync(ppi,TRUE) )
......
......@@ -276,7 +276,25 @@ int pp_state_machine(struct pp_instance *ppi, void *buf, int len)
if (ppi->state != ppi->next_state)
return pp_leave_current_state(ppi);
if (!len)
if (len) {
if ( ppi->link_state == PP_LSTATE_PROTOCOL_DETECTION ) {
if ( ppi->ptp_msg_received==FALSE ) {
/* First frame received since instance initialization */
int tmo;
ppi->ptp_msg_received=TRUE;
if ( ppi->ext_hooks->get_tmo_lstate_detection!=NULL)
tmo=(*ppi->ext_hooks->get_tmo_lstate_detection)(ppi);
else
tmo= ppi->timeouts[PP_TO_ANN_RECEIPT].initValueMs;
__pp_timeout_set(ppi,PP_TO_PROT_STATE, tmo);
}
}
if ( !ppi->ext_enabled && ppi->link_state==PP_LSTATE_PROTOCOL_DETECTION) {
/* Ptp protocol only */
ppi->link_state= PP_LSTATE_IN_PROGRESS;
}
} else
ppi->received_ptp_header.messageType = PPM_NO_MESSAGE;
err = ip->f1(ppi, buf, len);
......@@ -288,6 +306,19 @@ int pp_state_machine(struct pp_instance *ppi, void *buf, int len)
if (ppi->state != ppi->next_state)
return pp_leave_current_state(ppi);
/* Check protocol state */
if ( ppi->link_state==PP_LSTATE_PROTOCOL_DETECTION ) {
if ( ppi->ptp_msg_received && pp_timeout(ppi, PP_TO_PROT_STATE) ) {
if ( ppi->ptp_support && ppi->ext_enabled ) {
ppi->ext_enabled=FALSE;
ppi->ptp_msg_received=FALSE;
} else
ppi->link_state=PP_LSTATE_FAILURE;
}
}
if (ppi->link_state==PP_LSTATE_FAILURE ) {
}
/* run bmc independent of state, and since not message driven do this
* here 9.2.6.8 */
if (pp_timeout(ppi, PP_TO_BMC)) {
......@@ -307,6 +338,11 @@ int pp_state_machine(struct pp_instance *ppi, void *buf, int len)
/* Run the extension state machine. The extension can provide its own time-out */
if ( ppi->ext_hooks->run_ext_state_machine) {
int delay = ppi->ext_hooks->run_ext_state_machine(ppi);
if ( ppi->link_state==PP_LSTATE_FAILURE && ppi->ptp_support && ppi->ext_enabled ) {
ppi->ext_enabled=FALSE;
ppi->link_state=PP_LSTATE_PROTOCOL_DETECTION;
}
ppi->next_delay= (delay < ppi->next_delay) ? delay : ppi->next_delay;
}
......
......@@ -29,12 +29,6 @@
#define WRH_HW_CALIB_ERROR -1
#define WRH_HW_CALIB_NOT_FOUND -3
enum {
WRH_SERVO_ENTER, WRH_SERVO_LEAVE
};
#define FIX_ALPHA_FRACBITS 40
#define FIX_ALPHA_FRACBITS_AS_FLOAT 40.0
......@@ -70,7 +64,6 @@ struct wrh_operations {
unsigned int calibrationPatternLen);
int (*calib_pattern_disable)(struct pp_instance *ppi);
int (*enable_timing_output)(struct pp_instance *ppi, int enable);
int (*servo_hook)(struct pp_instance *ppi, int action);
int (*read_corr_data)(struct pp_instance *ppi, int64_t *fixAlpha,
int32_t *clock_period, uint32_t *bit_slide_ps);
......
......@@ -11,6 +11,7 @@
/* general purpose constants */
#define PP_NSEC_PER_SEC (1000*1000*1000)
#define PP_PSEC_PER_SEC ((int64_t)1000*(int64_t)PP_NSEC_PER_SEC)
/* implementation specific constants */
#define PP_MAX_LINKS 64
......@@ -112,6 +113,7 @@ enum pp_timeouts {
PP_TO_ANN_SEND,
PP_TO_FAULT,
PP_TO_QUALIFICATION,
PP_TO_PROT_STATE,
/* Two timeouts for the protocol extension */
PP_TO_EXT_0,
PP_TO_EXT_1,
......
......@@ -52,7 +52,9 @@ typedef struct _UInteger64 { /*/* TODO : Should be replaced by UInteger64 */
/* Page 19 :the time interval is expressed in units of nanoseconds and multiplied by 2 +16 */
#define TIME_INTERVAL_FRACBITS 16
#define TIME_INTERVAL_FRACMASK 0xFFFF
#define TIME_INTERVAL_FRACBITS_AS_FLOAT 16.0
#define TIME_INTERVAL_ROUNDING_VALUE (1<<(TIME_INTERVAL_FRACBITS-1))
/* Min/max value expressed in picos (int64_t) which can be stored in a TimeInterval type */
#define TIME_INTERVAL_MIN_PICOS_VALUE_AS_INT64 ((int64_t) 0xFE0C000000000000)
......@@ -74,6 +76,7 @@ typedef struct Timestamp { /* page 13 (33) -- no typedef expected */
#define REL_DIFF_FRACBITS 62
#define REL_DIFF_FRACBITS_AS_FLOAT 62.0
#define REL_DIFF_TWO_POW_FRACBITS ((double)4.611686018427388E18) /* double value returned by pow(2.0,62.0) */
#define REL_DIFF_FRACMASK 0x3fffffffffffffff
/* Min/max values for RelativeDifference type */
#define RELATIVE_DIFFERENCE_MIN_VALUE INT64_MIN
......
......@@ -105,21 +105,38 @@ struct pp_avg_fltr {
#define PP_SERVO_FLAG_VALID (1<<0)
#define PP_SERVO_FLAG_WAIT_HW (1<<1)
struct pp_servo {
int state;
#define PP_SERVO_RESET_DATA_SIZE (sizeof(struct pp_servo)-offsetof(struct pp_servo,reset_address))
#define PP_SERVO_RESET_DATA(servo) memset(&servo->reset_address,0,PP_SERVO_RESET_DATA_SIZE);
struct pp_time delayMS;
struct pp_time delaySM;
struct pp_servo {
/* ptp servo specific data */
long long obs_drift;
struct pp_avg_fltr mpd_fltr;
struct pp_time meanDelay;
struct pp_time offsetFromMaster;
/* diagnostic data */
/* Data shared with extension servo */
struct pp_time delayMM; /* Shared with extension servo */
struct pp_time delayMS; /* Shared with extension servo */
struct pp_time meanDelay; /* Shared with extension servo */
struct pp_time offsetFromMaster; /* Shared with extension servo */
unsigned long flags; /* PP_SERVO_FLAG_INVALID, PP_SERVO_FLAG_VALID, ...*/
uint32_t update_count; /* incremented each time the servo is running */
/* Data used only by extensions */
int state;
char servo_state_name[32]; /* Updated by the servo itself */
/*
* ----- All data after this line will be cleared during by a servo initialization
*/
int reset_address;
/* Data shared with extension servo */
uint32_t update_count; /* incremented each time the servo is running */
struct pp_time update_time; /* Last updated time of the servo */
struct pp_time t1, t2, t3, t4, t5, t6;
/* ptp servo specific data */
int servo_locked; /* TRUE when servo is locked. This info can be used by HAL */
int got_sync; /* True when T1/T2 are available */
};
enum { /* The two sockets. They are called "net path" for historical reasons */
......@@ -177,6 +194,17 @@ typedef struct {
unsigned long tmo;
} t_timeOutConfig;
/*
* This enumeration correspond to the protocol state of a pp_instance.
* It is used to decide which instance must be active on a given port.
*/
typedef enum {
PP_LSTATE_PROTOCOL_DETECTION, /* Checking if the peer instance is using the same protocol */
PP_LSTATE_IN_PROGRESS, /* Right protocol detected. Try to establish the link with peer instance */
PP_LSTATE_LINKED, /* Link with peer well established */
PP_LSTATE_FAILURE /* Impossible to connect correctly to a peer instance */
} pp_link_state;
/*
* Structure for the individual ppsi link
*/
......@@ -258,7 +286,10 @@ struct pp_instance {
Boolean received_dresp; /* Count the number of delay response messages received for a given delay request */
Boolean received_dresp_fup; /* Count the number of delay response follow up messages received for a given delay request */
#endif
Boolean ptp_msg_received; /* Use to detect reception of a ptp message after an ppsi instance initialization */
Boolean ptp_support; /* True if allow pure PTP support */
Boolean ext_enabled; /* True if the extension is enabled */
pp_link_state link_state;
};
/* The following things used to be bit fields. Other flags are now enums */
......
#ifndef __PPSI_PP_TIME_H__
#define __PPSI_PP_TIME_H__
#define TIME_FRACBITS 16
#define TIME_FRACMASK 0xFFFF
#define TIME_FRACBITS_AS_FLOAT 16.0
#define TIME_ROUNDING_VALUE (1<<(TIME_FRACBITS-1))
/* Everything internally uses this time format, *signed* */
struct pp_time {
int64_t secs;
......
......@@ -213,8 +213,8 @@ struct pp_ext_hooks {
void (*s1)(struct pp_instance *ppi, struct pp_frgn_master *frgn_master);
int (*execute_slave)(struct pp_instance *ppi);
int (*handle_announce)(struct pp_instance *ppi);
int (*handle_sync)(struct pp_instance *ppi, struct pp_time *orig);
int (*handle_followup)(struct pp_instance *ppi, struct pp_time *orig);
int (*handle_sync)(struct pp_instance *ppi);
int (*handle_followup)(struct pp_instance *ppi);
int (*handle_preq) (struct pp_instance * ppi);
int (*handle_presp) (struct pp_instance * ppi);
int (*handle_signaling) (struct pp_instance * ppi, void *buf, int len);
......@@ -224,8 +224,14 @@ struct pp_ext_hooks {
int (*state_decision)(struct pp_instance *ppi, int next_state);
void (*state_change)(struct pp_instance *ppi);
int (*run_ext_state_machine) (struct pp_instance *ppi);
void (*servo_reset)(struct pp_instance *ppi);
/* If the extension requires hardware support for precise time stamp, returns 1 */
int (*require_precise_timestamp)(struct pp_instance *ppi);
int (*get_tmo_lstate_detection) (struct pp_instance *ppi);
};
#define is_ext_hook_available(p, c) ( /*p->ext_enabled && */ p->ext_hooks->c)
/*
* Network methods are encapsulated in a structure, so each arch only needs
* to provide that structure. This simplifies management overall.
......@@ -508,9 +514,12 @@ extern int f_simple_int(struct pp_argline *l, int lineno,
/* Servo */
extern void pp_servo_init(struct pp_instance *ppi);
extern void pp_servo_got_sync(struct pp_instance *ppi); /* got t1 and t2 */
extern void pp_servo_got_resp(struct pp_instance *ppi); /* got all t1..t4 */
extern int pp_servo_got_resp(struct pp_instance *ppi); /* got all t1..t4 */
extern void pp_servo_got_psync(struct pp_instance *ppi); /* got t1 and t2 */
extern void pp_servo_got_presp(struct pp_instance *ppi); /* got all t3..t6 */
extern int pp_servo_got_presp(struct pp_instance *ppi); /* got all t3..t6 */
extern int pp_servo_calculate_delays(struct pp_instance *ppi);
extern void pp_servo_apply_faulty_stamp(struct pp_servo *s, struct pp_time *faulty_stamps, int index);
/* bmc.c */
extern void bmc_m1(struct pp_instance *ppi);
......@@ -554,11 +563,13 @@ extern void msg_unpack_pdelay_resp(void *buf, MsgPDelayResp * presp);
extern void msg_unpack_pdelay_req(void *buf, MsgPDelayReq * pdelay_req);
/* each of them returns 0 if ok, -1 in case of error in send, 1 if stamp err */
#define PP_SEND_OK 0
#define PP_SEND_ERROR -1
#define PP_SEND_NO_STAMP 1
#define PP_SEND_DROP -2
#define PP_RECV_DROP PP_SEND_DROP
typedef enum {
PP_SEND_OK=0,
PP_SEND_ERROR=-1,
PP_SEND_NO_STAMP=1,
PP_SEND_DROP=-2,
PP_RECV_DROP=PP_SEND_DROP
}pp_send_status;
extern void *msg_copy_header(MsgHeader *dest, MsgHeader *src); /* REMOVE ME!! */
extern int msg_issue_announce(struct pp_instance *ppi);
......@@ -587,7 +598,9 @@ extern void picos_to_pp_time(int64_t picos, struct pp_time *ts);
extern void pp_time_hardwarize(struct pp_time *time, int clock_period_ps,int32_t *ticks, int32_t *picos);
extern int64_t interval_to_picos(TimeInterval interval);
extern int is_timestamps_incorrect(struct pp_instance *ppsi, int *err_count, int ts_mask);
extern char *time_to_string(struct pp_time *t);
extern char *interval_to_string(TimeInterval time);
extern char *relative_interval_to_string(TimeInterval time);
/*
* The state machine itself is an array of these structures.
......
......@@ -6,6 +6,9 @@
*/
#include <limits.h>
#include <stdio.h>
#include <string.h>
#include <inttypes.h>
#include <ppsi/ppsi.h>
void normalize_pp_time(struct pp_time *t)
......@@ -38,17 +41,30 @@ void normalize_pp_time(struct pp_time *t)
/* Add a TimeInterval to a pp_time */
void pp_time_add_interval(struct pp_time *t1, TimeInterval t2)
{
t1->scaled_nsecs += t2;
normalize_pp_time(t1);
struct pp_time t = {
.secs=0,
.scaled_nsecs=t2
};
normalize_pp_time(&t);
pp_time_add(t1,&t);
}
/* Substract a TimeInterval to a pp_time */
void pp_time_sub_interval(struct pp_time *t1, TimeInterval t2)
{
t1->scaled_nsecs -= t2;
normalize_pp_time(t1);
struct pp_time t = {
.secs=0,
.scaled_nsecs=t2
};
normalize_pp_time(&t);
pp_time_sub(t1,&t);
}
#define IS_ADD_WILL_OVERFLOW(a,b) \
( (((a > 0) && (b > INT64_MAX - a))) ||\
(((a < 0) && (b < INT64_MIN - a))) \
)
void pp_time_add(struct pp_time *t1, struct pp_time *t2)
{
t1->secs += t2->secs;
......@@ -56,6 +72,11 @@ void pp_time_add(struct pp_time *t1, struct pp_time *t2)
normalize_pp_time(t1);
}
#define IS_SUB_WILL_OVERFLOW(a,b) \
( ((b < 0) && (a > INT64_MAX + b)) ||\
((b > 0) && (a < INT64_MIN + b)) \
)
void pp_time_sub(struct pp_time *t1, struct pp_time *t2)
{
t1->secs -= t2->secs;
......@@ -76,28 +97,28 @@ void pp_time_div2(struct pp_time *t)
int64_t pp_time_to_picos(struct pp_time *ts)
{
return ts->secs * PP_NSEC_PER_SEC
+ ((ts->scaled_nsecs * 1000 + 0x8000) >> TIME_INTERVAL_FRACBITS);
return ts->secs * PP_PSEC_PER_SEC
+ ((ts->scaled_nsecs * 1000 + TIME_ROUNDING_VALUE) >> TIME_FRACBITS);
}
void picos_to_pp_time(int64_t picos, struct pp_time *ts)
{
uint64_t sec, nsec;
int phase;
int sign = (picos < 0 ? -1 : 1);
picos *= sign;
nsec = picos;
phase = __div64_32(&nsec, 1000);
sec = nsec;
sec=picos/PP_PSEC_PER_SEC;
picos-=sec*PP_PSEC_PER_SEC;
nsec = picos/1000;
picos-=nsec*1000;
ts->scaled_nsecs = ((int64_t)__div64_32(&sec, PP_NSEC_PER_SEC)) << TIME_INTERVAL_FRACBITS;
ts->scaled_nsecs += (phase << TIME_INTERVAL_FRACBITS) / 1000;
ts->scaled_nsecs = nsec << TIME_FRACBITS;
ts->scaled_nsecs += (picos << TIME_FRACBITS) / 1000;
ts->scaled_nsecs *= sign;
ts->secs = sec * sign;
}
#if 0
/* "Hardwarizes" the timestamp - e.g. makes the nanosecond field a multiple
* of 8/16ns cycles and puts the extra nanoseconds in the picos result */
void pp_time_hardwarize(struct pp_time *time, int clock_period_ps,
......@@ -105,6 +126,10 @@ void pp_time_hardwarize(struct pp_time *time, int clock_period_ps,
{
int32_t s, ns, ps, clock_ns;
if ( clock_period_ps <= 0 ) {
pp_error("%s : Invalid clock period %d\n",__func__, clock_period_ps);
exit(-1);
}
/* clock_period_ps *must* be a multiple of 1000 -- assert()? */
clock_ns = clock_period_ps / 1000;
......@@ -139,7 +164,26 @@ void pp_time_hardwarize(struct pp_time *time, int clock_period_ps,
*ticks = ns;
*picos = ps;
}
#else
/* "Hardwarizes" the timestamp - e.g. makes the nanosecond field a multiple
* of 8/16ns cycles and puts the extra nanoseconds in the picos result */
void pp_time_hardwarize(struct pp_time *time, int clock_period_ps,
int32_t *ticks, int32_t *picos)
{
int64_t ps, adj_ps;
int32_t sign=(time->scaled_nsecs<0) ? -1 : 1;
int64_t scaled_nsecs=time->scaled_nsecs*sign;
if ( clock_period_ps <= 0 ) {
pp_error("%s : Invalid clock period %d\n",__func__, clock_period_ps);
return;
}
ps = (scaled_nsecs * 1000L+TIME_INTERVAL_ROUNDING_VALUE) >> TIME_INTERVAL_FRACBITS; /* now picoseconds 0..999 -- positive*/
adj_ps=ps - ps%clock_period_ps;
*picos=(int32_t)(ps-adj_ps)*sign;
*ticks = (int32_t)(adj_ps/1000)*sign;
}
#endif
TimeInterval pp_time_to_interval(struct pp_time *ts)
{
......@@ -165,7 +209,7 @@ TimeInterval picos_to_interval(int64_t picos)
int sign = (picos < 0 ? -1 : 1);
picos *= sign;
scaled_ns=(picos/1000) << TIME_INTERVAL_FRACBITS; /* Calculate nanos */
scaled_ns|=((picos%1000) << TIME_INTERVAL_FRACBITS)/1000; /* Add picos */
scaled_ns+=((picos%1000) << TIME_INTERVAL_FRACBITS)/1000; /* Add picos */
return scaled_ns*sign;
}
......@@ -214,3 +258,65 @@ int is_timestamps_incorrect(struct pp_instance *ppsi, int *err_count, int ts_mas
}
return *err_count=0;
}
static char time_as_string[32];
/* Convert pp_time to string */
char *time_to_string(struct pp_time *t)
{
struct pp_time time=*t;
int picos;
char *sign;
if (t->secs < 0 || (t->secs == 0 && t->scaled_nsecs < 0) ) {
sign="-";
time.scaled_nsecs=-t->scaled_nsecs;
time.secs=-t->secs;
} else {
sign="";
time=*t;
}
picos=((int)(time.scaled_nsecs&TIME_FRACMASK)*1000+TIME_ROUNDING_VALUE)>>TIME_FRACBITS;
pp_sprintf(time_as_string, "%s%d.%09d%03d",
sign,
(int)time.secs,
(int)(time.scaled_nsecs >> TIME_FRACBITS),
picos);
return time_as_string;
}
/* Convert TimeInterval to string */
char *interval_to_string(TimeInterval time)
{
int64_t sign,nanos,picos;
if ( time<0 && time !=INT64_MIN) {
sign=-1;
time=-time;
} else {
sign=1;
}
nanos = time >> TIME_INTERVAL_FRACBITS;
picos = (((time & TIME_INTERVAL_FRACMASK) * 1000) + TIME_INTERVAL_ROUNDING_VALUE ) >> TIME_INTERVAL_FRACBITS;
sprintf(time_as_string,"%" PRId64 ".%03" PRId64, sign*nanos,picos);
return time_as_string;
}
/* Convert RelativeInterval to string */
char *relative_interval_to_string(TimeInterval time) {
int32_t nsecs=time >> REL_DIFF_FRACBITS;
uint64_t sub_yocto=0;
int64_t fraction;
uint64_t bitWeight=500000000000000000;
uint64_t mask;
fraction=time & REL_DIFF_FRACMASK;
for (mask=(uint64_t) 1<< (REL_DIFF_FRACBITS-1);mask!=0; mask>>=1 ) {
if ( mask & fraction )
sub_yocto+=bitWeight;
bitWeight/=2;
}
sprintf(time_as_string,"%"PRId32".%018"PRIu64, nsecs, sub_yocto);
return time_as_string;
}
......@@ -101,11 +101,8 @@ typedef struct { /*draft P1588_v_29: page 101 and 340-341 */
/* Add all extension port DS related structure must be store here */
typedef struct {
wrh_portds_head_t head; /* Must be always at the first place */
L1SyncBasicPortDS_t basic;
L1SyncOptParamsPortDS_t opt_params;
Boolean parentExtModeOn;
}l1e_ext_portDS_t;
static inline l1e_ext_portDS_t *L1E_DSPOR(struct pp_instance *ppi)
......@@ -127,13 +124,23 @@ static inline L1SyncOptParamsPortDS_t *L1E_DSPOR_OP(struct pp_instance *ppi)
/****************************************************************************************/
/* l1e_servo interface */
#define L1E_SERVO_RESET_DATA_SIZE (sizeof(struct l1e_servo_state)-offsetof(struct l1e_servo_state,reset_address))
#define L1E_SERVO_RESET_DATA(servo) memset(&servo->reset_address,0,L1E_SERVO_RESET_DATA_SIZE);
struct l1e_servo_state {
/* Values used by snmp. Values are increased at servo update when
* erroneous condition occurs. */
uint32_t n_err_state;
uint32_t n_err_offset;
uint32_t n_err_delta_rtt;
/* ----- All data after this line will cleared during a servo reset */
int reset_address;
/* These fields are used by servo code, after setting at init time */
int32_t clock_period_ps;
/* Following fields are for monitoring/diagnostics (use w/ shmem) */
struct pp_time delayMM;
int64_t delayMM_ps;
int32_t cur_setpoint_ps;
int64_t delayMS_ps;
......@@ -141,15 +148,7 @@ struct l1e_servo_state {
int64_t skew_ps;
int64_t offsetMS_ps;
/* Values used by snmp. Values are increased at servo update when
* erroneous condition occurs. */
uint32_t n_err_state;
uint32_t n_err_offset;
uint32_t n_err_delta_rtt;
struct pp_time update_time;
/* These fields are used by servo code, across iterations */
struct pp_time t1, t2, t3, t4, t5, t6;
int64_t prev_delayMS_ps;
int missed_iters;
};
......@@ -158,9 +157,9 @@ struct l1e_servo_state {
int l1e_servo_init(struct pp_instance *ppi);
void l1e_servo_reset(struct pp_instance *ppi);
void l1e_servo_enable_tracking(int enable);
int l1e_servo_got_sync(struct pp_instance *ppi, struct pp_time *t1,
struct pp_time *t2);
int l1e_servo_got_delay(struct pp_instance *ppi);
int l1e_servo_got_sync(struct pp_instance *ppi);
int l1e_servo_got_resp(struct pp_instance *ppi);
int l1e_servo_got_presp(struct pp_instance *ppi);
int l1e_servo_update(struct pp_instance *ppi);
uint8_t l1e_creat_L1Sync_bitmask(int tx_coh, int rx_coh, int congru);
void l1e_print_L1Sync_basic_bitmaps(struct pp_instance *ppi,
......@@ -180,6 +179,9 @@ static inline struct l1e_servo_state *L1E_SRV(struct pp_instance *ppi)
}
static inline int l1e_get_rx_tmo_ms(L1SyncBasicPortDS_t * bds) {
return (4 << (bds->logL1SyncInterval + 8)) * bds->L1SyncReceiptTimeout;
}
extern struct pp_ext_hooks l1e_ext_hooks;
......
......@@ -19,7 +19,6 @@ char *l1e_state_name[] = {
[L1SYNC_UP] = "L1SYNC_UP",
};
void l1e_print_L1Sync_basic_bitmaps(struct pp_instance *ppi, uint8_t configed,
uint8_t active, char* text)
{
......@@ -100,6 +99,7 @@ static int l1e_init(struct pp_instance *ppi, void *buf, int len)
/* Init configuration members of L1SyncOptParamsPortDS */
L1E_DSPOR_OP(ppi)->timestampsCorrectedTx=TRUE;
ppi->ext_enabled=TRUE;
return 0;
}
......@@ -112,13 +112,16 @@ static int l1e_handle_signaling(struct pp_instance * ppi, void *buf, int len)
if ( l1e_unpack_signal(ppi, buf, len)==0 ) {
/* Valid Sync message */
int timeout_tx_sync;
/* Reset reception timeout */
timeout_tx_sync = (4 << (bds->logL1SyncInterval + 8)) * bds->L1SyncReceiptTimeout;
__pp_timeout_set(ppi, L1E_TIMEOUT_RX_SYNC, timeout_tx_sync);
__pp_timeout_set(ppi, L1E_TIMEOUT_RX_SYNC, l1e_get_rx_tmo_ms(bds));
bds->L1SyncLinkAlive = TRUE;
if ( ppi->link_state==PP_LSTATE_PROTOCOL_DETECTION ||
ppi->link_state==PP_LSTATE_FAILURE) {
ppi->link_state=PP_LSTATE_IN_PROGRESS;
ppi->ext_enabled=TRUE; // Force L1SYNC extension as L1SYNC messages are received
}
}
return 0;
}
......@@ -135,102 +138,64 @@ uint8_t l1e_creat_L1Sync_bitmask(int tx_coh, int rx_coh, int congru)
static int l1e_handle_resp(struct pp_instance *ppi)
{
struct pp_time *ofm = &SRV(ppi)->offsetFromMaster;
l1e_ext_portDS_t *l1epds = L1E_DSPOR(ppi);
pp_diag(ppi, ext, 2, "hook: %s\n", __func__);
if ( l1epds->basic.L1SyncState != L1SYNC_UP )
return 0;
/* This correction_field we received is already part of t4 */
/*
* If no WR mode is on, run normal code, if T2/T3 are valid.
* After we adjusted the pps counter, stamps are invalid, so
* we'll have the Unix time instead, marked by "correct"
*/
if (!l1epds->head.extModeOn) {
if ( is_timestamps_incorrect(ppi, NULL, 0x6 /* mask=t2&t3 */) ) {
pp_diag(ppi, servo, 1,"T2 or T3 incorrect, discarding tuple\n");
return 0;
}
if ( ppi->ext_enabled ) {
l1e_servo_got_resp(ppi);
}
else {
pp_servo_got_resp(ppi);