diff --git a/clock.c b/clock.c index 3bf2415..cf0d936 100644 --- a/clock.c +++ b/clock.c @@ -25,6 +25,7 @@ #include "bmc.h" #include "clock.h" #include "foreign.h" +#include "mave.h" #include "missing.h" #include "msg.h" #include "phc.h" @@ -34,6 +35,8 @@ #include "tmv.h" #include "util.h" +#define MAVE_LENGTH 10 + #define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0])) struct clock { @@ -50,6 +53,7 @@ struct clock { int nports; tmv_t master_offset; tmv_t path_delay; + struct mave *avg_delay; tmv_t c1; tmv_t c2; tmv_t t1; @@ -183,6 +187,11 @@ struct clock *clock_create(char *phc, struct interface *iface, int count, pr_err("Failed to create clock servo"); return NULL; } + c->avg_delay = mave_create(MAVE_LENGTH); + if (!c->avg_delay) { + pr_err("Failed to create moving average"); + return NULL; + } c->dds = *ds; @@ -315,7 +324,7 @@ int clock_poll(struct clock *c) void clock_path_delay(struct clock *c, struct timespec req, struct timestamp rx, Integer64 correction) { - tmv_t c1, c2, c3, t1, t2, t3, t4; + tmv_t c1, c2, c3, pd, t1, t2, t3, t4; c1 = c->c1; c2 = c->c2; @@ -330,11 +339,13 @@ void clock_path_delay(struct clock *c, struct timespec req, struct timestamp rx, * c->path_delay -= c_sync + c_fup + c_delay_resp; * c->path_delay /= 2.0; */ - c->path_delay = tmv_add(tmv_sub(t2, t3), tmv_sub(t4, t1)); - c->path_delay = tmv_sub(c->path_delay, tmv_add(c1, tmv_add(c2, c3))); - c->path_delay = tmv_div(c->path_delay, 2); + pd = tmv_add(tmv_sub(t2, t3), tmv_sub(t4, t1)); + pd = tmv_sub(pd, tmv_add(c1, tmv_add(c2, c3))); + pd = tmv_div(pd, 2); - pr_debug("path delay %10lld", c->path_delay); + c->path_delay = mave_accumulate(c->avg_delay, pd); + + pr_debug("path delay %10lld %10lld", c->path_delay, pd); } int clock_slave_only(struct clock *c) @@ -404,6 +415,9 @@ static void handle_state_decision_event(struct clock *c) pr_info("selected best master clock %s", cid2str(&best->dataset.identity)); + if (c->best != best) + mave_reset(c->avg_delay); + c->best = best; for (i = 0; i < c->nports; i++) { diff --git a/makefile b/makefile index c559288..852eb1a 100644 --- a/makefile +++ b/makefile @@ -24,8 +24,8 @@ CFLAGS = -Wall $(INC) $(DEBUG) LDFLAGS = LDLIBS = -lm -lrt PRG = linuxptp -OBJ = bmc.o clock.o fsm.o linuxptp.o msg.o phc.o pi.o port.o print.o \ - servo.o tmtab.o transport.o udp.o util.o +OBJ = bmc.o clock.o fsm.o linuxptp.o mave.o msg.o phc.o pi.o port.o \ + print.o servo.o tmtab.o transport.o udp.o util.o SRC = $(OBJ:.o=.c) DEPEND = $(OBJ:.o=.d) diff --git a/mave.c b/mave.c new file mode 100644 index 0000000..7742f05 --- /dev/null +++ b/mave.c @@ -0,0 +1,72 @@ +/** + * @file mave.c + * @note Copyright (C) 2011 Richard Cochran + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ +#include +#include + +#include "mave.h" + +struct mave { + int cnt; + int len; + int index; + tmv_t sum; + tmv_t *val; +}; + +struct mave *mave_create(int length) +{ + struct mave *m; + m = calloc(1, sizeof(*m)); + if (!m) { + return NULL; + } + m->val = calloc(1, length * sizeof(*m->val)); + if (!m->val) { + free(m); + return NULL; + } + m->len = length; + return m; +} + +void mave_destroy(struct mave *m) +{ + free(m->val); + free(m); +} + +tmv_t mave_accumulate(struct mave *m, tmv_t val) +{ + m->sum = tmv_sub(m->sum, m->val[m->index]); + m->val[m->index] = val; + m->index = (1 + m->index) % m->len; + m->sum = tmv_add(m->sum, val); + if (m->cnt < m->len) { + m->cnt++; + } + return tmv_div(m->sum, m->cnt); +} + +void mave_reset(struct mave *m) +{ + m->cnt = 0; + m->index = 0; + m->sum = 0; + memset(m->val, 0, m->len * sizeof(*m->val)); +} diff --git a/mave.h b/mave.h new file mode 100644 index 0000000..84241d4 --- /dev/null +++ b/mave.h @@ -0,0 +1,35 @@ +/** + * @file mave.h + * @brief Implements a moving average. + * @note Copyright (C) 2011 Richard Cochran + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ +#ifndef HAVE_MAVE_H +#define HAVE_MAVE_H + +#include "tmv.h" + +struct mave; + +struct mave *mave_create(int length); + +void mave_destroy(struct mave *m); + +tmv_t mave_accumulate(struct mave *m, tmv_t val); + +void mave_reset(struct mave *m); + +#endif diff --git a/tmv.h b/tmv.h index 0a6ddf7..610e7c6 100644 --- a/tmv.h +++ b/tmv.h @@ -20,6 +20,8 @@ #ifndef HAVE_TMV_H #define HAVE_TMV_H +#include + #include "ddt.h" #include "pdt.h"