servo.c 10.7 KB
Newer Older
1
/*
2 3
 * Copyright (C) 2011 CERN (www.cern.ch)
 * Author: Aurelio Colosimo
4
 * Based on PTPd project v. 2.1.0 (see AUTHORS for details)
5 6
 *
 * Released according to the GNU LGPL, version 2.1 or any later version.
7 8
 */

Alessandro Rubini's avatar
Alessandro Rubini committed
9
#include <ppsi/ppsi.h>
10

11 12
static void pp_servo_mpd_fltr(struct pp_instance *, struct pp_avg_fltr *,
			      TimeInternal *);
13
static int pp_servo_offset_master(struct pp_instance *, TimeInternal *,
14 15 16 17
				   TimeInternal *, TimeInternal *);
static Integer32 pp_servo_pi_controller(struct pp_instance *, TimeInternal *);


18
void pp_servo_init(struct pp_instance *ppi)
19
{
20
	int d;
21

22
	SRV(ppi)->mpd_fltr.s_exp = 0;	/* clears meanPathDelay filter */
23 24
	ppi->frgn_rec_num = 0;		/* no known master */
	DSPAR(ppi)->parentPortIdentity.portNumber = 0; /* invalid */
25 26 27 28 29 30 31 32

	if (ppi->t_ops->init_servo) {
		/* The system may pre-set us to keep current frequency */
		d = ppi->t_ops->init_servo(ppi);
		if (d == -1) {
			pp_diag(ppi, servo, 1, "error in t_ops->servo_init");
			d = 0;
		}
33
		SRV(ppi)->obs_drift = -d << 10; /* note "-" */
34 35
	} else {
		/* level clock */
36
		if (pp_can_adjust(ppi))
37 38 39 40
			ppi->t_ops->adjust(ppi, 0, 0);
		SRV(ppi)->obs_drift = 0;
	}

41
	pp_diag(ppi, servo, 1, "Initialized: obs_drift %lli\n",
42
		SRV(ppi)->obs_drift);
43 44
}

45
/* internal helper, returning static storage to be used immediately */
46
static char *fmt_TI(TimeInternal *t)
47
{
48 49
	static char s[24];

50 51 52 53
	pp_sprintf(s, "%s%d.%09d",
		(t->seconds < 0 || (t->seconds == 0 && t->nanoseconds < 0))
		   ? "-" : " ",
		   (int)abs(t->seconds), (int)abs(t->nanoseconds));
54
	return s;
55 56
}

57 58
/* Called by slave and uncalib when we have t1 and t2 */
void pp_servo_got_sync(struct pp_instance *ppi)
59
{
60
	TimeInternal *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
61

62 63 64
	/*
	 * calc 'master_to_slave_delay', removing the correction field
	 * added by transparent clocks in the path.
65 66
	 * cField contains the sum of sync and followup messages
	 * correctionField(s)
67
	 */
68 69
	sub_TimeInternal(m_to_s_dly, &ppi->t2, &ppi->t1);
	sub_TimeInternal(m_to_s_dly, m_to_s_dly, &ppi->cField);
70 71
	pp_diag(ppi, servo, 3, "correction field 1: %s\n",
		fmt_TI(&ppi->cField));
72 73
}

74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93
/* Called by slave and uncalib when we have t1 and t2 */
void pp_servo_got_psync(struct pp_instance *ppi)
{
	TimeInternal *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
	TimeInternal *mpd = &DSCUR(ppi)->meanPathDelay;
	TimeInternal *ofm = &DSCUR(ppi)->offsetFromMaster;
	Integer32 adj;

	pp_diag(ppi, servo, 2, "T1: %s\n", fmt_TI(&ppi->t1));
	pp_diag(ppi, servo, 2, "T2: %s\n", fmt_TI(&ppi->t2));

	/*
	 * calc 'master_to_slave_delay', removing the correction field
	 * added by transparent clocks in the path.
	 */
	sub_TimeInternal(m_to_s_dly, &ppi->t2, &ppi->t1);
	sub_TimeInternal(m_to_s_dly, m_to_s_dly, &ppi->cField);
	pp_diag(ppi, servo, 3, "correction field 1: %s\n",
		fmt_TI(&ppi->cField));

94 95 96
	/* update 'offsetFromMaster' and possibly jump in time */
	if (pp_servo_offset_master(ppi, mpd, ofm, m_to_s_dly))
		return;
97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113

	/* 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);
}

114 115 116
/* called by slave states when delay_resp is received (all t1..t4 are valid) */
void pp_servo_got_resp(struct pp_instance *ppi)
{
117 118
	TimeInternal *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
	TimeInternal *s_to_m_dly = &SRV(ppi)->s_to_m_dly;
119
	TimeInternal *mpd = &DSCUR(ppi)->meanPathDelay;
120
	TimeInternal *ofm = &DSCUR(ppi)->offsetFromMaster;
121
	struct pp_avg_fltr *mpd_fltr = &SRV(ppi)->mpd_fltr;
122 123
	Integer32 adj;

124 125 126 127 128 129

	/* We sometimes enter here before we got sync/f-up */
	if (ppi->t1.seconds == 0 && ppi->t1.nanoseconds == 0) {
		pp_diag(ppi, servo, 2, "discard T3/T4: we miss T1/T2\n");
		return;
	}
130
	/*
131
	 * calc 'slave_to_master_delay', removing delay_resp correction field
132 133
	 * added by transparent clocks in the path.
	 */
134 135
	sub_TimeInternal(s_to_m_dly, &ppi->t4,	&ppi->t3);
	sub_TimeInternal(s_to_m_dly, s_to_m_dly, &ppi->cField);
136 137 138 139 140 141 142 143 144
	pp_diag(ppi, servo, 3, "correction field 2: %s\n",
		fmt_TI(&ppi->cField));

	pp_diag(ppi, servo, 2, "T1: %s\n", fmt_TI(&ppi->t1));
	pp_diag(ppi, servo, 2, "T2: %s\n", fmt_TI(&ppi->t2));
	pp_diag(ppi, servo, 2, "T3: %s\n", fmt_TI(&ppi->t3));
	pp_diag(ppi, servo, 2, "T4: %s\n", fmt_TI(&ppi->t4));
	pp_diag(ppi, servo, 1, "Master to slave: %s\n", fmt_TI(m_to_s_dly));
	pp_diag(ppi, servo, 1, "Slave to master: %s\n", fmt_TI(s_to_m_dly));
145

146
	/* Calc mean path delay, used later to calc "offset from master" */
147 148
	add_TimeInternal(mpd, &SRV(ppi)->m_to_s_dly, &SRV(ppi)->s_to_m_dly);
	div2_TimeInternal(mpd);
149
	pp_diag(ppi, servo, 1, "meanPathDelay: %s\n", fmt_TI(mpd));
150

151
	if (mpd->seconds) /* Hmm.... we called this "bad event" */
152
		return;
153

154 155
	/* mean path delay filtering */
	pp_servo_mpd_fltr(ppi, mpd_fltr, mpd);
156

157 158 159
	/* update 'offsetFromMaster' and possibly jump in time */
	if (pp_servo_offset_master(ppi, mpd, ofm, m_to_s_dly))
		return;
160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176

	/* 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);
}

177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207
/* called by slave states when delay_resp is received (all t1..t4 are valid) */
void pp_servo_got_presp(struct pp_instance *ppi)
{
	TimeInternal *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
	TimeInternal *s_to_m_dly = &SRV(ppi)->s_to_m_dly;
	TimeInternal *mpd = &DSCUR(ppi)->meanPathDelay;
	struct pp_avg_fltr *mpd_fltr = &SRV(ppi)->mpd_fltr;

	/*
	 * calc 'slave_to_master_delay', removing the correction field
	 * added by transparent clocks in the path.
	 */
	sub_TimeInternal(s_to_m_dly, &ppi->t6, &ppi->t5);
	sub_TimeInternal(s_to_m_dly, s_to_m_dly, &ppi->cField);
	pp_diag(ppi, servo, 3, "correction field 2: %s\n",
		fmt_TI(&ppi->cField));

	sub_TimeInternal(m_to_s_dly, &ppi->t4, &ppi->t3);

	pp_diag(ppi, servo, 2, "T3: %s\n", fmt_TI(&ppi->t3));
	pp_diag(ppi, servo, 2, "T4: %s\n", fmt_TI(&ppi->t4));
	pp_diag(ppi, servo, 2, "T5: %s\n", fmt_TI(&ppi->t5));
	pp_diag(ppi, servo, 2, "T6: %s\n", fmt_TI(&ppi->t6));
	pp_diag(ppi, servo, 1, "Master to slave: %s\n", fmt_TI(m_to_s_dly));
	pp_diag(ppi, servo, 1, "Slave to master: %s\n", fmt_TI(s_to_m_dly));

	/* Calc mean path delay, used later to calc "offset from master" */
	add_TimeInternal(mpd, &SRV(ppi)->m_to_s_dly, &SRV(ppi)->s_to_m_dly);
	div2_TimeInternal(mpd);
	pp_diag(ppi, servo, 1, "meanPathDelay: %s\n", fmt_TI(mpd));

208
	if (mpd->seconds) /* Hmm.... we called this "bad event" */
209 210 211 212 213
		return;

	pp_servo_mpd_fltr(ppi, mpd_fltr, mpd);
}

214
static
215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270
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);
}

271
static
272
int pp_servo_offset_master(struct pp_instance *ppi, TimeInternal * mpd,
273 274
			    TimeInternal * ofm, TimeInternal * m_to_s_dly)
{
275
	TimeInternal time_tmp;
276

277
	sub_TimeInternal(ofm, m_to_s_dly, mpd);
278
	pp_diag(ppi, servo, 1, "Offset from master:     %s\n", fmt_TI(ofm));
279

280 281
	if (!ofm->seconds)
		return 0; /* proceeed with adjust */
282

283 284
	if (!pp_can_adjust(ppi))
		return 0; /* e.g., a loopback test run... "-t" on cmdline */
285

286 287 288 289 290
	ppi->t_ops->get(ppi, &time_tmp);
	sub_TimeInternal(&time_tmp, &time_tmp, ofm);
	ppi->t_ops->set(ppi, &time_tmp);
	pp_servo_init(ppi);
	return 1; /* done */
291
}
292

293
static
294 295 296 297 298 299 300 301
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;
302

303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343
	/* the accumulator for the I component, shift by 10 to avoid losing bits
	 * later in the division */
	SRV(ppi)->obs_drift += ((long long)ofm->nanoseconds) << 10;

	/* Anti-windup. The PP_ADJ_FREQ_MAX value is multiplied by OPTS(ppi)->ai
	 * (which is the reciprocal of the integral gain of the controller).
	 * Then it's scaled by 10 bits to match the bit shift used earlier to
	 * avoid bit losses */
	tmp = (((long long)PP_ADJ_FREQ_MAX) * OPTS(ppi)->ai) << 10;
	if (SRV(ppi)->obs_drift > tmp)
		SRV(ppi)->obs_drift = tmp;
	else if (SRV(ppi)->obs_drift < -tmp)
		SRV(ppi)->obs_drift = -tmp;

	/* calculation of the I component, based on obs_drift */
	I_sign = (SRV(ppi)->obs_drift > 0) ? 0 : -1;
	I_term = SRV(ppi)->obs_drift;
	if (I_sign)
		I_term = -I_term;
	__div64_32((uint64_t *)&I_term, OPTS(ppi)->ai);
	if (I_sign)
		I_term = -I_term;

	/* calculation of the P component */
	P_sign = (ofm->nanoseconds > 0) ? 0 : -1;
	/* shift 10 bits again to avoid losses */
	P_term = ((long long)ofm->nanoseconds) << 10;
	if (P_sign)
		P_term = -P_term;
	__div64_32((uint64_t *)&P_term, OPTS(ppi)->ap);
	if (P_sign)
		P_term = -P_term;

	/* calculate the correction of applied by the controller */
	tmp = P_term + I_term;
	/* Now scale it before passing the argument to adjtimex
	 * Be careful with the signs */
	if (tmp > 0)
		adj = (tmp >> 10);
	else
		adj = -((-tmp) >> 10);
344

345
	return adj;
346
}