bat/analyze.c

   1 /*
   2  * Copyright (C) 2013-2015 Intel Corporation
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  */
  15
  16 #include <stdio.h>
  17 #include <stdlib.h>
  18 #include <errno.h>
  19 #include <stdbool.h>
  20 #include <stdint.h>
  21
  22 #include <math.h>
  23 #include <fftw3.h>
  24
  25 #include "aconfig.h"
  26 #include "gettext.h"
  27
  28 #include "common.h"
  29 #include "bat-signal.h"
  30
  31 static void check_amplitude(struct bat *bat, float *buf)
  32 {
  33         float sum, average, amplitude;
  34         int i, percent;
  35
  36         /* calculate average value */
  37         for (i = 0, sum = 0.0, average = 0.0; i < bat->frames; i++)
  38                 sum += buf[i];
  39         average = sum / bat->frames;
  40
  41         /* calculate peak-to-average amplitude */
  42         for (i = 0, sum = 0.0; i < bat->frames; i++)
  43                 sum += fabsf(buf[i] - average);
  44         amplitude = sum / bat->frames * M_PI / 2.0;
  45
  46         /* calculate amplitude percentage against full range */
  47         percent = amplitude * 100 / ((1 << ((bat->sample_size << 3) - 1)) - 1);
  48
  49         fprintf(bat->log, _("Amplitude: %.1f; Percentage: [%d]\n"),
  50                         amplitude, percent);
  51         if (percent < 0)
  52                 fprintf(bat->err, _("ERROR: Amplitude can't be negative!\n"));
  53         else if (percent < 1)
  54                 fprintf(bat->err, _("WARNING: Signal too weak!\n"));
  55         else if (percent > 100)
  56                 fprintf(bat->err, _("WARNING: Signal overflow!\n"));
  57 }
  58
  59 /**
  60  *
  61  * @return 0 if peak detected at right frequency,
  62  *         1 if peak detected somewhere else
  63  *         2 if DC detected
  64  */
  65 int check_peak(struct bat *bat, struct analyze *a, int end, int peak, float hz,
  66                 float mean, float p, int channel, int start)
  67 {
  68         int err;
  69         float hz_peak = (float) (peak) * hz;
  70         float delta_rate = DELTA_RATE * bat->target_freq[channel];
  71         float delta_HZ = DELTA_HZ;
  72         float tolerance = (delta_rate > delta_HZ) ? delta_rate : delta_HZ;
  73
  74         fprintf(bat->log, _("Detected peak at %2.2f Hz of %2.2f dB\n"), hz_peak,
  75                         10.0 * log10f(a->mag[peak] / mean));
  76         fprintf(bat->log, _(" Total %3.1f dB from %2.2f to %2.2f Hz\n"),
  77                         10.0 * log10f(p / mean), start * hz, end * hz);
  78
  79         if (hz_peak < DC_THRESHOLD) {
  80                 fprintf(bat->err, _(" WARNING: Found low peak %2.2f Hz,"),
  81                                 hz_peak);
  82                 fprintf(bat->err, _(" very close to DC\n"));
  83                 err = FOUND_DC;
  84         } else if (hz_peak < bat->target_freq[channel] - tolerance) {
  85                 fprintf(bat->err, _(" FAIL: Peak freq too low %2.2f Hz\n"),
  86                                 hz_peak);
  87                 err = FOUND_WRONG_PEAK;
  88         } else if (hz_peak > bat->target_freq[channel] + tolerance) {
  89                 fprintf(bat->err, _(" FAIL: Peak freq too high %2.2f Hz\n"),
  90                                 hz_peak);
  91                 err = FOUND_WRONG_PEAK;
  92         } else {
  93                 fprintf(bat->log, _(" PASS: Peak detected"));
  94                 fprintf(bat->log, _(" at target frequency\n"));
  95                 err = 0;
  96         }
  97
  98         return err;
  99 }
 100
 101 /**
 102  * Search for main frequencies in fft results and compare it to target
 103  */
 104 static int check(struct bat *bat, struct analyze *a, int channel)
 105 {
 106         float hz = 1.0 / ((float) bat->frames / (float) bat->rate);
 107         float mean = 0.0, t, sigma = 0.0, p = 0.0;
 108         int i, start = -1, end = -1, peak = 0, signals = 0;
 109         int err = 0, N = bat->frames / 2;
 110
 111         /* calculate mean */
 112         for (i = 0; i < N; i++)
 113                 mean += a->mag[i];
 114         mean /= (float) N;
 115
 116         /* calculate standard deviation */
 117         for (i = 0; i < N; i++) {
 118                 t = a->mag[i] - mean;
 119                 t *= t;
 120                 sigma += t;
 121         }
 122         sigma /= (float) N;
 123         sigma = sqrtf(sigma);
 124
 125         /* clip any data less than k sigma + mean */
 126         for (i = 0; i < N; i++) {
 127                 if (a->mag[i] > mean + bat->sigma_k * sigma) {
 128
 129                         /* find peak start points */
 130                         if (start == -1) {
 131                                 start = peak = end = i;
 132                                 signals++;
 133                         } else {
 134                                 if (a->mag[i] > a->mag[peak])
 135                                         peak = i;
 136                                 end = i;
 137                         }
 138                         p += a->mag[i];
 139                 } else if (start != -1) {
 140                         /* Check if peak is as expected */
 141                         err |= check_peak(bat, a, end, peak, hz, mean,
 142                                         p, channel, start);
 143                         end = start = -1;
 144                         if (signals == MAX_PEAKS)
 145                                 break;
 146                 }
 147         }
 148         if (signals == 0)
 149                 err = -ENOPEAK; /* No peak detected */
 150         else if ((err == FOUND_DC) && (signals == 1))
 151                 err = -EONLYDC; /* Only DC detected */
 152         else if ((err & FOUND_WRONG_PEAK) == FOUND_WRONG_PEAK)
 153                 err = -EBADPEAK; /* Bad peak detected */
 154         else
 155                 err = 0; /* Correct peak detected */
 156
 157         fprintf(bat->log, _("Detected at least %d signal(s) in total\n"),
 158                         signals);
 159
 160         return err;
 161 }
 162
 163 static void calc_magnitude(struct bat *bat, struct analyze *a, int N)
 164 {
 165         float r2, i2;
 166         int i;
 167
 168         for (i = 1; i < N / 2; i++) {
 169                 r2 = a->out[i] * a->out[i];
 170                 i2 = a->out[N - i] * a->out[N - i];
 171
 172                 a->mag[i] = sqrtf(r2 + i2);
 173         }
 174         a->mag[0] = 0.0;
 175 }
 176
 177 static int find_and_check_harmonics(struct bat *bat, struct analyze *a,
 178                 int channel)
 179 {
 180         fftwf_plan p;
 181         int err = -ENOMEM, N = bat->frames;
 182
 183         /* Allocate FFT buffers */
 184         a->in = (float *) fftwf_malloc(sizeof(float) * bat->frames);
 185         if (a->in == NULL)
 186                 goto out1;
 187
 188         a->out = (float *) fftwf_malloc(sizeof(float) * bat->frames);
 189         if (a->out == NULL)
 190                 goto out2;
 191
 192         a->mag = (float *) fftwf_malloc(sizeof(float) * bat->frames);
 193         if (a->mag == NULL)
 194                 goto out3;
 195
 196         /* create FFT plan */
 197         p = fftwf_plan_r2r_1d(N, a->in, a->out, FFTW_R2HC,
 198                         FFTW_MEASURE | FFTW_PRESERVE_INPUT);
 199         if (p == NULL)
 200                 goto out4;
 201
 202         /* convert source PCM to floats */
 203         bat->convert_sample_to_float(a->buf, a->in, bat->frames);
 204
 205         /* check amplitude */
 206         check_amplitude(bat, a->in);
 207
 208         /* run FFT */
 209         fftwf_execute(p);
 210
 211         /* FFT out is real and imaginary numbers - calc magnitude for each */
 212         calc_magnitude(bat, a, N);
 213
 214         /* check data */
 215         err = check(bat, a, channel);
 216
 217         fftwf_destroy_plan(p);
 218
 219 out4:
 220         fftwf_free(a->mag);
 221 out3:
 222         fftwf_free(a->out);
 223 out2:
 224         fftwf_free(a->in);
 225 out1:
 226         return err;
 227 }
 228
 229 static int calculate_noise_one_period(struct bat *bat,
 230                 struct noise_analyzer *na, float *src,
 231                 int length, int channel)
 232 {
 233         int i, shift = 0;
 234         float tmp, rms, gain, residual;
 235         float a = 0.0, b = 1.0;
 236
 237         /* step 1. phase compensation */
 238
 239         if (length < 2 * na->nsamples)
 240                 return -EINVAL;
 241
 242         /* search for the beginning of a sine period */
 243         for (i = 0, tmp = 0.0, shift = -1; i < na->nsamples; i++) {
 244                 /* find i where src[i] >= 0 && src[i+1] < 0 */
 245                 if (src[i] < 0.0)
 246                         continue;
 247                 if (src[i + 1] < 0.0) {
 248                         tmp = src[i] - src[i + 1];
 249                         a = src[i] / tmp;
 250                         b = -src[i + 1] / tmp;
 251                         shift = i;
 252                         break;
 253                 }
 254         }
 255
 256         /* didn't find the beginning of a sine period */
 257         if (shift == -1)
 258                 return -EINVAL;
 259
 260         /* shift sine waveform to source[0] = 0.0 */
 261         for (i = 0; i < na->nsamples; i++)
 262                 na->source[i] = a * src[i + shift + 1] + b * src[i + shift];
 263
 264         /* step 2. gain compensation */
 265
 266         /* calculate rms of signal amplitude */
 267         for (i = 0, tmp = 0.0; i < na->nsamples; i++)
 268                 tmp += na->source[i] * na->source[i];
 269         rms = sqrtf(tmp / na->nsamples);
 270
 271         gain = na->rms_tgt / rms;
 272
 273         for (i = 0; i < na->nsamples; i++)
 274                 na->source[i] *= gain;
 275
 276         /* step 3. calculate snr in dB */
 277
 278         for (i = 0, tmp = 0.0, residual = 0.0; i < na->nsamples; i++) {
 279                 tmp = fabsf(na->target[i] - na->source[i]);
 280                 residual += tmp * tmp;
 281         }
 282
 283         tmp = na->rms_tgt / sqrtf(residual / na->nsamples);
 284         na->snr_db = 20.0 * log10f(tmp);
 285
 286         return 0;
 287 }
 288
 289 static int calculate_noise(struct bat *bat, float *src, int channel)
 290 {
 291         int err = 0;
 292         struct noise_analyzer na;
 293         float freq = bat->target_freq[channel];
 294         float tmp, sum_snr_pc, avg_snr_pc, avg_snr_db;
 295         int offset, i, cnt_noise, cnt_clean;
 296         /* num of samples in each sine period */
 297         int nsamples = (int) ceilf(bat->rate / freq);
 298         /* each section has 2 sine periods, the first one for locating
 299          * and the second one for noise calculating */
 300         int nsamples_per_section = nsamples * 2;
 301         /* all sine periods will be calculated except the first one */
 302         int nsection = bat->frames / nsamples - 1;
 303
 304         fprintf(bat->log, _("samples per period: %d\n"), nsamples);
 305         fprintf(bat->log, _("total sections to detect: %d\n"), nsection);
 306         na.source = (float *)malloc(sizeof(float) * nsamples);
 307         if (!na.source) {
 308                 err = -ENOMEM;
 309                 goto out1;
 310         }
 311
 312         na.target = (float *)malloc(sizeof(float) * nsamples);
 313         if (!na.target) {
 314                 err = -ENOMEM;
 315                 goto out2;
 316         }
 317
 318         /* generate standard single-tone signal */
 319         err = generate_sine_wave_raw_mono(bat, na.target, freq, nsamples);
 320         if (err < 0)
 321                 goto out3;
 322
 323         na.nsamples = nsamples;
 324
 325         /* calculate rms of standard signal */
 326         for (i = 0, tmp = 0.0; i < nsamples; i++)
 327                 tmp += na.target[i] * na.target[i];
 328         na.rms_tgt = sqrtf(tmp / nsamples);
 329
 330         /* calculate average noise level */
 331         sum_snr_pc = 0.0;
 332         cnt_clean = cnt_noise = 0;
 333         for (i = 0, offset = 0; i < nsection; i++) {
 334                 na.snr_db = SNR_DB_INVALID;
 335
 336                 err = calculate_noise_one_period(bat, &na, src + offset,
 337                                 nsamples_per_section, channel);
 338                 if (err < 0)
 339                         goto out3;
 340
 341                 if (na.snr_db > bat->snr_thd_db) {
 342                         cnt_clean++;
 343                         sum_snr_pc += 100.0 / powf(10.0, na.snr_db / 20.0);
 344                 } else {
 345                         cnt_noise++;
 346                 }
 347                 offset += nsamples;
 348         }
 349
 350         if (cnt_noise > 0) {
 351                 fprintf(bat->err, _("Noise detected at %d points.\n"),
 352                                 cnt_noise);
 353                 err = -cnt_noise;
 354                 if (cnt_clean == 0)
 355                         goto out3;
 356         } else {
 357                 fprintf(bat->log, _("No noise detected.\n"));
 358         }
 359
 360         avg_snr_pc = sum_snr_pc / cnt_clean;
 361         avg_snr_db = 20.0 * log10f(100.0 / avg_snr_pc);
 362         fprintf(bat->log, _("Average SNR is %.2f dB (%.2f %%) at %d points.\n"),
 363                         avg_snr_db, avg_snr_pc, cnt_clean);
 364
 365 out3:
 366         free(na.target);
 367 out2:
 368         free(na.source);
 369 out1:
 370         return err;
 371 }
 372
 373 static int find_and_check_noise(struct bat *bat, void *buf, int channel)
 374 {
 375         int err = 0;
 376         float *source;
 377
 378         source = (float *)malloc(sizeof(float) * bat->frames);
 379         if (!source)
 380                 return -ENOMEM;
 381
 382         /* convert source PCM to floats */
 383         bat->convert_sample_to_float(buf, source, bat->frames);
 384
 385         /* adjust waveform and calculate noise */
 386         err = calculate_noise(bat, source, channel);
 387
 388         free(source);
 389         return err;
 390 }
 391
 392 /**
 393  * Convert interleaved samples from channels in samples from a single channel
 394  */
 395 static int reorder_data(struct bat *bat)
 396 {
 397         char *p, *new_bat_buf;
 398         int ch, i, j;
 399
 400         if (bat->channels == 1)
 401                 return 0; /* No need for reordering */
 402
 403         p = malloc(bat->frames * bat->frame_size);
 404         new_bat_buf = p;
 405         if (p == NULL)
 406                 return -ENOMEM;
 407
 408         for (ch = 0; ch < bat->channels; ch++) {
 409                 for (j = 0; j < bat->frames; j++) {
 410                         for (i = 0; i < bat->sample_size; i++) {
 411                                 *p++ = ((char *) (bat->buf))[j * bat->frame_size
 412                                                 + ch * bat->sample_size + i];
 413                         }
 414                 }
 415         }
 416
 417         free(bat->buf);
 418         bat->buf = new_bat_buf;
 419
 420         return 0;
 421 }
 422
 423 /* truncate sample frames for faster FFT analysis process */
 424 static int truncate_frames(struct bat *bat)
 425 {
 426         int shift = SHIFT_MAX;
 427
 428         for (; shift > SHIFT_MIN; shift--)
 429                 if (bat->frames & (1 << shift)) {
 430                         bat->frames = 1 << shift;
 431                         return 0;
 432                 }
 433
 434         return -EINVAL;
 435 }
 436
 437 int analyze_capture(struct bat *bat)
 438 {
 439         int err = 0;
 440         size_t items;
 441         int c;
 442         struct analyze a;
 443
 444         err = truncate_frames(bat);
 445         if (err < 0) {
 446                 fprintf(bat->err, _("Invalid frame number for analysis: %d\n"),
 447                                 bat->frames);
 448                 return err;
 449         }
 450
 451         fprintf(bat->log, _("\nBAT analysis: signal has %d frames at %d Hz,"),
 452                         bat->frames, bat->rate);
 453         fprintf(bat->log, _(" %d channels, %d bytes per sample.\n"),
 454                         bat->channels, bat->sample_size);
 455
 456         bat->buf = malloc(bat->frames * bat->frame_size);
 457         if (bat->buf == NULL)
 458                 return -ENOMEM;
 459
 460         bat->fp = fopen(bat->capture.file, "rb");
 461         err = -errno;
 462         if (bat->fp == NULL) {
 463                 fprintf(bat->err, _("Cannot open file: %s %d\n"),
 464                                 bat->capture.file, err);
 465                 goto exit1;
 466         }
 467
 468         /* Skip header */
 469         err = read_wav_header(bat, bat->capture.file, bat->fp, true);
 470         if (err != 0)
 471                 goto exit2;
 472
 473         items = fread(bat->buf, bat->frame_size, bat->frames, bat->fp);
 474         if (items != bat->frames) {
 475                 err = -EIO;
 476                 goto exit2;
 477         }
 478
 479         err = reorder_data(bat);
 480         if (err != 0)
 481                 goto exit2;
 482
 483         for (c = 0; c < bat->channels; c++) {
 484                 fprintf(bat->log, _("\nChannel %i - "), c + 1);
 485                 fprintf(bat->log, _("Checking for target frequency %2.2f Hz\n"),
 486                                 bat->target_freq[c]);
 487                 a.buf = bat->buf +
 488                                 c * bat->frames * bat->frame_size
 489                                 / bat->channels;
 490                 if (!bat->standalone) {
 491                         err = find_and_check_harmonics(bat, &a, c);
 492                         if (err != 0)
 493                                 goto exit2;
 494                 }
 495
 496                 if (snr_is_valid(bat->snr_thd_db)) {
 497                         fprintf(bat->log, _("\nChecking for SNR: "));
 498                         fprintf(bat->log, _("Threshold is %.2f dB (%.2f%%)\n"),
 499                                         bat->snr_thd_db, 100.0
 500                                         / powf(10.0, bat->snr_thd_db / 20.0));
 501                         err = find_and_check_noise(bat, a.buf, c);
 502                         if (err != 0)
 503                                 goto exit2;
 504                 }
 505         }
 506
 507 exit2:
 508         fclose(bat->fp);
 509 exit1:
 510         free(bat->buf);
 511
 512         return err;
 513 }