From: Miroslav Lichvar <mlichvar@redhat.com>
Date: Thu, 24 Feb 2022 07:44:10 +0000 (+0100)
Subject: samplefilt: add function to correct accumulated offsets
X-Git-Tag: 4.3-pre1~55
X-Git-Url: http://git.ipfire.org/?a=commitdiff_plain;h=d5e645eb38b06e66a16856eb2ba6cf4b73cf2bec;p=thirdparty%2Fchrony.git

samplefilt: add function to correct accumulated offsets

Analogously to SST_CorrectOffset(), add SPF_CorrectOffset() to correct
the offsets accumulated in the filter.
---

diff --git a/samplefilt.c b/samplefilt.c
index fe32b5ce..9cb892d3 100644
--- a/samplefilt.c
+++ b/samplefilt.c
@@ -412,24 +412,36 @@ SPF_GetFilteredSample(SPF_Instance filter, NTP_Sample *sample)
 
 /* ================================================== */
 
-void
-SPF_SlewSamples(SPF_Instance filter, struct timespec *when, double dfreq, double doffset)
+static int
+get_first_last(SPF_Instance filter, int *first, int *last)
 {
-  int i, first, last;
-  double delta_time;
-
   if (filter->last < 0)
-    return;
+    return 0;
 
   /* Always slew the last sample as it may be returned even if no new
      samples were accumulated */
   if (filter->used > 0) {
-    first = 0;
-    last = filter->used - 1;
+    *first = 0;
+    *last = filter->used - 1;
   } else {
-    first = last = filter->last;
+    *first = *last = filter->last;
   }
 
+  return 1;
+}
+
+
+/* ================================================== */
+
+void
+SPF_SlewSamples(SPF_Instance filter, struct timespec *when, double dfreq, double doffset)
+{
+  int i, first, last;
+  double delta_time;
+
+  if (!get_first_last(filter, &first, &last))
+    return;
+
   for (i = first; i <= last; i++) {
     UTI_AdjustTimespec(&filter->samples[i].time, when, &filter->samples[i].time,
                        &delta_time, dfreq, doffset);
@@ -439,6 +451,20 @@ SPF_SlewSamples(SPF_Instance filter, struct timespec *when, double dfreq, double
 
 /* ================================================== */
 
+void
+SPF_CorrectOffset(SPF_Instance filter, double doffset)
+{
+  int i, first, last;
+
+  if (!get_first_last(filter, &first, &last))
+    return;
+
+  for (i = first; i <= last; i++)
+    filter->samples[i].offset -= doffset;
+}
+
+/* ================================================== */
+
 void
 SPF_AddDispersion(SPF_Instance filter, double dispersion)
 {
diff --git a/samplefilt.h b/samplefilt.h
index 3b4350a3..e55a7767 100644
--- a/samplefilt.h
+++ b/samplefilt.h
@@ -44,6 +44,7 @@ extern void SPF_DropSamples(SPF_Instance filter);
 extern int SPF_GetFilteredSample(SPF_Instance filter, NTP_Sample *sample);
 extern void SPF_SlewSamples(SPF_Instance filter, struct timespec *when,
                             double dfreq, double doffset);
+extern void SPF_CorrectOffset(SPF_Instance filter, double doffset);
 extern void SPF_AddDispersion(SPF_Instance filter, double dispersion);
 
 #endif
diff --git a/test/unit/samplefilt.c b/test/unit/samplefilt.c
index 513ff2c4..20e6febc 100644
--- a/test/unit/samplefilt.c
+++ b/test/unit/samplefilt.c
@@ -69,6 +69,7 @@ test_unit(void)
         TEST_CHECK(!memcmp(&sample_in, &sample_out, sizeof (sample_in)));
 
         SPF_SlewSamples(filter, &sample_in.time, 0.0, 0.0);
+        SPF_CorrectOffset(filter, 0.0);
         SPF_AddDispersion(filter, 0.0);
 
         if (k + 1 < min_samples)