Commit 8680e32e authored by Cesar Prados's avatar Cesar Prados Committed by Alessandro Rubini

pdelay/servo: breakdown of pp_servo_got_resp, offset to master and pid

the calculation of the offset to the master
clock is also moved to a function. The pid controller
is moved to a function as well.
Signed-off-by: Cesar Prados's avatarC.Prados <c.prados@gsi.de>
parent efe3cf72
......@@ -380,6 +380,11 @@ extern void pp_servo_got_resp(struct pp_instance *ppi); /* got all t1..t4 */
extern void pp_servo_mpd_fltr(struct pp_instance *ppi,
struct pp_avg_fltr *mpd_fltr, TimeInternal * mpd);
extern void pp_servo_offset_master(struct pp_instance *ppi, TimeInternal * mpd,
TimeInternal * ofm,
TimeInternal * m_to_s_dly);
extern Integer32 pp_servo_pi_controller(struct pp_instance *ppi,
TimeInternal * ofm);
/* bmc.c */
extern void m1(struct pp_instance *ppi);
......
......@@ -103,11 +103,6 @@ void pp_servo_got_resp(struct pp_instance *ppi)
TimeInternal *mpd = &DSCUR(ppi)->meanPathDelay;
TimeInternal *ofm = &DSCUR(ppi)->offsetFromMaster;
struct pp_avg_fltr *mpd_fltr = &SRV(ppi)->mpd_fltr;
long long I_term;
long long P_term;
long long tmp;
int I_sign;
int P_sign;
Integer32 adj;
......@@ -145,6 +140,85 @@ void pp_servo_got_resp(struct pp_instance *ppi)
pp_servo_mpd_fltr(ppi, mpd_fltr, mpd);
/* update 'offsetFromMaster', (End to End mode) */
pp_servo_offset_master(ppi, mpd, ofm, m_to_s_dly);
/* PI controller */
adj = pp_servo_pi_controller(ppi, ofm);
/* apply controller output as a clock tick rate adjustment, if
* provided by arch, or as a raw offset otherwise */
if (pp_can_adjust(ppi)) {
if (ppi->t_ops->adjust_freq)
ppi->t_ops->adjust_freq(ppi, -adj);
else
ppi->t_ops->adjust_offset(ppi, -adj);
}
pp_diag(ppi, servo, 2, "Observed drift: %9i\n",
(int)SRV(ppi)->obs_drift >> 10);
}
void pp_servo_mpd_fltr(struct pp_instance *ppi, struct pp_avg_fltr *mpd_fltr,
TimeInternal * mpd)
{
int s;
if (mpd_fltr->s_exp < 1) {
/* First time, keep what we have */
mpd_fltr->y = mpd->nanoseconds;
}
/* avoid overflowing filter */
s = OPTS(ppi)->s;
while (abs(mpd_fltr->y) >> (31 - s))
--s;
if (mpd_fltr->s_exp > 1 << s)
mpd_fltr->s_exp = 1 << s;
/* crank down filter cutoff by increasing 's_exp' */
if (mpd_fltr->s_exp < 1 << s)
++mpd_fltr->s_exp;
/*
* It may happen that mpd appears as negative. This happens when
* the slave clock is running fast to recover a late time: the
* (t3 - t2) measured in the slave appears longer than the (t4 - t1)
* measured in the master. Ignore such values, by keeping the
* current average instead.
*/
if (mpd->nanoseconds < 0)
mpd->nanoseconds = mpd_fltr->y;
if (mpd->nanoseconds < 0)
mpd->nanoseconds = 0;
/*
* It may happen that mpd appears to be very big. This happens
* when we have software timestamps and there is overhead
* involved -- or when the slave clock is running slow. In
* this case use a value just slightly bigger than the current
* average (so if it really got longer, we will adapt). This
* kills most outliers on loaded networks.
* The constant multipliers have been chosed arbitrarily, but
* they work well in testing environment.
*/
if (mpd->nanoseconds > 3 * mpd_fltr->y) {
pp_diag(ppi, servo, 1, "Trim too-long mpd: %i\n",
mpd->nanoseconds);
/* add fltr->s_exp to ensure we are not trapped into 0 */
mpd->nanoseconds = mpd_fltr->y * 2 + mpd_fltr->s_exp + 1;
}
/* filter 'meanPathDelay' (running average) */
mpd_fltr->y = (mpd_fltr->y * (mpd_fltr->s_exp - 1) + mpd->nanoseconds)
/ mpd_fltr->s_exp;
mpd->nanoseconds = mpd_fltr->y;
pp_diag(ppi, servo, 1, "After avg(%i), meanPathDelay: %i\n",
(int)mpd_fltr->s_exp, mpd->nanoseconds);
}
void pp_servo_offset_master(struct pp_instance *ppi, TimeInternal * mpd,
TimeInternal * ofm, TimeInternal * m_to_s_dly)
{
Integer32 adj;
sub_TimeInternal(ofm, m_to_s_dly, mpd);
pp_diag(ppi, servo, 1, "Offset from master: %s\n", fmt_TI(ofm));
......@@ -187,10 +261,16 @@ void pp_servo_got_resp(struct pp_instance *ppi)
}
return; /* ok */
}
}
/*
* What follows is the PI controller
*/
Integer32 pp_servo_pi_controller(struct pp_instance * ppi, TimeInternal * ofm)
{
long long I_term;
long long P_term;
long long tmp;
int I_sign;
int P_sign;
Integer32 adj;
/* the accumulator for the I component, shift by 10 to avoid losing bits
* later in the division */
......@@ -234,71 +314,5 @@ void pp_servo_got_resp(struct pp_instance *ppi)
else
adj = -((-tmp) >> 10);
/* apply controller output as a clock tick rate adjustment, if
* provided by arch, or as a raw offset otherwise */
if (pp_can_adjust(ppi)) {
if (ppi->t_ops->adjust_freq)
ppi->t_ops->adjust_freq(ppi, -adj);
else
ppi->t_ops->adjust_offset(ppi, -adj);
}
pp_diag(ppi, servo, 2, "Observed drift: %9i\n",
(int)SRV(ppi)->obs_drift >> 10);
}
void pp_servo_mpd_fltr(struct pp_instance *ppi, struct pp_avg_fltr *mpd_fltr,
TimeInternal * mpd)
{
int s;
if (mpd_fltr->s_exp < 1) {
/* First time, keep what we have */
mpd_fltr->y = mpd->nanoseconds;
}
/* avoid overflowing filter */
s = OPTS(ppi)->s;
while (abs(mpd_fltr->y) >> (31 - s))
--s;
if (mpd_fltr->s_exp > 1 << s)
mpd_fltr->s_exp = 1 << s;
/* crank down filter cutoff by increasing 's_exp' */
if (mpd_fltr->s_exp < 1 << s)
++mpd_fltr->s_exp;
/*
* It may happen that mpd appears as negative. This happens when
* the slave clock is running fast to recover a late time: the
* (t3 - t2) measured in the slave appears longer than the (t4 - t1)
* measured in the master. Ignore such values, by keeping the
* current average instead.
*/
if (mpd->nanoseconds < 0)
mpd->nanoseconds = mpd_fltr->y;
if (mpd->nanoseconds < 0)
mpd->nanoseconds = 0;
/*
* It may happen that mpd appears to be very big. This happens
* when we have software timestamps and there is overhead
* involved -- or when the slave clock is running slow. In
* this case use a value just slightly bigger than the current
* average (so if it really got longer, we will adapt). This
* kills most outliers on loaded networks.
* The constant multipliers have been chosed arbitrarily, but
* they work well in testing environment.
*/
if (mpd->nanoseconds > 3 * mpd_fltr->y) {
pp_diag(ppi, servo, 1, "Trim too-long mpd: %i\n",
mpd->nanoseconds);
/* add fltr->s_exp to ensure we are not trapped into 0 */
mpd->nanoseconds = mpd_fltr->y * 2 + mpd_fltr->s_exp + 1;
}
/* filter 'meanPathDelay' (running average) */
mpd_fltr->y = (mpd_fltr->y * (mpd_fltr->s_exp - 1) + mpd->nanoseconds)
/ mpd_fltr->s_exp;
mpd->nanoseconds = mpd_fltr->y;
pp_diag(ppi, servo, 1, "After avg(%i), meanPathDelay: %i\n",
(int)mpd_fltr->s_exp, mpd->nanoseconds);
return adj;
}
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