3 files changed, 63 insertions, 30 deletions
diff --git a/gnuradio-runtime/include/gnuradio/math.h b/gnuradio-runtime/include/gnuradio/math.h
index c0e1a7dc88..f628a5875c 100644
--- a/gnuradio-runtime/include/gnuradio/math.h
+++ b/gnuradio-runtime/include/gnuradio/math.h
@@ -31,12 +31,30 @@
 #define GR_M_LOG2E 1.4426950408889634074          /* log_2 e */
 #define GR_M_PI 3.14159265358979323846            /* pi */
 #define GR_M_PI_4 0.78539816339744830961566084582 /* pi/4 */
-#define GR_M_TWOPI (2 * GR_M_PI)                  /* 2*pi */
+#define GR_M_TWOPI 6.28318530717958647692         /* 2*pi */
 #define GR_M_SQRT2 1.41421356237309504880         /* sqrt(2) */
+#define GR_M_ONE_OVER_2PI 0.15915494309189533577  /* 1 / (2*pi) */
+#define GR_M_MINUS_TWO_PI -6.28318530717958647692 /* - 2*pi */
 
 
 namespace gr {
 
+static inline void
+fast_cc_multiply(gr_complex& out, const gr_complex cc1, const gr_complex cc2)
+{
+    // The built-in complex.h multiply has significant NaN/INF checking that
+    // considerably slows down performance.  While on some compilers the
+    // -fcx-limit-range flag can be used, this fast function makes the math consistent
+    // in terms of performance for the Costas loop.
+    float o_r, o_i;
+
+    o_r = (cc1.real() * cc2.real()) - (cc1.imag() * cc2.imag());
+    o_i = (cc1.real() * cc2.imag()) + (cc1.imag() * cc2.real());
+
+    out.real(o_r);
+    out.imag(o_i);
+}
+
 static inline bool is_power_of_2(long x) { return x != 0 && (x & (x - 1)) == 0; }
 
 /*!
@@ -62,10 +80,7 @@ static inline float fast_atan2f(gr_complex z) { return fast_atan2f(z.imag(), z.r
 /* This bounds x by +/- clip without a branch */
 static inline float branchless_clip(float x, float clip)
 {
-    float x1 = fabsf(x + clip);
-    float x2 = fabsf(x - clip);
-    x1 -= x2;
-    return 0.5 * x1;
+    return 0.5 * (std::abs(x + clip) - std::abs(x - clip));
 }
 
 static inline float clip(float x, float clip)
diff --git a/gnuradio-runtime/include/gnuradio/nco.h b/gnuradio-runtime/include/gnuradio/nco.h
index 6542fe58de..4bcdfe686b 100644
--- a/gnuradio-runtime/include/gnuradio/nco.h
+++ b/gnuradio-runtime/include/gnuradio/nco.h
@@ -44,27 +44,15 @@ public:
     void adjust_freq(double delta_angle_rate) { phase_inc += delta_angle_rate; }
 
     // increment current phase angle
-    void step()
-    {
-        phase += phase_inc;
-        if (fabs(phase) > GR_M_PI) {
-            while (phase > GR_M_PI)
-                phase -= 2 * GR_M_PI;
-
-            while (phase < -GR_M_PI)
-                phase += 2 * GR_M_PI;
-        }
-    }
-
-    void step(int n)
+    void step(int n = 1)
     {
         phase += phase_inc * n;
         if (fabs(phase) > GR_M_PI) {
             while (phase > GR_M_PI)
-                phase -= 2 * GR_M_PI;
+                phase -= GR_M_TWOPI;
 
             while (phase < -GR_M_PI)
-                phase += 2 * GR_M_PI;
+                phase += GR_M_TWOPI;
         }
     }
 
@@ -73,7 +61,7 @@ public:
     double get_freq() const { return phase_inc; }
 
     // compute sin and cos for current phase angle
-    void sincos(float* sinx, float* cosx) const;
+    void sincos(float* sinx, float* cosx) const { gr::sincosf(phase, sinx, cosx); }
 
     // compute cos or sin for current phase angle
     float cos() const { return std::cos(phase); }
@@ -94,12 +82,6 @@ protected:
 };
 
 template <class o_type, class i_type>
-void nco<o_type, i_type>::sincos(float* sinx, float* cosx) const
-{
-    gr::sincosf(phase, sinx, cosx);
-}
-
-template <class o_type, class i_type>
 void nco<o_type, i_type>::sin(float* output, int noutput_items, double ampl)
 {
     for (int i = 0; i < noutput_items; i++) {
diff --git a/gnuradio-runtime/include/gnuradio/sincos.h b/gnuradio-runtime/include/gnuradio/sincos.h
index 15241195d0..cd0f6eb0dd 100644
--- a/gnuradio-runtime/include/gnuradio/sincos.h
+++ b/gnuradio-runtime/include/gnuradio/sincos.h
@@ -12,12 +12,48 @@
 #define INCLUDED_GR_SINCOS_H
 
 #include <gnuradio/api.h>
+#include <cmath>
 
 namespace gr {
 
-// compute sine and cosine at the same time
-GR_RUNTIME_API void sincos(double x, double* sin, double* cos);
-GR_RUNTIME_API void sincosf(float x, float* sin, float* cos);
+#if defined(HAVE_SINCOS)
+
+inline void sincos(double x, double* sinx, double* cosx) { ::sincos(x, sinx, cosx); }
+
+#else
+
+inline void sincos(double x, double* sinx, double* cosx)
+{
+    *sinx = ::sin(x);
+    *cosx = ::cos(x);
+}
+
+#endif
+
+// ----------------------------------------------------------------
+
+#if defined(HAVE_SINCOSF)
+
+inline void sincosf(float x, float* sinx, float* cosx) { ::sincosf(x, sinx, cosx); }
+
+#elif defined(HAVE_SINF) && defined(HAVE_COSF)
+
+inline void sincosf(float x, float* sinx, float* cosx)
+{
+    *sinx = ::sinf(x);
+    *cosx = ::cosf(x);
+}
+
+#else
+
+inline void sincosf(float x, float* sinx, float* cosx)
+{
+    *sinx = ::sin(x);
+    *cosx = ::cos(x);
+}
+
+#endif
+
 } // namespace gr
 
 #endif /* INCLUDED_GR_SINCOS_H */