src/effects_i_dsp.c

   1 /* libSoX internal DSP functions.
   2  * All public functions & data are prefixed with lsx_ .
   3  *
   4  * Copyright (c) 2008,2012 robs@users.sourceforge.net
   5  *
   6  * This library is free software; you can redistribute it and/or modify it
   7  * under the terms of the GNU Lesser General Public License as published by
   8  * the Free Software Foundation; either version 2.1 of the License, or (at
   9  * your option) any later version.
  10  *
  11  * This library is distributed in the hope that it will be useful, but
  12  * WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser
  14  * General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU Lesser General Public License
  17  * along with this library; if not, write to the Free Software Foundation,
  18  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
  19  */
  20
  21 #ifdef NDEBUG /* Enable assert always. */
  22 #undef NDEBUG /* Must undef above assert.h or other that might include it. */
  23 #endif
  24
  25 #include "sox_i.h"
  26 #include <assert.h>
  27 #include <string.h>
  28
  29 /* Concurrent Control with "Readers" and "Writers", P.J. Courtois et al, 1971:*/
  30
  31 #if defined HAVE_OPENMP
  32
  33 typedef struct {
  34   int readcount, writecount; /* initial value = 0 */
  35   omp_lock_t mutex_1, mutex_2, mutex_3, w, r; /* initial value = 1 */
  36 } ccrw2_t; /* Problem #2: `writers-preference' */
  37
  38 #define ccrw2_become_reader(p) do {\
  39   omp_set_lock(&p.mutex_3);\
  40     omp_set_lock(&p.r);\
  41       omp_set_lock(&p.mutex_1);\
  42         if (++p.readcount == 1) omp_set_lock(&p.w);\
  43       omp_unset_lock(&p.mutex_1);\
  44     omp_unset_lock(&p.r);\
  45   omp_unset_lock(&p.mutex_3);\
  46 } while (0)
  47 #define ccrw2_cease_reading(p) do {\
  48   omp_set_lock(&p.mutex_1);\
  49     if (!--p.readcount) omp_unset_lock(&p.w);\
  50   omp_unset_lock(&p.mutex_1);\
  51 } while (0)
  52 #define ccrw2_become_writer(p) do {\
  53   omp_set_lock(&p.mutex_2);\
  54     if (++p.writecount == 1) omp_set_lock(&p.r);\
  55   omp_unset_lock(&p.mutex_2);\
  56   omp_set_lock(&p.w);\
  57 } while (0)
  58 #define ccrw2_cease_writing(p) do {\
  59   omp_unset_lock(&p.w);\
  60   omp_set_lock(&p.mutex_2);\
  61     if (!--p.writecount) omp_unset_lock(&p.r);\
  62   omp_unset_lock(&p.mutex_2);\
  63 } while (0)
  64 #define ccrw2_init(p) do {\
  65   omp_init_lock(&p.mutex_1);\
  66   omp_init_lock(&p.mutex_2);\
  67   omp_init_lock(&p.mutex_3);\
  68   omp_init_lock(&p.w);\
  69   omp_init_lock(&p.r);\
  70 } while (0)
  71 #define ccrw2_clear(p) do {\
  72   omp_destroy_lock(&p.r);\
  73   omp_destroy_lock(&p.w);\
  74   omp_destroy_lock(&p.mutex_3);\
  75   omp_destroy_lock(&p.mutex_2);\
  76   omp_destroy_lock(&p.mutex_1);\
  77 } while (0)
  78
  79 #else
  80
  81 #define ccrw2_become_reader(x) (void)0
  82 #define ccrw2_cease_reading(x) (void)0
  83 #define ccrw2_become_writer(x) (void)0
  84 #define ccrw2_cease_writing(x) (void)0
  85 #define ccrw2_init(x) (void)0
  86 #define ccrw2_clear(x) (void)0
  87
  88 #endif /* HAVE_OPENMP */
  89
  90 /* Numerical Recipes cubic spline: */
  91
  92 void lsx_prepare_spline3(double const * x, double const * y, int n,
  93     double start_1d, double end_1d, double * y_2d)
  94 {
  95   double p, qn, sig, un, * u = lsx_malloc((n - 1) * sizeof(*u));
  96   int i;
  97
  98   if (start_1d == HUGE_VAL)
  99     y_2d[0] = u[0] = 0;      /* Start with natural spline or */
 100   else {                     /* set the start first derivative */
 101     y_2d[0] = -.5;
 102     u[0] = (3 / (x[1] - x[0])) * ((y[1] - y[0]) / (x[1] - x[0]) - start_1d);
 103   }
 104
 105   for (i = 1; i < n - 1; ++i) {
 106     sig = (x[i] - x[i - 1]) / (x[i + 1] - x[i - 1]);
 107     p = sig * y_2d[i - 1] + 2;
 108     y_2d[i] = (sig - 1) / p;
 109     u[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) -
 110            (y[i] - y[i - 1]) / (x[i] - x[i - 1]);
 111     u[i] = (6 * u[i] / (x[i + 1] - x[i - 1]) - sig * u[i - 1]) / p;
 112   }
 113   if (end_1d == HUGE_VAL)
 114     qn = un = 0;             /* End with natural spline or */
 115   else {                     /* set the end first derivative */
 116     qn = .5;
 117     un = 3 / (x[n - 1] - x[n - 2]) * (end_1d - (y[n - 1] - y[n - 2]) / (x[n - 1] - x[n - 2]));
 118   }
 119   y_2d[n - 1] = (un - qn * u[n - 2]) / (qn * y_2d[n - 2] + 1);
 120   for (i = n - 2; i >= 0; --i)
 121     y_2d[i] = y_2d[i] * y_2d[i + 1] + u[i];
 122   free(u);
 123 }
 124
 125 double lsx_spline3(double const * x, double const * y, double const * y_2d,
 126     int n, double x1)
 127 {
 128   int     t, i[2] = {0, 0};
 129   double  d, a, b;
 130
 131   for (i[1] = n - 1; i[1] - i[0] > 1; t = (i[1] + i[0]) >> 1, i[x[t] > x1] = t);
 132   d = x[i[1]] - x[i[0]];
 133   assert(d != 0);
 134   a = (x[i[1]] - x1) / d;
 135   b = (x1 - x[i[0]]) / d;
 136   return a * y[i[0]] + b * y[i[1]] +
 137     ((a * a * a - a) * y_2d[i[0]] + (b * b * b - b) * y_2d[i[1]]) * d * d / 6;
 138 }
 139
 140 double lsx_bessel_I_0(double x)
 141 {
 142   double term = 1, sum = 1, last_sum, x2 = x / 2;
 143   int i = 1;
 144   do {
 145     double y = x2 / i++;
 146     last_sum = sum, sum += term *= y * y;
 147   } while (sum != last_sum);
 148   return sum;
 149 }
 150
 151 int lsx_set_dft_length(int num_taps) /* Set to 4 x nearest power of 2 */
 152 {      /* or half of that if danger of causing too many cache misses. */
 153   int min = sox_globals.log2_dft_min_size;
 154   double d = log((double)num_taps) / log(2.);
 155   return 1 << range_limit((int)(d + 2.77), min, max((int)(d + 1.77), 17));
 156 }
 157
 158 #include "fft4g.h"
 159 static int * lsx_fft_br;
 160 static double * lsx_fft_sc;
 161 static int fft_len = -1;
 162 #if defined HAVE_OPENMP
 163 static ccrw2_t fft_cache_ccrw;
 164 #endif
 165
 166 void init_fft_cache(void)
 167 {
 168   assert(lsx_fft_br == NULL);
 169   assert(lsx_fft_sc == NULL);
 170   assert(fft_len == -1);
 171   ccrw2_init(fft_cache_ccrw);
 172   fft_len = 0;
 173 }
 174
 175 void clear_fft_cache(void)
 176 {
 177   assert(fft_len >= 0);
 178   ccrw2_clear(fft_cache_ccrw);
 179   free(lsx_fft_br);
 180   free(lsx_fft_sc);
 181   lsx_fft_sc = NULL;
 182   lsx_fft_br = NULL;
 183   fft_len = -1;
 184 }
 185
 186 static sox_bool update_fft_cache(int len)
 187 {
 188   assert(lsx_is_power_of_2(len));
 189   assert(fft_len >= 0);
 190   ccrw2_become_reader(fft_cache_ccrw);
 191   if (len > fft_len) {
 192     ccrw2_cease_reading(fft_cache_ccrw);
 193     ccrw2_become_writer(fft_cache_ccrw);
 194     if (len > fft_len) {
 195       int old_n = fft_len;
 196       fft_len = len;
 197       lsx_fft_br = lsx_realloc(lsx_fft_br, dft_br_len(fft_len) * sizeof(*lsx_fft_br));
 198       lsx_fft_sc = lsx_realloc(lsx_fft_sc, dft_sc_len(fft_len) * sizeof(*lsx_fft_sc));
 199       if (!old_n)
 200         lsx_fft_br[0] = 0;
 201       return sox_true;
 202     }
 203     ccrw2_cease_writing(fft_cache_ccrw);
 204     ccrw2_become_reader(fft_cache_ccrw);
 205   }
 206   return sox_false;
 207 }
 208
 209 static void done_with_fft_cache(sox_bool is_writer)
 210 {
 211   if (is_writer)
 212     ccrw2_cease_writing(fft_cache_ccrw);
 213   else ccrw2_cease_reading(fft_cache_ccrw);
 214 }
 215
 216 void lsx_safe_rdft(int len, int type, double * d)
 217 {
 218   sox_bool is_writer = update_fft_cache(len);
 219   lsx_rdft(len, type, d, lsx_fft_br, lsx_fft_sc);
 220   done_with_fft_cache(is_writer);
 221 }
 222
 223 void lsx_safe_cdft(int len, int type, double * d)
 224 {
 225   sox_bool is_writer = update_fft_cache(len);
 226   lsx_cdft(len, type, d, lsx_fft_br, lsx_fft_sc);
 227   done_with_fft_cache(is_writer);
 228 }
 229
 230 void lsx_power_spectrum(int n, double const * in, double * out)
 231 {
 232   int i;
 233   double * work = lsx_memdup(in, n * sizeof(*work));
 234   lsx_safe_rdft(n, 1, work);
 235   out[0] = sqr(work[0]);
 236   for (i = 2; i < n; i += 2)
 237     out[i >> 1] = sqr(work[i]) + sqr(work[i + 1]);
 238   out[i >> 1] = sqr(work[1]);
 239   free(work);
 240 }
 241
 242 void lsx_power_spectrum_f(int n, float const * in, float * out)
 243 {
 244   int i;
 245   double * work = lsx_malloc(n * sizeof(*work));
 246   for (i = 0; i< n; ++i) work[i] = in[i];
 247   lsx_safe_rdft(n, 1, work);
 248   out[0] = sqr(work[0]);
 249   for (i = 2; i < n; i += 2)
 250     out[i >> 1] = sqr(work[i]) + sqr(work[i + 1]);
 251   out[i >> 1] = sqr(work[1]);
 252   free(work);
 253 }
 254
 255 void lsx_apply_hann_f(float h[], const int num_points)
 256 {
 257   int i, m = num_points - 1;
 258   for (i = 0; i < num_points; ++i) {
 259     double x = 2 * M_PI * i / m;
 260     h[i] *= .5 - .5 * cos(x);
 261   }
 262 }
 263
 264 void lsx_apply_hann(double h[], const int num_points)
 265 {
 266   int i, m = num_points - 1;
 267   for (i = 0; i < num_points; ++i) {
 268     double x = 2 * M_PI * i / m;
 269     h[i] *= .5 - .5 * cos(x);
 270   }
 271 }
 272
 273 void lsx_apply_hamming(double h[], const int num_points)
 274 {
 275   int i, m = num_points - 1;
 276   for (i = 0; i < num_points; ++i) {
 277     double x = 2 * M_PI * i / m;
 278     h[i] *= .53836 - .46164 * cos(x);
 279   }
 280 }
 281
 282 void lsx_apply_bartlett(double h[], const int num_points)
 283 {
 284   int i, m = num_points - 1;
 285   for (i = 0; i < num_points; ++i) {
 286     h[i] *= 2. / m * (m / 2. - fabs(i - m / 2.));
 287   }
 288 }
 289
 290 void lsx_apply_blackman(double h[], const int num_points, double alpha /*.16*/)
 291 {
 292   int i, m = num_points - 1;
 293   for (i = 0; i < num_points; ++i) {
 294     double x = 2 * M_PI * i / m;
 295     h[i] *= (1 - alpha) *.5 - .5 * cos(x) + alpha * .5 * cos(2 * x);
 296   }
 297 }
 298
 299 void lsx_apply_blackman_nutall(double h[], const int num_points)
 300 {
 301   int i, m = num_points - 1;
 302   for (i = 0; i < num_points; ++i) {
 303     double x = 2 * M_PI * i / m;
 304     h[i] *= .3635819 - .4891775 * cos(x) + .1365995 * cos(2 * x) - .0106411 * cos(3 * x);
 305   }
 306 }
 307
 308 double lsx_kaiser_beta(double att, double tr_bw)
 309 {
 310   if (att >= 60) {
 311     static const double coefs[][4] = {
 312       {-6.784957e-10,1.02856e-05,0.1087556,-0.8988365+.001},
 313       {-6.897885e-10,1.027433e-05,0.10876,-0.8994658+.002},
 314       {-1.000683e-09,1.030092e-05,0.1087677,-0.9007898+.003},
 315       {-3.654474e-10,1.040631e-05,0.1087085,-0.8977766+.006},
 316       {8.106988e-09,6.983091e-06,0.1091387,-0.9172048+.015},
 317       {9.519571e-09,7.272678e-06,0.1090068,-0.9140768+.025},
 318       {-5.626821e-09,1.342186e-05,0.1083999,-0.9065452+.05},
 319       {-9.965946e-08,5.073548e-05,0.1040967,-0.7672778+.085},
 320       {1.604808e-07,-5.856462e-05,0.1185998,-1.34824+.1},
 321       {-1.511964e-07,6.363034e-05,0.1064627,-0.9876665+.18},
 322     };
 323     double realm = log(tr_bw/.0005)/log(2.);
 324     double const * c0 = coefs[range_limit(  (int)realm, 0, (int)array_length(coefs)-1)];
 325     double const * c1 = coefs[range_limit(1+(int)realm, 0, (int)array_length(coefs)-1)];
 326     double b0 = ((c0[0]*att + c0[1])*att + c0[2])*att + c0[3];
 327     double b1 = ((c1[0]*att + c1[1])*att + c1[2])*att + c1[3];
 328     return b0 + (b1 - b0) * (realm - (int)realm);
 329   }
 330   if (att > 50   ) return .1102 * (att - 8.7);
 331   if (att > 20.96) return .58417 * pow(att -20.96, .4) + .07886 * (att - 20.96);
 332   return 0;
 333 }
 334
 335 void lsx_apply_kaiser(double h[], const int num_points, double beta)
 336 {
 337   int i, m = num_points - 1;
 338   for (i = 0; i <= m; ++i) {
 339     double x = 2. * i / m - 1;
 340     h[i] *= lsx_bessel_I_0(beta * sqrt(1 - x * x)) / lsx_bessel_I_0(beta);
 341   }
 342 }
 343
 344 void lsx_apply_dolph(double h[], const int N, double att)
 345 {
 346   double b = cosh(acosh(pow(10., att/20)) / (N-1)), sum, t, c, norm = 0;
 347   int i, j;
 348   for (c = 1 - 1 / (b*b), i = (N-1) / 2; i >= 0; --i) {
 349     for (sum = !i, b = t = j = 1; j <= i && sum != t; b *= (i-j) * (1./j), ++j)
 350       t = sum, sum += (b *= c * (N - i - j) * (1./j));
 351     sum /= (N - 1 - i), sum /= (norm = norm? norm : sum);
 352     h[i] *= sum, h[N - 1 - i] *= sum;
 353   }
 354 }
 355
 356 double * lsx_make_lpf(int num_taps, double Fc, double beta, double rho,
 357     double scale, sox_bool dc_norm)
 358 {
 359   int i, m = num_taps - 1;
 360   double * h = calloc(num_taps, sizeof(*h)), sum = 0;
 361   double mult = scale / lsx_bessel_I_0(beta), mult1 = 1 / (.5 * m + rho);
 362   assert(Fc >= 0 && Fc <= 1);
 363   lsx_debug("make_lpf(n=%i Fc=%.7g β=%g ρ=%g dc-norm=%i scale=%g)", num_taps, Fc, beta, rho, dc_norm, scale);
 364
 365   if (!h)
 366     return NULL;
 367
 368   for (i = 0; i <= m / 2; ++i) {
 369     double z = i - .5 * m, x = z * M_PI, y = z * mult1;
 370     h[i] = x? sin(Fc * x) / x : Fc;
 371     sum += h[i] *= lsx_bessel_I_0(beta * sqrt(1 - y * y)) * mult;
 372     if (m - i != i)
 373       sum += h[m - i] = h[i];
 374   }
 375   for (i = 0; dc_norm && i < num_taps; ++i) h[i] *= scale / sum;
 376   return h;
 377 }
 378
 379 void lsx_kaiser_params(double att, double Fc, double tr_bw, double * beta, int * num_taps)
 380 {
 381   *beta = *beta < 0? lsx_kaiser_beta(att, tr_bw * .5 / Fc): *beta;
 382   att = att < 60? (att - 7.95) / (2.285 * M_PI * 2) :
 383     ((.0007528358-1.577737e-05**beta)**beta+.6248022)**beta+.06186902;
 384   *num_taps = !*num_taps? ceil(att/tr_bw + 1) : *num_taps;
 385 }
 386
 387 double * lsx_design_lpf(
 388     double Fp,      /* End of pass-band */
 389     double Fs,      /* Start of stop-band */
 390     double Fn,      /* Nyquist freq; e.g. 0.5, 1, PI */
 391     double att,     /* Stop-band attenuation in dB */
 392     int * num_taps, /* 0: value will be estimated */
 393     int k,          /* >0: number of phases; <0: num_taps ≡ 1 (mod -k) */
 394     double beta)    /* <0: value will be estimated */
 395 {
 396   int n = *num_taps, phases = max(k, 1), modulo = max(-k, 1);
 397   double tr_bw, Fc, rho = phases == 1? .5 : att < 120? .63 : .75;
 398
 399   Fp /= fabs(Fn), Fs /= fabs(Fn);        /* Normalise to Fn = 1 */
 400   tr_bw = .5 * (Fs - Fp); /* Transition band-width: 6dB to stop points */
 401   tr_bw /= phases, Fs /= phases;
 402   tr_bw = min(tr_bw, .5 * Fs);
 403   Fc = Fs - tr_bw;
 404   assert(Fc - tr_bw >= 0);
 405   lsx_kaiser_params(att, Fc, tr_bw, &beta, num_taps);
 406   if (!n)
 407     *num_taps = phases > 1? *num_taps / phases * phases + phases - 1 : (*num_taps + modulo - 2) / modulo * modulo + 1;
 408   return Fn < 0? 0 : lsx_make_lpf(
 409       *num_taps, Fc, beta, rho, (double)phases, sox_false);
 410 }
 411
 412 static double safe_log(double x)
 413 {
 414   assert(x >= 0);
 415   if (x)
 416     return log(x);
 417   lsx_debug("log(0)");
 418   return -26;
 419 }
 420
 421 void lsx_fir_to_phase(double * * h, int * len, int * post_len, double phase)
 422 {
 423   double * pi_wraps, * work, phase1 = (phase > 50 ? 100 - phase : phase) / 50;
 424   int i, work_len, begin, end, imp_peak = 0, peak = 0;
 425   double imp_sum = 0, peak_imp_sum = 0;
 426   double prev_angle2 = 0, cum_2pi = 0, prev_angle1 = 0, cum_1pi = 0;
 427
 428   for (i = *len, work_len = 2 * 2 * 8; i > 1; work_len <<= 1, i >>= 1);
 429
 430   work = lsx_calloc((size_t)work_len + 2, sizeof(*work)); /* +2: (UN)PACK */
 431   pi_wraps = lsx_malloc((((size_t)work_len + 2) / 2) * sizeof(*pi_wraps));
 432
 433   memcpy(work, *h, *len * sizeof(*work));
 434   lsx_safe_rdft(work_len, 1, work); /* Cepstral: */
 435   LSX_UNPACK(work, work_len);
 436
 437   for (i = 0; i <= work_len; i += 2) {
 438     double angle = atan2(work[i + 1], work[i]);
 439     double detect = 2 * M_PI;
 440     double delta = angle - prev_angle2;
 441     double adjust = detect * ((delta < -detect * .7) - (delta > detect * .7));
 442     prev_angle2 = angle;
 443     cum_2pi += adjust;
 444     angle += cum_2pi;
 445     detect = M_PI;
 446     delta = angle - prev_angle1;
 447     adjust = detect * ((delta < -detect * .7) - (delta > detect * .7));
 448     prev_angle1 = angle;
 449     cum_1pi += fabs(adjust); /* fabs for when 2pi and 1pi have combined */
 450     pi_wraps[i >> 1] = cum_1pi;
 451
 452     work[i] = safe_log(sqrt(sqr(work[i]) + sqr(work[i + 1])));
 453     work[i + 1] = 0;
 454   }
 455   LSX_PACK(work, work_len);
 456   lsx_safe_rdft(work_len, -1, work);
 457   for (i = 0; i < work_len; ++i) work[i] *= 2. / work_len;
 458
 459   for (i = 1; i < work_len / 2; ++i) { /* Window to reject acausal components */
 460     work[i] *= 2;
 461     work[i + work_len / 2] = 0;
 462   }
 463   lsx_safe_rdft(work_len, 1, work);
 464
 465   for (i = 2; i < work_len; i += 2) /* Interpolate between linear & min phase */
 466     work[i + 1] = phase1 * i / work_len * pi_wraps[work_len >> 1] +
 467         (1 - phase1) * (work[i + 1] + pi_wraps[i >> 1]) - pi_wraps[i >> 1];
 468
 469   work[0] = exp(work[0]), work[1] = exp(work[1]);
 470   for (i = 2; i < work_len; i += 2) {
 471     double x = exp(work[i]);
 472     work[i    ] = x * cos(work[i + 1]);
 473     work[i + 1] = x * sin(work[i + 1]);
 474   }
 475
 476   lsx_safe_rdft(work_len, -1, work);
 477   for (i = 0; i < work_len; ++i) work[i] *= 2. / work_len;
 478
 479   /* Find peak pos. */
 480   for (i = 0; i <= (int)(pi_wraps[work_len >> 1] / M_PI + .5); ++i) {
 481     imp_sum += work[i];
 482     if (fabs(imp_sum) > fabs(peak_imp_sum)) {
 483       peak_imp_sum = imp_sum;
 484       peak = i;
 485     }
 486     if (work[i] > work[imp_peak]) /* For debug check only */
 487       imp_peak = i;
 488   }
 489   while (peak && fabs(work[peak-1]) > fabs(work[peak]) && work[peak-1] * work[peak] > 0)
 490     --peak;
 491
 492   if (!phase1)
 493     begin = 0;
 494   else if (phase1 == 1)
 495     begin = peak - *len / 2;
 496   else {
 497     begin = (.997 - (2 - phase1) * .22) * *len + .5;
 498     end   = (.997 + (0 - phase1) * .22) * *len + .5;
 499     begin = peak - (begin & ~3);
 500     end   = peak + 1 + ((end + 3) & ~3);
 501     *len = end - begin;
 502     *h = lsx_realloc(*h, *len * sizeof(**h));
 503   }
 504   for (i = 0; i < *len; ++i) (*h)[i] =
 505     work[(begin + (phase > 50 ? *len - 1 - i : i) + work_len) & (work_len - 1)];
 506   *post_len = phase > 50 ? peak - begin : begin + *len - (peak + 1);
 507
 508   lsx_debug("nPI=%g peak-sum@%i=%g (val@%i=%g); len=%i post=%i (%g%%)",
 509       pi_wraps[work_len >> 1] / M_PI, peak, peak_imp_sum, imp_peak,
 510       work[imp_peak], *len, *post_len, 100 - 100. * *post_len / (*len - 1));
 511   free(pi_wraps), free(work);
 512 }
 513
 514 void lsx_plot_fir(double * h, int num_points, sox_rate_t rate, sox_plot_t type, char const * title, double y1, double y2)
 515 {
 516   int i, N = lsx_set_dft_length(num_points);
 517   if (type == sox_plot_gnuplot) {
 518     double * h1 = lsx_calloc(N, sizeof(*h1));
 519     double * H = lsx_malloc((N / 2 + 1) * sizeof(*H));
 520     memcpy(h1, h, sizeof(*h1) * num_points);
 521     lsx_power_spectrum(N, h1, H);
 522     printf(
 523         "# gnuplot file\n"
 524         "set title '%s'\n"
 525         "set xlabel 'Frequency (Hz)'\n"
 526         "set ylabel 'Amplitude Response (dB)'\n"
 527         "set grid xtics ytics\n"
 528         "set key off\n"
 529         "plot '-' with lines\n"
 530         , title);
 531     for (i = 0; i <= N/2; ++i)
 532       printf("%g %g\n", i * rate / N, 10 * log10(H[i]));
 533     printf(
 534         "e\n"
 535         "pause -1 'Hit return to continue'\n");
 536     free(H);
 537     free(h1);
 538   }
 539   else if (type == sox_plot_octave) {
 540     printf("%% GNU Octave file (may also work with MATLAB(R) )\nb=[");
 541     for (i = 0; i < num_points; ++i)
 542       printf("%24.16e\n", h[i]);
 543     printf("];\n"
 544         "[h,w]=freqz(b,1,%i);\n"
 545         "plot(%g*w/pi,20*log10(h))\n"
 546         "title('%s')\n"
 547         "xlabel('Frequency (Hz)')\n"
 548         "ylabel('Amplitude Response (dB)')\n"
 549         "grid on\n"
 550         "axis([0 %g %g %g])\n"
 551         "disp('Hit return to continue')\n"
 552         "pause\n"
 553         , N, rate * .5, title, rate * .5, y1, y2);
 554   }
 555   else if (type == sox_plot_data) {
 556     printf("# %s\n"
 557         "# FIR filter\n"
 558         "# rate: %g\n"
 559         "# name: b\n"
 560         "# type: matrix\n"
 561         "# rows: %i\n"
 562         "# columns: 1\n", title, rate, num_points);
 563     for (i = 0; i < num_points; ++i)
 564       printf("%24.16e\n", h[i]);
 565   }
 566 }
 567
 568 #if HAVE_FENV_H
 569   #include <fenv.h>
 570   #if defined FE_INVALID
 571     #if HAVE_LRINT && LONG_MAX == 2147483647
 572       #define lrint32 lrint
 573     #elif defined __GNUC__ && defined __x86_64__
 574       #define lrint32 lrint32
 575       static __inline sox_int32_t lrint32(double input) {
 576         sox_int32_t result;
 577         __asm__ __volatile__("fistpl %0": "=m"(result): "t"(input): "st");
 578         return result;
 579       }
 580     #endif
 581   #endif
 582 #endif
 583
 584 #if defined lrint32
 585 #define _ dest[i] = lrint32(src[i]), ++i,
 586 #pragma STDC FENV_ACCESS ON
 587
 588 static void rint_clip(sox_sample_t * const dest, double const * const src,
 589     size_t i, size_t const n, sox_uint64_t * const clips)
 590 {
 591   for (; i < n; ++i) {
 592     dest[i] = lrint32(src[i]);
 593     if (fetestexcept(FE_INVALID)) {
 594       feclearexcept(FE_INVALID);
 595       dest[i] = src[i] > 0? SOX_SAMPLE_MAX : SOX_SAMPLE_MIN;
 596       ++*clips;
 597     }
 598   }
 599 }
 600
 601 void lsx_save_samples(sox_sample_t * const dest, double const * const src,
 602     size_t const n, sox_uint64_t * const clips)
 603 {
 604   size_t i;
 605   feclearexcept(FE_INVALID);
 606   for (i = 0; i < (n & ~7);) {
 607     _ _ _ _ _ _ _ _ 0;
 608     if (fetestexcept(FE_INVALID)) {
 609       feclearexcept(FE_INVALID);
 610       rint_clip(dest, src, i - 8, i, clips);
 611     }
 612   }
 613   rint_clip(dest, src, i, n, clips);
 614 }
 615
 616 void lsx_load_samples(double * const dest, sox_sample_t const * const src,
 617     size_t const n)
 618 {
 619   size_t i;
 620   for (i = 0; i < n; ++i)
 621     dest[i] = src[i];
 622 }
 623
 624 #pragma STDC FENV_ACCESS OFF
 625 #undef _
 626 #else
 627
 628 void lsx_save_samples(sox_sample_t * const dest, double const * const src,
 629     size_t const n, sox_uint64_t * const clips)
 630 {
 631   SOX_SAMPLE_LOCALS;
 632   size_t i;
 633   for (i = 0; i < n; ++i)
 634     dest[i] = SOX_FLOAT_64BIT_TO_SAMPLE(src[i], *clips);
 635 }
 636
 637 void lsx_load_samples(double * const dest, sox_sample_t const * const src,
 638     size_t const n)
 639 {
 640   size_t i;
 641   for (i = 0; i < n; ++i)
 642     dest[i] = SOX_SAMPLE_TO_FLOAT_64BIT(src[i],);
 643 }
 644
 645 #endif