Commit 78277307 authored by Alessandro Rubini's avatar Alessandro Rubini

Merge branch 'wr-servo-cleanup'

parents 3165143f 89613261
...@@ -53,7 +53,7 @@ static DSCurrent currentDS; ...@@ -53,7 +53,7 @@ static DSCurrent currentDS;
static DSParent parentDS; static DSParent parentDS;
static DSTimeProperties timePropertiesDS; static DSTimeProperties timePropertiesDS;
static struct pp_servo servo; static struct pp_servo servo;
static struct wr_servo_state_t servo_state; static struct wr_servo_state servo_state;
static struct wr_dsport wr_dsport = { static struct wr_dsport wr_dsport = {
.ops = &wrpc_wr_operations, .ops = &wrpc_wr_operations,
......
...@@ -161,7 +161,7 @@ int main(int argc, char **argv) ...@@ -161,7 +161,7 @@ int main(int argc, char **argv)
ppg->max_links = PP_MAX_LINKS; ppg->max_links = PP_MAX_LINKS;
ppg->global_ext_data = alloc_fn(ppsi_head, ppg->global_ext_data = alloc_fn(ppsi_head,
sizeof(struct wr_servo_state_t)); sizeof(struct wr_servo_state));
/* NOTE: arch_data is not in shmem */ /* NOTE: arch_data is not in shmem */
ppg->arch_data = malloc( sizeof(struct unix_arch_data)); ppg->arch_data = malloc( sizeof(struct unix_arch_data));
ppg->pp_instances = alloc_fn(ppsi_head, ppg->pp_instances = alloc_fn(ppsi_head,
......
...@@ -12,8 +12,7 @@ ...@@ -12,8 +12,7 @@
#include <ppsi/lib.h> #include <ppsi/lib.h>
#include "wr-constants.h" #include "wr-constants.h"
#define WRS_PPSI_SHMEM_VERSION 3 /* added fields to pp_globals and #define WRS_PPSI_SHMEM_VERSION 4 /* several changes to wr_servo_state */
wr_servo_state_t */
/* /*
* This structure is used as extension-specific data in the DSPort * This structure is used as extension-specific data in the DSPort
...@@ -132,26 +131,33 @@ int wr_servo_got_sync(struct pp_instance *ppi, TimeInternal *t1, ...@@ -132,26 +131,33 @@ int wr_servo_got_sync(struct pp_instance *ppi, TimeInternal *t1,
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);
struct wr_servo_state_t { struct wr_servo_state {
char if_name[16]; char if_name[16]; /* Informative, for wr_mon through shmem */
unsigned long flags;
#define WR_FLAG_VALID 1
#define WR_FLAG_WAIT_HW 2
int state; int state;
int next_state;
TimeInternal mu; /* half of the RTT */ /* These fields are used by servo code, after asetting at init time */
int64_t picos_mu;
int32_t delta_tx_m; int32_t delta_tx_m;
int32_t delta_rx_m; int32_t delta_rx_m;
int32_t delta_tx_s; int32_t delta_tx_s;
int32_t delta_rx_s; int32_t delta_rx_s;
int32_t cur_setpoint;
int64_t delta_ms;
int64_t delta_ms_prev;
TimeInternal t1, t2, t3, t4;
uint64_t last_tics;
int32_t fiber_fix_alpha; int32_t fiber_fix_alpha;
int32_t clock_period_ps; int32_t clock_period_ps;
/* These fields are used by servo code, across iterations */
TimeInternal t1, t2, t3, t4;
int64_t delta_ms_prev;
int missed_iters; int missed_iters;
int valid; /* Following fields are for monitoring/diagnostics (use w/ shmem) */
TimeInternal mu;
int64_t picos_mu;
int32_t cur_setpoint;
int64_t delta_ms;
uint32_t update_count; uint32_t update_count;
int tracking_enabled; int tracking_enabled;
char servo_state_name[32]; char servo_state_name[32];
...@@ -160,8 +166,8 @@ struct wr_servo_state_t { ...@@ -160,8 +166,8 @@ struct wr_servo_state_t {
}; };
/* All data used as extension ppsi-wr must be put here */ /* All data used as extension ppsi-wr must be put here */
struct wr_data_t { struct wr_data {
struct wr_servo_state_t servo_state; struct wr_servo_state servo_state;
}; };
#endif /* __WREXT_WR_API_H__ */ #endif /* __WREXT_WR_API_H__ */
...@@ -7,8 +7,7 @@ ...@@ -7,8 +7,7 @@
#define WR_SYNC_TAI 2 #define WR_SYNC_TAI 2
#define WR_SYNC_PHASE 3 #define WR_SYNC_PHASE 3
#define WR_TRACK_PHASE 4 #define WR_TRACK_PHASE 4
#define WR_WAIT_SYNC_IDLE 5 #define WR_WAIT_OFFSET_STABLE 5
#define WR_WAIT_OFFSET_STABLE 6
#define WR_SERVO_OFFSET_STABILITY_THRESHOLD 60 /* psec */ #define WR_SERVO_OFFSET_STABILITY_THRESHOLD 60 /* psec */
...@@ -20,13 +19,12 @@ static const char *servo_name[] = { ...@@ -20,13 +19,12 @@ static const char *servo_name[] = {
[WR_SYNC_TAI] = "SYNC_SEC", [WR_SYNC_TAI] = "SYNC_SEC",
[WR_SYNC_PHASE] = "SYNC_PHASE", [WR_SYNC_PHASE] = "SYNC_PHASE",
[WR_TRACK_PHASE] = "TRACK_PHASE", [WR_TRACK_PHASE] = "TRACK_PHASE",
[WR_WAIT_SYNC_IDLE] = "WAIT_SYNC_IDLE",
[WR_WAIT_OFFSET_STABLE] = "WAIT_OFFSET_STABLE", [WR_WAIT_OFFSET_STABLE] = "WAIT_OFFSET_STABLE",
}; };
static int tracking_enabled = 1; /* FIXME: why? */ static int tracking_enabled = 1; /* FIXME: why? */
extern struct wrs_shm_head *ppsi_head; extern struct wrs_shm_head *ppsi_head;
static struct wr_servo_state_t *saved_servo_pointer; /* required for static struct wr_servo_state *saved_servo_pointer; /* required for
* wr_servo_reset, which doesn't * wr_servo_reset, which doesn't
* have ppi context. */ * have ppi context. */
...@@ -145,14 +143,21 @@ static int got_sync = 0; ...@@ -145,14 +143,21 @@ static int got_sync = 0;
void wr_servo_reset(void) void wr_servo_reset(void)
{ {
if (saved_servo_pointer) if (saved_servo_pointer)
saved_servo_pointer->valid = 0; saved_servo_pointer->flags = 0;
}
static inline int32_t delta_to_ps(struct FixedDelta d)
{
UInteger64 *sps = &d.scaledPicoseconds; /* ieee type :( */
return (sps->lsb >> 16) | (sps->msb << 16);
} }
int wr_servo_init(struct pp_instance *ppi) int wr_servo_init(struct pp_instance *ppi)
{ {
struct wr_dsport *wrp = WR_DSPOR(ppi); struct wr_dsport *wrp = WR_DSPOR(ppi);
struct wr_servo_state_t *s = struct wr_servo_state *s =
&((struct wr_data_t *)ppi->ext_data)->servo_state; &((struct wr_data *)ppi->ext_data)->servo_state;
/* shmem lock */ /* shmem lock */
wrs_shm_write(ppsi_head, WRS_SHM_WRITE_BEGIN); wrs_shm_write(ppsi_head, WRS_SHM_WRITE_BEGIN);
/* Determine the alpha coefficient */ /* Determine the alpha coefficient */
...@@ -162,29 +167,20 @@ int wr_servo_init(struct pp_instance *ppi) ...@@ -162,29 +167,20 @@ int wr_servo_init(struct pp_instance *ppi)
wrp->ops->enable_timing_output(ppi, 0); wrp->ops->enable_timing_output(ppi, 0);
/* FIXME useful? strncpy(s->if_name, ppi->cfg.iface_name, sizeof(s->if_name));
strncpy(s->if_name, clock->netPath.ifaceName, 16);
*/
s->state = WR_SERVO_NONE; /* Turned into SYNC_TAI at 1st iteration */ s->state = WR_SERVO_NONE; /* Turned into SYNC_TAI at 1st iteration */
s->cur_setpoint = 0; s->cur_setpoint = 0;
s->missed_iters = 0; s->missed_iters = 0;
s->delta_tx_m = ((((int32_t)WR_DSPOR(ppi)->otherNodeDeltaTx.scaledPicoseconds.lsb) >> 16) & 0xffff) | (((int32_t)WR_DSPOR(ppi)->otherNodeDeltaTx.scaledPicoseconds.msb) << 16); s->delta_tx_m = delta_to_ps(wrp->otherNodeDeltaTx);
s->delta_rx_m = ((((int32_t)WR_DSPOR(ppi)->otherNodeDeltaRx.scaledPicoseconds.lsb) >> 16) & 0xffff) | (((int32_t)WR_DSPOR(ppi)->otherNodeDeltaRx.scaledPicoseconds.msb) << 16); s->delta_rx_m = delta_to_ps(wrp->otherNodeDeltaRx);
s->delta_tx_s = delta_to_ps(wrp->deltaTx);
s->delta_tx_s = ((((int32_t)WR_DSPOR(ppi)->deltaTx.scaledPicoseconds.lsb) >> 16) & 0xffff) | (((int32_t)WR_DSPOR(ppi)->deltaTx.scaledPicoseconds.msb) << 16); s->delta_rx_s = delta_to_ps(wrp->deltaRx);
s->delta_rx_s = ((((int32_t)WR_DSPOR(ppi)->deltaRx.scaledPicoseconds.lsb) >> 16) & 0xffff) | (((int32_t)WR_DSPOR(ppi)->deltaRx.scaledPicoseconds.msb) << 16);
/* FIXME: useful?
strncpy(cur_servo_state.sync_source,
clock->netPath.ifaceName, 16);//fixme
*/
strcpy(s->servo_state_name, "Uninitialized"); strcpy(s->servo_state_name, "Uninitialized");
saved_servo_pointer = s; saved_servo_pointer = s;
saved_servo_pointer->valid = 1; saved_servo_pointer->flags |= WR_FLAG_VALID;
s->update_count = 0; s->update_count = 0;
got_sync = 0; got_sync = 0;
...@@ -205,8 +201,8 @@ int wr_servo_man_adjust_phase(int phase) ...@@ -205,8 +201,8 @@ int wr_servo_man_adjust_phase(int phase)
int wr_servo_got_sync(struct pp_instance *ppi, TimeInternal *t1, int wr_servo_got_sync(struct pp_instance *ppi, TimeInternal *t1,
TimeInternal *t2) TimeInternal *t2)
{ {
struct wr_servo_state_t *s = struct wr_servo_state *s =
&((struct wr_data_t *)ppi->ext_data)->servo_state; &((struct wr_data *)ppi->ext_data)->servo_state;
s->t1 = *t1; s->t1 = *t1;
s->t1.correct = 1; s->t1.correct = 1;
...@@ -219,8 +215,8 @@ int wr_servo_got_sync(struct pp_instance *ppi, TimeInternal *t1, ...@@ -219,8 +215,8 @@ int wr_servo_got_sync(struct pp_instance *ppi, TimeInternal *t1,
int wr_servo_got_delay(struct pp_instance *ppi, Integer32 cf) int wr_servo_got_delay(struct pp_instance *ppi, Integer32 cf)
{ {
struct wr_servo_state_t *s = struct wr_servo_state *s =
&((struct wr_data_t *)ppi->ext_data)->servo_state; &((struct wr_data *)ppi->ext_data)->servo_state;
s->t3 = ppi->t3; s->t3 = ppi->t3;
/* s->t3.phase = 0; */ /* s->t3.phase = 0; */
...@@ -235,14 +231,13 @@ int wr_servo_got_delay(struct pp_instance *ppi, Integer32 cf) ...@@ -235,14 +231,13 @@ 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)
{ {
struct wr_dsport *wrp = WR_DSPOR(ppi); struct wr_dsport *wrp = WR_DSPOR(ppi);
struct wr_servo_state_t *s = struct wr_servo_state *s =
&((struct wr_data_t *)ppi->ext_data)->servo_state; &((struct wr_data *)ppi->ext_data)->servo_state;
uint64_t tics;
uint64_t big_delta_fix; uint64_t big_delta_fix;
uint64_t delay_ms_fix; uint64_t delay_ms_fix;
static int errcount; static int errcount;
int i, remaining_offset; int remaining_offset;
TimeInternal ts_offset, ts_offset_hw /*, ts_phase_adjust */; TimeInternal ts_offset, ts_offset_hw /*, ts_phase_adjust */;
...@@ -282,6 +277,11 @@ int wr_servo_update(struct pp_instance *ppi) ...@@ -282,6 +277,11 @@ int wr_servo_update(struct pp_instance *ppi)
big_delta_fix = s->delta_tx_m + s->delta_tx_s big_delta_fix = s->delta_tx_m + s->delta_tx_s
+ s->delta_rx_m + s->delta_rx_s; + s->delta_rx_m + s->delta_rx_s;
if ((signed)s->picos_mu < (signed)big_delta_fix) {
/* avoid negatives in calculations */
s->picos_mu = big_delta_fix;
}
delay_ms_fix = (((int64_t)(s->picos_mu - big_delta_fix) * (int64_t) s->fiber_fix_alpha) >> FIX_ALPHA_FRACBITS) delay_ms_fix = (((int64_t)(s->picos_mu - big_delta_fix) * (int64_t) s->fiber_fix_alpha) >> FIX_ALPHA_FRACBITS)
+ ((s->picos_mu - big_delta_fix) >> 1) + ((s->picos_mu - big_delta_fix) >> 1)
+ s->delta_tx_m + s->delta_rx_s + ph_adjust; + s->delta_tx_m + s->delta_rx_s + ph_adjust;
...@@ -297,8 +297,6 @@ int wr_servo_update(struct pp_instance *ppi) ...@@ -297,8 +297,6 @@ int wr_servo_update(struct pp_instance *ppi)
s->delta_ms = delay_ms_fix; s->delta_ms = delay_ms_fix;
tics = ppi->t_ops->calc_timeout(ppi, 0);
if (wrp->ops->locking_poll(ppi, 0) != WR_SPLL_READY) { if (wrp->ops->locking_poll(ppi, 0) != WR_SPLL_READY) {
pp_diag(ppi, servo, 1, "PLL OutOfLock, should restart sync\n"); pp_diag(ppi, servo, 1, "PLL OutOfLock, should restart sync\n");
wrp->ops->enable_timing_output(ppi, 0); wrp->ops->enable_timing_output(ppi, 0);
...@@ -318,34 +316,34 @@ int wr_servo_update(struct pp_instance *ppi) ...@@ -318,34 +316,34 @@ int wr_servo_update(struct pp_instance *ppi)
pp_diag(ppi, servo, 2, "offset_hw: %li.%09li\n", pp_diag(ppi, servo, 2, "offset_hw: %li.%09li\n",
(long)ts_offset_hw.seconds, (long)ts_offset_hw.nanoseconds); (long)ts_offset_hw.seconds, (long)ts_offset_hw.nanoseconds);
pp_diag(ppi, servo, 1, "wr_servo state: %s %s\n", pp_diag(ppi, servo, 1, "wr_servo state: %s%s\n",
servo_name[s->state], s->state == WR_WAIT_SYNC_IDLE ? servo_name[s->state],
servo_name[s->next_state] : ""); s->flags & WR_FLAG_WAIT_HW ? " (wait for hw)" : "");
switch (s->state) { /*
case WR_WAIT_SYNC_IDLE: * After each action on the hardware, we must verify if it is over.
if (!wrp->ops->adjust_in_progress()) { * However, we loose one iteration every two. To be audited later.
s->state = s->next_state; */
} else { if (s->flags & WR_FLAG_WAIT_HW) {
if (!wrp->ops->adjust_in_progress())
s->flags &= ~WR_FLAG_WAIT_HW;
else
pp_diag(ppi, servo, 1, "servo:busy\n"); pp_diag(ppi, servo, 1, "servo:busy\n");
} goto out;
break; }
switch (s->state) {
case WR_SYNC_TAI: case WR_SYNC_TAI:
wrp->ops->adjust_counters(ts_offset_hw.seconds, 0); wrp->ops->adjust_counters(ts_offset_hw.seconds, 0);
wrp->ops->adjust_phase(0); wrp->ops->adjust_phase(0);
s->flags |= WR_FLAG_WAIT_HW;
s->next_state = WR_SYNC_NSEC; s->state = WR_SYNC_NSEC;
s->state = WR_WAIT_SYNC_IDLE;
s->last_tics = tics;
break; break;
case WR_SYNC_NSEC: case WR_SYNC_NSEC:
wrp->ops->adjust_counters(0, ts_offset_hw.nanoseconds); wrp->ops->adjust_counters(0, ts_offset_hw.nanoseconds);
s->flags |= WR_FLAG_WAIT_HW;
s->next_state = WR_SYNC_NSEC; s->state = WR_SYNC_PHASE;
s->state = WR_WAIT_SYNC_IDLE;
s->last_tics = tics;
break; break;
case WR_SYNC_PHASE: case WR_SYNC_PHASE:
...@@ -354,9 +352,8 @@ int wr_servo_update(struct pp_instance *ppi) ...@@ -354,9 +352,8 @@ int wr_servo_update(struct pp_instance *ppi)
wrp->ops->adjust_phase(s->cur_setpoint); wrp->ops->adjust_phase(s->cur_setpoint);
s->next_state = WR_WAIT_OFFSET_STABLE; s->flags |= WR_FLAG_WAIT_HW;
s->state = WR_WAIT_SYNC_IDLE; s->state = WR_WAIT_OFFSET_STABLE;
s->last_tics = tics;
s->delta_ms_prev = s->delta_ms; s->delta_ms_prev = s->delta_ms;
break; break;
...@@ -374,15 +371,19 @@ int wr_servo_update(struct pp_instance *ppi) ...@@ -374,15 +371,19 @@ int wr_servo_update(struct pp_instance *ppi)
} else { } else {
s->missed_iters++; s->missed_iters++;
} }
if (s->missed_iters >= 10) if (s->missed_iters >= 10) {
s->missed_iters = 0;
s->state = WR_SYNC_TAI; s->state = WR_SYNC_TAI;
}
break; break;
case WR_TRACK_PHASE: case WR_TRACK_PHASE:
s->skew = s->delta_ms - s->delta_ms_prev; s->skew = s->delta_ms - s->delta_ms_prev;
if (ts_offset_hw.seconds !=0 || ts_offset_hw.nanoseconds != 0) if (ts_offset_hw.seconds || ts_offset_hw.nanoseconds) {
s->state = WR_SYNC_TAI; s->state = WR_SYNC_TAI;
break;
}
if(tracking_enabled) { if(tracking_enabled) {
// just follow the changes of deltaMS // just follow the changes of deltaMS
...@@ -393,20 +394,16 @@ int wr_servo_update(struct pp_instance *ppi) ...@@ -393,20 +394,16 @@ int wr_servo_update(struct pp_instance *ppi)
s->cur_setpoint); s->cur_setpoint);
s->delta_ms_prev = s->delta_ms; s->delta_ms_prev = s->delta_ms;
s->next_state = WR_TRACK_PHASE; s->flags |= WR_FLAG_WAIT_HW;
s->state = WR_WAIT_SYNC_IDLE; s->state = WR_TRACK_PHASE;
s->last_tics = tics;
} }
break; break;
} }
/* update string state name */ /* update string state name */
if (s->state == WR_WAIT_SYNC_IDLE) strcpy(s->servo_state_name, servo_name[s->state]);
i = s->next_state;
else
i = s->state;
strcpy(s->servo_state_name, servo_name[i]);
out:
/* shmem unlock */ /* shmem unlock */
wrs_shm_write(ppsi_head, WRS_SHM_WRITE_END); wrs_shm_write(ppsi_head, WRS_SHM_WRITE_END);
return 0; return 0;
......
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