From a6163888f3c56123b1db313743c6147ba498732c Mon Sep 17 00:00:00 2001 From: Yuval Adam Date: Fri, 8 Aug 2014 14:42:07 +0300 Subject: Add third_party libs --- third_party/ptpd-1.1.0/src/dep/servo.c | 215 +++++++++++++++++++++++++++++++++ 1 file changed, 215 insertions(+) create mode 100644 third_party/ptpd-1.1.0/src/dep/servo.c (limited to 'third_party/ptpd-1.1.0/src/dep/servo.c') diff --git a/third_party/ptpd-1.1.0/src/dep/servo.c b/third_party/ptpd-1.1.0/src/dep/servo.c new file mode 100644 index 0000000..b888593 --- /dev/null +++ b/third_party/ptpd-1.1.0/src/dep/servo.c @@ -0,0 +1,215 @@ +/** + * @file servo.c + * @date Tue Jul 20 16:19:19 2010 + * + * @brief Code which implements the clock servo in software. + * + * + */ + +#include "../ptpd.h" + +void +initClock(RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + DBG("initClock\n"); + + /* clear vars */ + ptpClock->master_to_slave_delay.seconds = ptpClock->master_to_slave_delay.nanoseconds = 0; + ptpClock->slave_to_master_delay.seconds = ptpClock->slave_to_master_delay.nanoseconds = 0; + ptpClock->observed_variance = 0; + ptpClock->observed_drift = 0; /* clears clock servo accumulator (the + * I term) */ + ptpClock->owd_filt.s_exp = 0; /* clears one-way delay filter */ + ptpClock->halfEpoch = ptpClock->halfEpoch || rtOpts->halfEpoch; + rtOpts->halfEpoch = 0; + + /* level clock */ + if (!rtOpts->noAdjust) + adjFreq(0); +} + +void +updateDelay(TimeInternal * send_time, TimeInternal * recv_time, + one_way_delay_filter * owd_filt, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + Integer16 s; + TimeInternal slave_to_master_delay; + + DBGV("updateDelay\n"); + + /* calc 'slave_to_master_delay' */ + subTime(&slave_to_master_delay, recv_time, send_time); + + if (rtOpts->maxDelay) { /* If maxDelay is 0 then it's OFF */ + if (slave_to_master_delay.seconds && rtOpts->maxDelay) { + INFO("updateDelay aborted, delay greater than 1" + " second."); + return; + } + + if (slave_to_master_delay.nanoseconds > rtOpts->maxDelay) { + INFO("updateDelay aborted, delay %d greater than " + "administratively set maximum %d\n", + slave_to_master_delay.nanoseconds, + rtOpts->maxDelay); + return; + } + } + + ptpClock->slave_to_master_delay = slave_to_master_delay; + + /* update 'one_way_delay' */ + addTime(&ptpClock->one_way_delay, &ptpClock->master_to_slave_delay, &ptpClock->slave_to_master_delay); + ptpClock->one_way_delay.seconds /= 2; + ptpClock->one_way_delay.nanoseconds /= 2; + + if (ptpClock->one_way_delay.seconds) { + /* cannot filter with secs, clear filter */ + owd_filt->s_exp = owd_filt->nsec_prev = 0; + return; + } + /* avoid overflowing filter */ + s = rtOpts->s; + while (abs(owd_filt->y) >> (31 - s)) + --s; + + /* crank down filter cutoff by increasing 's_exp' */ + if (owd_filt->s_exp < 1) + owd_filt->s_exp = 1; + else if (owd_filt->s_exp < 1 << s) + ++owd_filt->s_exp; + else if (owd_filt->s_exp > 1 << s) + owd_filt->s_exp = 1 << s; + + /* filter 'one_way_delay' */ + owd_filt->y = (owd_filt->s_exp - 1) * owd_filt->y / owd_filt->s_exp + + (ptpClock->one_way_delay.nanoseconds / 2 + owd_filt->nsec_prev / 2) / owd_filt->s_exp; + + owd_filt->nsec_prev = ptpClock->one_way_delay.nanoseconds; + ptpClock->one_way_delay.nanoseconds = owd_filt->y; + + DBG("delay filter %d, %d\n", owd_filt->y, owd_filt->s_exp); +} + +void +updateOffset(TimeInternal * send_time, TimeInternal * recv_time, + offset_from_master_filter * ofm_filt, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + TimeInternal master_to_slave_delay; + DBGV("updateOffset\n"); + + /* calc 'master_to_slave_delay' */ + subTime(&master_to_slave_delay, recv_time, send_time); + + if (rtOpts->maxDelay) { /* If maxDelay is 0 then it's OFF */ + if (master_to_slave_delay.seconds && rtOpts->maxDelay) { + INFO("updateDelay aborted, delay greater than 1" + " second."); + return; + } + + if (master_to_slave_delay.nanoseconds > rtOpts->maxDelay) { + INFO("updateDelay aborted, delay %d greater than " + "administratively set maximum %d\n", + master_to_slave_delay.nanoseconds, + rtOpts->maxDelay); + return; + } + } + ptpClock->master_to_slave_delay = master_to_slave_delay; + + /* update 'offset_from_master' */ + subTime(&ptpClock->offset_from_master, &ptpClock->master_to_slave_delay, &ptpClock->one_way_delay); + + if (ptpClock->offset_from_master.seconds) { + /* cannot filter with secs, clear filter */ + ofm_filt->nsec_prev = 0; + return; + } + /* filter 'offset_from_master' */ + ofm_filt->y = ptpClock->offset_from_master.nanoseconds / 2 + ofm_filt->nsec_prev / 2; + ofm_filt->nsec_prev = ptpClock->offset_from_master.nanoseconds; + ptpClock->offset_from_master.nanoseconds = ofm_filt->y; + + DBGV("offset filter %d\n", ofm_filt->y); +} + +void +updateClock(RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + Integer32 adj; + TimeInternal timeTmp; + + DBGV("updateClock\n"); + + if (rtOpts->maxReset) { /* If maxReset is 0 then it's OFF */ + if (ptpClock->offset_from_master.seconds) { + INFO("updateClock aborted, offset greater than 1" + " second."); + goto display; + } + + if (ptpClock->offset_from_master.nanoseconds > + rtOpts->maxReset) { + INFO("updateClock aborted, offset %d greater than " + "administratively set maximum %d\n", + ptpClock->offset_from_master.nanoseconds, + rtOpts->maxReset); + goto display; + } + } + + if (ptpClock->offset_from_master.seconds) { + /* if secs, reset clock or set freq adjustment to max */ + if (!rtOpts->noAdjust) { + if (!rtOpts->noResetClock) { + getTime(&timeTmp); + subTime(&timeTmp, &timeTmp, &ptpClock->offset_from_master); + setTime(&timeTmp); + initClock(rtOpts, ptpClock); + } else { + adj = ptpClock->offset_from_master.nanoseconds > 0 ? ADJ_FREQ_MAX : -ADJ_FREQ_MAX; + adjFreq(-adj); + } + } + } else { + /* the PI controller */ + + /* no negative or zero attenuation */ + if (rtOpts->ap < 1) + rtOpts->ap = 1; + if (rtOpts->ai < 1) + rtOpts->ai = 1; + + /* the accumulator for the I component */ + ptpClock->observed_drift += ptpClock->offset_from_master.nanoseconds / rtOpts->ai; + + /* clamp the accumulator to ADJ_FREQ_MAX for sanity */ + if (ptpClock->observed_drift > ADJ_FREQ_MAX) + ptpClock->observed_drift = ADJ_FREQ_MAX; + else if (ptpClock->observed_drift < -ADJ_FREQ_MAX) + ptpClock->observed_drift = -ADJ_FREQ_MAX; + + adj = ptpClock->offset_from_master.nanoseconds / rtOpts->ap + ptpClock->observed_drift; + + /* apply controller output as a clock tick rate adjustment */ + if (!rtOpts->noAdjust) + adjFreq(-adj); + } + +display: + + if (rtOpts->displayStats) + displayStats(rtOpts, ptpClock); + + DBGV("master-to-slave delay: %10ds %11dns\n", + ptpClock->master_to_slave_delay.seconds, ptpClock->master_to_slave_delay.nanoseconds); + DBGV("slave-to-master delay: %10ds %11dns\n", + ptpClock->slave_to_master_delay.seconds, ptpClock->slave_to_master_delay.nanoseconds); + DBGV("one-way delay: %10ds %11dns\n", + ptpClock->one_way_delay.seconds, ptpClock->one_way_delay.nanoseconds); + DBG("offset from master: %10ds %11dns\n", + ptpClock->offset_from_master.seconds, ptpClock->offset_from_master.nanoseconds); + DBG("observed drift: %10d\n", ptpClock->observed_drift); +} -- cgit v1.3.1