servo.c 10.3 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
static void pp_servo_mpd_fltr(struct pp_instance *, struct pp_avg_fltr *,
12 13 14 15
			      struct pp_time *);
static int pp_servo_offset_master(struct pp_instance *, struct pp_time *,
				   struct pp_time *, struct pp_time *);
static int64_t pp_servo_pi_controller(struct pp_instance *, struct pp_time *);
16 17


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_timeout_set(ppi, PP_TO_FAULT);
42
	pp_diag(ppi, servo, 1, "Initialized: obs_drift %lli\n",
43
		SRV(ppi)->obs_drift);
44 45
}

46
/* internal helper, returning static storage to be used immediately */
47
static char *fmt_ppt(struct pp_time *t)
48
{
49 50
	static char s[24];

51
	pp_sprintf(s, "%s%d.%09d",
52
		   (t->secs < 0 || (t->secs == 0 && t->scaled_nsecs < 0))
53
		   ? "-" : " ",
54 55
		   /* FIXME: this is wrong for some of the negatives */
		   (int)abs(t->secs), (int)abs(t->scaled_nsecs >> 16));
56
	return s;
57 58
}

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

64
	/*
65 66
	 * calc 'master_to_slave_delay'; no correction field
	 * appears in the formulas because it's already merged with t1
67
	 */
68 69
	*m_to_s_dly = ppi->t2;
	pp_time_sub(m_to_s_dly, &ppi->t1);
70 71
}

72 73 74
/* Called by slave and uncalib when we have t1 and t2 */
void pp_servo_got_psync(struct pp_instance *ppi)
{
75 76 77 78
	struct pp_time *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
	struct pp_time *mpd = &DSCUR(ppi)->meanPathDelay;
	struct pp_time *ofm = &DSCUR(ppi)->offsetFromMaster;
	int adj32;
79

80 81
	pp_diag(ppi, servo, 2, "T1: %s\n", fmt_ppt(&ppi->t1));
	pp_diag(ppi, servo, 2, "T2: %s\n", fmt_ppt(&ppi->t2));
82 83

	/*
84 85
	 * calc 'master_to_slave_delay'; no correction field
	 * appears in the formulas because it's already merged with t1
86
	 */
87 88
	*m_to_s_dly = ppi->t2;
	pp_time_sub(m_to_s_dly, &ppi->t1);
89

90 91 92
	/* update 'offsetFromMaster' and possibly jump in time */
	if (pp_servo_offset_master(ppi, mpd, ofm, m_to_s_dly))
		return;
93

94 95
	/* PI controller returns a scaled_nsecs adjustment, so shift back */
	adj32 = (int)(pp_servo_pi_controller(ppi, ofm) >> 16);
96 97 98 99 100

	/* 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)
101
			ppi->t_ops->adjust_freq(ppi, -adj32);
102
		else
103
			ppi->t_ops->adjust_offset(ppi, -adj32);
104 105 106 107 108 109
	}

	pp_diag(ppi, servo, 2, "Observed drift: %9i\n",
		(int)SRV(ppi)->obs_drift >> 10);
}

110 111 112
/* called by slave states when delay_resp is received (all t1..t4 are valid) */
void pp_servo_got_resp(struct pp_instance *ppi)
{
113 114 115 116
	struct pp_time *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
	struct pp_time *s_to_m_dly = &SRV(ppi)->s_to_m_dly;
	struct pp_time *mpd = &DSCUR(ppi)->meanPathDelay;
	struct pp_time *ofm = &DSCUR(ppi)->offsetFromMaster;
117
	struct pp_avg_fltr *mpd_fltr = &SRV(ppi)->mpd_fltr;
118
	int adj32;
119

120 121

	/* We sometimes enter here before we got sync/f-up */
122
	if (ppi->t1.secs == 0 && ppi->t1.scaled_nsecs == 0) {
123 124 125
		pp_diag(ppi, servo, 2, "discard T3/T4: we miss T1/T2\n");
		return;
	}
126
	/*
127
	 * calc 'slave_to_master_delay', removing delay_resp correction field
128 129
	 * added by transparent clocks in the path.
	 */
130 131
	*s_to_m_dly = ppi->t4;
	pp_time_sub(s_to_m_dly, &ppi->t3);
132

133 134 135 136 137 138
	pp_diag(ppi, servo, 2, "T1: %s\n", fmt_ppt(&ppi->t1));
	pp_diag(ppi, servo, 2, "T2: %s\n", fmt_ppt(&ppi->t2));
	pp_diag(ppi, servo, 2, "T3: %s\n", fmt_ppt(&ppi->t3));
	pp_diag(ppi, servo, 2, "T4: %s\n", fmt_ppt(&ppi->t4));
	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_ppt(s_to_m_dly));
139

140
	/* Calc mean path delay, used later to calc "offset from master" */
141 142 143 144
	*mpd = SRV(ppi)->m_to_s_dly;
	pp_time_add(mpd, &SRV(ppi)->s_to_m_dly);
	pp_time_div2(mpd);
	pp_diag(ppi, servo, 1, "meanPathDelay: %s\n", fmt_ppt(mpd));
145

146
	if (mpd->secs) /* Hmm.... we called this "bad event" */
147
		return;
148

149 150
	/* mean path delay filtering */
	pp_servo_mpd_fltr(ppi, mpd_fltr, mpd);
151

152 153 154
	/* update 'offsetFromMaster' and possibly jump in time */
	if (pp_servo_offset_master(ppi, mpd, ofm, m_to_s_dly))
		return;
155 156

	/* PI controller */
157
	adj32 = (int)(pp_servo_pi_controller(ppi, ofm) >> 16);
158 159 160 161 162

	/* 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)
163
			ppi->t_ops->adjust_freq(ppi, -adj32);
164
		else
165
			ppi->t_ops->adjust_offset(ppi, -adj32);
166 167 168 169 170 171
	}

	pp_diag(ppi, servo, 2, "Observed drift: %9i\n",
		(int)SRV(ppi)->obs_drift >> 10);
}

172 173 174
/* called by slave states when delay_resp is received (all t1..t4 are valid) */
void pp_servo_got_presp(struct pp_instance *ppi)
{
175 176 177
	struct pp_time *m_to_s_dly = &SRV(ppi)->m_to_s_dly;
	struct pp_time *s_to_m_dly = &SRV(ppi)->s_to_m_dly;
	struct pp_time *mpd = &DSCUR(ppi)->meanPathDelay;
178 179 180 181 182 183
	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.
	 */
184 185
	*s_to_m_dly = ppi->t6;
	pp_time_sub(s_to_m_dly, &ppi->t5);
186

187 188
	*m_to_s_dly = ppi->t4;
	pp_time_sub(m_to_s_dly, &ppi->t3);
189

190 191 192 193 194 195
	pp_diag(ppi, servo, 2, "T3: %s\n", fmt_ppt(&ppi->t3));
	pp_diag(ppi, servo, 2, "T4: %s\n", fmt_ppt(&ppi->t4));
	pp_diag(ppi, servo, 2, "T5: %s\n", fmt_ppt(&ppi->t5));
	pp_diag(ppi, servo, 2, "T6: %s\n", fmt_ppt(&ppi->t6));
	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_ppt(s_to_m_dly));
196 197

	/* Calc mean path delay, used later to calc "offset from master" */
198 199 200 201
	*mpd = SRV(ppi)->m_to_s_dly;
	pp_time_add(mpd, &SRV(ppi)->s_to_m_dly);
	pp_time_div2(mpd);
	pp_diag(ppi, servo, 1, "meanPathDelay: %s\n", fmt_ppt(mpd));
202

203
	if (mpd->secs) /* Hmm.... we called this "bad event" */
204 205 206 207 208
		return;

	pp_servo_mpd_fltr(ppi, mpd_fltr, mpd);
}

209
static
210
void pp_servo_mpd_fltr(struct pp_instance *ppi, struct pp_avg_fltr *mpd_fltr,
211
		       struct pp_time *mpd)
212 213
{
	int s;
214
	uint64_t y;
215 216 217

	if (mpd_fltr->s_exp < 1) {
		/* First time, keep what we have */
218
		mpd_fltr->y = mpd->scaled_nsecs;
219 220
		if (mpd->scaled_nsecs < 0)
			mpd_fltr->y = 0;
221
	}
222
	/* avoid overflowing filter: calculate number of bits */
223
	s = OPTS(ppi)->s;
224
	while (mpd_fltr->y >> (63 - s))
225
		--s;
226 227
	if (mpd_fltr->s_exp > 1LL << s)
		mpd_fltr->s_exp = 1LL << s;
228
	/* crank down filter cutoff by increasing 's_exp' */
229
	if (mpd_fltr->s_exp < 1LL << s)
230 231 232 233 234 235 236 237 238
		++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.
	 */
239 240 241 242
	if (mpd->scaled_nsecs < 0)
		mpd->scaled_nsecs = mpd_fltr->y;
	if (mpd->scaled_nsecs < 0)
		mpd->scaled_nsecs = 0;
243 244 245 246 247 248 249 250 251 252 253

	/*
	 * 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.
	 */
254
	if (mpd->scaled_nsecs > 3 * mpd_fltr->y) {
255
		pp_diag(ppi, servo, 1, "Trim too-long mpd: %i\n",
256
			(int)(mpd->scaled_nsecs >> 16));
257
		/* add fltr->s_exp to ensure we are not trapped into 0 */
258
		mpd->scaled_nsecs = mpd_fltr->y * 2 + mpd_fltr->s_exp + 1;
259
	}
260 261 262 263
	/* filter 'meanPathDelay' (running average) -- use an unsigned "y" */
	y = (mpd_fltr->y * (mpd_fltr->s_exp - 1) + mpd->scaled_nsecs);
	__div64_32(&y, mpd_fltr->s_exp);
	mpd->scaled_nsecs = mpd_fltr->y = y;
264 265

	pp_diag(ppi, servo, 1, "After avg(%i), meanPathDelay: %i\n",
266
		(int)mpd_fltr->s_exp, (int)(mpd->scaled_nsecs >> 16));
267 268
}

269
static
270 271
int pp_servo_offset_master(struct pp_instance *ppi, struct pp_time *mpd,
			    struct pp_time *ofm, struct pp_time *m_to_s_dly)
272
{
273 274 275 276
	struct pp_time time_tmp;
	*ofm = *m_to_s_dly;
	pp_time_sub(ofm, mpd);
	pp_diag(ppi, servo, 1, "Offset from master:     %s\n", fmt_ppt(ofm));
277

278
	if (!ofm->secs)
279
		return 0; /* proceeed with adjust */
280

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

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

291
static
292
int64_t pp_servo_pi_controller(struct pp_instance * ppi, struct pp_time *ofm)
293 294 295 296 297 298
{
	long long I_term;
	long long P_term;
	long long tmp;
	int I_sign;
	int P_sign;
299
	int64_t adj;
300

301 302
	/* the accumulator for the I component */
	SRV(ppi)->obs_drift += ofm->scaled_nsecs;
303 304 305

	/* 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).
306
	 * Then it's scaled by 16 bits to match our granularity and
307
	 * avoid bit losses */
308
	tmp = (((long long)PP_ADJ_FREQ_MAX) * OPTS(ppi)->ai) << 16;
309 310 311 312 313 314 315 316 317 318 319 320 321 322 323
	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 */
324 325 326
	P_sign = (ofm->scaled_nsecs > 0) ? 0 : -1;
	/* alrady shifted 16 bits, so we avoid losses */
	P_term = ofm->scaled_nsecs;
327 328 329 330 331 332 333
	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 */
334 335
	adj = P_term + I_term;
	/* Return the scaled-nanos values; the caller is scaling back */
336

337
	return adj;
338
}