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
check_amplitude(struct bat * bat,float * buf)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 */
check_peak(struct bat * bat,struct analyze * a,int end,int peak,float hz,float mean,float p,int channel,int start)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 */
check(struct bat * bat,struct analyze * a,int channel)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
calc_magnitude(struct bat * bat,struct analyze * a,int N)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
find_and_check_harmonics(struct bat * bat,struct analyze * a,int channel)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
calculate_noise_one_period(struct bat * bat,struct noise_analyzer * na,float * src,int length,int channel)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
calculate_noise(struct bat * bat,float * src,int channel)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
find_and_check_noise(struct bat * bat,void * buf,int channel)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 */
reorder_data(struct bat * bat)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 */
truncate_frames(struct bat * bat)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
analyze_capture(struct bat * bat)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 }
514