sys: clamp frequency set in generic driver on exit

diff --git a/sys_generic.c b/sys_generic.c
index 41e36fffb56d0278cc510ae541e7cb1a7902ee8d..db343e55d3facc962206ed41afd841ed5b3e315d 100644 (file)
--- a/sys_generic.c
+++ b/sys_generic.c
@@ -107,6 +107,18 @@ handle_step(struct timeval *raw, struct timeval *cooked, double dfreq,
   }
 }
 
+/* ================================================== */
+
+static double
+clamp_freq(double freq)
+{
+  if (freq > max_freq)
+    return max_freq;
+  if (freq < -max_freq)
+    return -max_freq;
+  return freq;
+}
+
 /* ================================================== */
 /* End currently running slew and start a new one */
 
@@ -144,11 +156,7 @@ update_slew(void)
     corr_freq = max_corr_freq;
 
   /* Get the new real frequency and clamp it */
-  total_freq = base_freq + corr_freq * (1.0e6 - base_freq);
-  if (total_freq > max_freq)
-    total_freq = max_freq;
-  else if (total_freq < -max_freq)
-    total_freq = -max_freq;
+  total_freq = clamp_freq(base_freq + corr_freq * (1.0e6 - base_freq));
 
   /* Set the new frequency (the actual frequency returned by the call may be
      slightly different from the requested frequency due to rounding) */
@@ -330,7 +338,7 @@ SYS_Generic_Finalise(void)
     slew_timer_running = 0;
   }
 
-  (*drv_set_freq)(base_freq);
+  (*drv_set_freq)(clamp_freq(base_freq));
 }
 
 /* ================================================== */