From 2c9718f3c22f078b9b69831c0ec1da4550e0d24a Mon Sep 17 00:00:00 2001 From: Miroslav Lichvar Date: Tue, 29 Oct 2013 11:58:18 +0100 Subject: [PATCH] Add modular filter interface. Similarly to the servo interface, allow multiple filters to be used for delay filtering. Convert mave to the new interface. Signed-off-by: Miroslav Lichvar --- clock.c | 16 ++++++------ filter.c | 46 +++++++++++++++++++++++++++++++++++ filter.h | 63 ++++++++++++++++++++++++++++++++++++++++++++++++ filter_private.h | 33 +++++++++++++++++++++++++ makefile | 5 ++-- mave.c | 48 +++++++++++++++++++++--------------- mave.h | 12 ++------- port.c | 14 +++++------ 8 files changed, 191 insertions(+), 46 deletions(-) create mode 100644 filter.c create mode 100644 filter.h create mode 100644 filter_private.h diff --git a/clock.c b/clock.c index 74d5c77..cf2f752 100644 --- a/clock.c +++ b/clock.c @@ -27,7 +27,7 @@ #include "clockadj.h" #include "clockcheck.h" #include "foreign.h" -#include "mave.h" +#include "filter.h" #include "missing.h" #include "msg.h" #include "phc.h" @@ -86,7 +86,7 @@ struct clock { enum servo_state servo_state; tmv_t master_offset; tmv_t path_delay; - struct mave *avg_delay; + struct filter *delay_filter; struct freq_estimator fest; struct time_status_np status; double nrr; @@ -121,7 +121,7 @@ void clock_destroy(struct clock *c) phc_close(c->clkid); } servo_destroy(c->servo); - mave_destroy(c->avg_delay); + filter_destroy(c->delay_filter); stats_destroy(c->stats.offset); stats_destroy(c->stats.freq); stats_destroy(c->stats.delay); @@ -632,9 +632,9 @@ struct clock *clock_create(int phc_index, struct interface *iface, int count, return NULL; } c->servo_state = SERVO_UNLOCKED; - c->avg_delay = mave_create(MAVE_LENGTH); - if (!c->avg_delay) { - pr_err("Failed to create moving average"); + c->delay_filter = filter_create(FILTER_MOVING_AVERAGE, MAVE_LENGTH); + if (!c->delay_filter) { + pr_err("Failed to create delay filter"); return NULL; } c->stats_interval = dds->stats_interval; @@ -992,7 +992,7 @@ void clock_path_delay(struct clock *c, struct timespec req, struct timestamp rx, pr_warning("c3 %10lld", c3); } - c->path_delay = mave_accumulate(c->avg_delay, pd); + c->path_delay = filter_sample(c->delay_filter, pd); c->cur.meanPathDelay = tmv_to_TimeInterval(c->path_delay); @@ -1170,7 +1170,7 @@ static void handle_state_decision_event(struct clock *c) if (!cid_eq(&best_id, &c->best_id)) { clock_freq_est_reset(c); - mave_reset(c->avg_delay); + filter_reset(c->delay_filter); c->t1 = tmv_zero(); c->t2 = tmv_zero(); c->path_delay = 0; diff --git a/filter.c b/filter.c new file mode 100644 index 0000000..65eb003 --- /dev/null +++ b/filter.c @@ -0,0 +1,46 @@ +/** + * @file filter.c + * @note Copyright (C) 2013 Miroslav Lichvar + * + * 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 "filter_private.h" +#include "mave.h" + +struct filter *filter_create(enum filter_type type, int length) +{ + switch (type) { + case FILTER_MOVING_AVERAGE: + return mave_create(length); + default: + return NULL; + } +} + +void filter_destroy(struct filter *filter) +{ + filter->destroy(filter); +} + +tmv_t filter_sample(struct filter *filter, tmv_t sample) +{ + return filter->sample(filter, sample); +} + +void filter_reset(struct filter *filter) +{ + filter->reset(filter); +} diff --git a/filter.h b/filter.h new file mode 100644 index 0000000..4d7b370 --- /dev/null +++ b/filter.h @@ -0,0 +1,63 @@ +/** + * @file filter.h + * @brief Implements a generic filter interface. + * @note Copyright (C) 2013 Miroslav Lichvar + * + * 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_FILTER_H +#define HAVE_FILTER_H + +#include "tmv.h" + +/** Opaque type */ +struct filter; + +/** + * Defines the available filters. + */ +enum filter_type { + FILTER_MOVING_AVERAGE, +}; + +/** + * Create a new instance of a filter. + * @param type The type of the filter to create. + * @param length The filter's length. + * @return A pointer to a new filter on success, NULL otherwise. + */ +struct filter *filter_create(enum filter_type type, int length); + +/** + * Destroy an instance of a filter. + * @param filter Pointer to a filter obtained via @ref filter_create(). + */ +void filter_destroy(struct filter *filter); + +/** + * Feed a sample into a filter. + * @param filter Pointer to a filter obtained via @ref filter_create(). + * @param sample The input sample. + * @return The output value. + */ +tmv_t filter_sample(struct filter *filter, tmv_t sample); + +/** + * Reset a filter. + * @param filter Pointer to a filter obtained via @ref filter_create(). + */ +void filter_reset(struct filter *filter); + +#endif diff --git a/filter_private.h b/filter_private.h new file mode 100644 index 0000000..26062b4 --- /dev/null +++ b/filter_private.h @@ -0,0 +1,33 @@ +/** + * @file filter_private.h + * @note Copyright (C) 2013 Miroslav Lichvar + * + * 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_FILTER_PRIVATE_H +#define HAVE_FILTER_PRIVATE_H + +#include "tmv.h" +#include "contain.h" + +struct filter { + void (*destroy)(struct filter *filter); + + tmv_t (*sample)(struct filter *filter, tmv_t sample); + + void (*reset)(struct filter *filter); +}; + +#endif diff --git a/makefile b/makefile index 3f7ec90..5e9b009 100644 --- a/makefile +++ b/makefile @@ -24,8 +24,9 @@ CFLAGS = -Wall $(VER) $(incdefs) $(DEBUG) $(EXTRA_CFLAGS) LDLIBS = -lm -lrt $(EXTRA_LDFLAGS) PRG = ptp4l pmc phc2sys hwstamp_ctl OBJ = bmc.o clock.o clockadj.o clockcheck.o config.o fault.o \ - fsm.o ptp4l.o mave.o msg.o phc.o pi.o port.o print.o raw.o servo.o \ - sk.o stats.o tlv.o tmtab.o transport.o udp.o udp6.o uds.o util.o version.o + filter.o fsm.o mave.o msg.o phc.o pi.o port.o print.o \ + ptp4l.o raw.o servo.o sk.o stats.o tlv.o tmtab.o transport.o udp.o \ + udp6.o uds.o util.o version.o OBJECTS = $(OBJ) hwstamp_ctl.o phc2sys.o pmc.o pmc_common.o sysoff.o SRC = $(OBJECTS:.o=.c) diff --git a/mave.c b/mave.c index 7742f05..fd09e5a 100644 --- a/mave.c +++ b/mave.c @@ -20,8 +20,10 @@ #include #include "mave.h" +#include "filter_private.h" struct mave { + struct filter filter; int cnt; int len; int index; @@ -29,30 +31,17 @@ struct mave { 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) +static void mave_destroy(struct filter *filter) { + struct mave *m = container_of(filter, struct mave, filter); free(m->val); free(m); } -tmv_t mave_accumulate(struct mave *m, tmv_t val) +static tmv_t mave_accumulate(struct filter *filter, tmv_t val) { + struct mave *m = container_of(filter, struct mave, filter); + m->sum = tmv_sub(m->sum, m->val[m->index]); m->val[m->index] = val; m->index = (1 + m->index) % m->len; @@ -63,10 +52,31 @@ tmv_t mave_accumulate(struct mave *m, tmv_t val) return tmv_div(m->sum, m->cnt); } -void mave_reset(struct mave *m) +static void mave_reset(struct filter *filter) { + struct mave *m = container_of(filter, struct mave, filter); + m->cnt = 0; m->index = 0; m->sum = 0; memset(m->val, 0, m->len * sizeof(*m->val)); } + +struct filter *mave_create(int length) +{ + struct mave *m; + m = calloc(1, sizeof(*m)); + if (!m) { + return NULL; + } + m->filter.destroy = mave_destroy; + m->filter.sample = mave_accumulate; + m->filter.reset = mave_reset; + m->val = calloc(1, length * sizeof(*m->val)); + if (!m->val) { + free(m); + return NULL; + } + m->len = length; + return &m->filter; +} diff --git a/mave.h b/mave.h index 84241d4..8f170e4 100644 --- a/mave.h +++ b/mave.h @@ -20,16 +20,8 @@ #ifndef HAVE_MAVE_H #define HAVE_MAVE_H -#include "tmv.h" +#include "filter.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); +struct filter *mave_create(int length); #endif diff --git a/port.c b/port.c index bd10d0a..e8448b8 100644 --- a/port.c +++ b/port.c @@ -25,7 +25,7 @@ #include "bmc.h" #include "clock.h" -#include "mave.h" +#include "filter.h" #include "missing.h" #include "msg.h" #include "port.h" @@ -82,7 +82,7 @@ struct port { } seqnum; struct tmtab tmtab; tmv_t peer_delay; - struct mave *avg_delay; + struct filter *delay_filter; int log_sync_interval; struct nrate_estimator nrate; unsigned int pdr_missing; @@ -1716,7 +1716,7 @@ calc: pd = tmv_sub(pd, c2); pd = tmv_div(pd, 2); - p->peer_delay = mave_accumulate(p->avg_delay, pd); + p->peer_delay = filter_sample(p->delay_filter, pd); p->peerMeanPathDelay = tmv_to_TimeInterval(p->peer_delay); @@ -1845,7 +1845,7 @@ void port_close(struct port *p) port_disable(p); } transport_destroy(p->trp); - mave_destroy(p->avg_delay); + filter_destroy(p->delay_filter); free(p); } @@ -2306,9 +2306,9 @@ struct port *port_open(int phc_index, p->delayMechanism = interface->dm; p->versionNumber = PTP_VERSION; - p->avg_delay = mave_create(PORT_MAVE_LENGTH); - if (!p->avg_delay) { - pr_err("Failed to create moving average"); + p->delay_filter = filter_create(FILTER_MOVING_AVERAGE, PORT_MAVE_LENGTH); + if (!p->delay_filter) { + pr_err("Failed to create delay filter"); transport_destroy(p->trp); free(p); return NULL;