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