xref: /third_party/alsa-utils/bat/analyze.c

/*
 * Copyright (C) 2013-2015 Intel Corporation
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 */

#include "aconfig.h"

#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <stdbool.h>
#include <stdint.h>

#include <math.h>
#include <fftw3.h>

#include "gettext.h"

#include "common.h"
#include "bat-signal.h"

static void check_amplitude(struct bat *bat, float *buf)
{
	float sum, average, amplitude;
	int i, percent;

	/* calculate average value */
	for (i = 0, sum = 0.0, average = 0.0; i < bat->frames; i++)
		sum += buf[i];
	average = sum / bat->frames;

	/* calculate peak-to-average amplitude */
	for (i = 0, sum = 0.0; i < bat->frames; i++)
		sum += fabsf(buf[i] - average);
	amplitude = sum / bat->frames * M_PI / 2.0;

	/* calculate amplitude percentage against full range */
	percent = amplitude * 100 / ((1 << ((bat->sample_size << 3) - 1)) - 1);

	fprintf(bat->log, _("Amplitude: %.1f; Percentage: [%d]\n"),
			amplitude, percent);
	if (percent < 0)
		fprintf(bat->err, _("ERROR: Amplitude can't be negative!\n"));
	else if (percent < 1)
		fprintf(bat->err, _("WARNING: Signal too weak!\n"));
	else if (percent > 100)
		fprintf(bat->err, _("WARNING: Signal overflow!\n"));
}

/**
 *
 * @return 0 if peak detected at right frequency,
 *         1 if peak detected somewhere else
 *         2 if DC detected
 */
int check_peak(struct bat *bat, struct analyze *a, int end, int peak, float hz,
		float mean, float p, int channel, int start)
{
	int err;
	float hz_peak = (float) (peak) * hz;
	float delta_rate = DELTA_RATE * bat->target_freq[channel];
	float delta_HZ = DELTA_HZ;
	float tolerance = (delta_rate > delta_HZ) ? delta_rate : delta_HZ;

	fprintf(bat->log, _("Detected peak at %2.2f Hz of %2.2f dB\n"), hz_peak,
			10.0 * log10f(a->mag[peak] / mean));
	fprintf(bat->log, _(" Total %3.1f dB from %2.2f to %2.2f Hz\n"),
			10.0 * log10f(p / mean), start * hz, end * hz);

	if (hz_peak < DC_THRESHOLD) {
		fprintf(bat->err, _(" WARNING: Found low peak %2.2f Hz,"),
				hz_peak);
		fprintf(bat->err, _(" very close to DC\n"));
		err = FOUND_DC;
	} else if (hz_peak < bat->target_freq[channel] - tolerance) {
		fprintf(bat->err, _(" FAIL: Peak freq too low %2.2f Hz\n"),
				hz_peak);
		err = FOUND_WRONG_PEAK;
	} else if (hz_peak > bat->target_freq[channel] + tolerance) {
		fprintf(bat->err, _(" FAIL: Peak freq too high %2.2f Hz\n"),
				hz_peak);
		err = FOUND_WRONG_PEAK;
	} else {
		fprintf(bat->log, _(" PASS: Peak detected"));
		fprintf(bat->log, _(" at target frequency\n"));
		err = 0;
	}

	return err;
}

/**
 * Search for main frequencies in fft results and compare it to target
 */
static int check(struct bat *bat, struct analyze *a, int channel)
{
	float hz = 1.0 / ((float) bat->frames / (float) bat->rate);
	float mean = 0.0, t, sigma = 0.0, p = 0.0;
	int i, start = -1, end = -1, peak = 0, signals = 0;
	int err = 0, N = bat->frames / 2;

	/* calculate mean */
	for (i = 0; i < N; i++)
		mean += a->mag[i];
	mean /= (float) N;

	/* calculate standard deviation */
	for (i = 0; i < N; i++) {
		t = a->mag[i] - mean;
		t *= t;
		sigma += t;
	}
	sigma /= (float) N;
	sigma = sqrtf(sigma);

	/* clip any data less than k sigma + mean */
	for (i = 0; i < N; i++) {
		if (a->mag[i] > mean + bat->sigma_k * sigma) {

			/* find peak start points */
			if (start == -1) {
				start = peak = end = i;
				signals++;
			} else {
				if (a->mag[i] > a->mag[peak])
					peak = i;
				end = i;
			}
			p += a->mag[i];
		} else if (start != -1) {
			/* Check if peak is as expected */
			err |= check_peak(bat, a, end, peak, hz, mean,
					p, channel, start);
			end = start = -1;
			if (signals == MAX_PEAKS)
				break;
		}
	}
	if (signals == 0)
		err = -ENOPEAK; /* No peak detected */
	else if ((err == FOUND_DC) && (signals == 1))
		err = -EONLYDC; /* Only DC detected */
	else if ((err & FOUND_WRONG_PEAK) == FOUND_WRONG_PEAK)
		err = -EBADPEAK; /* Bad peak detected */
	else
		err = 0; /* Correct peak detected */

	fprintf(bat->log, _("Detected at least %d signal(s) in total\n"),
			signals);

	return err;
}

static void calc_magnitude(struct bat *bat, struct analyze *a, int N)
{
	float r2, i2;
	int i;

	for (i = 1; i < N / 2; i++) {
		r2 = a->out[i] * a->out[i];
		i2 = a->out[N - i] * a->out[N - i];

		a->mag[i] = sqrtf(r2 + i2);
	}
	a->mag[0] = 0.0;
}

static int find_and_check_harmonics(struct bat *bat, struct analyze *a,
		int channel)
{
	fftwf_plan p;
	int err = -ENOMEM, N = bat->frames;

	/* Allocate FFT buffers */
	a->in = (float *) fftwf_malloc(sizeof(float) * bat->frames);
	if (a->in == NULL)
		goto out1;

	a->out = (float *) fftwf_malloc(sizeof(float) * bat->frames);
	if (a->out == NULL)
		goto out2;

	a->mag = (float *) fftwf_malloc(sizeof(float) * bat->frames);
	if (a->mag == NULL)
		goto out3;

	/* create FFT plan */
	p = fftwf_plan_r2r_1d(N, a->in, a->out, FFTW_R2HC,
			FFTW_MEASURE | FFTW_PRESERVE_INPUT);
	if (p == NULL)
		goto out4;

	/* convert source PCM to floats */
	bat->convert_sample_to_float(a->buf, a->in, bat->frames);

	/* check amplitude */
	check_amplitude(bat, a->in);

	/* run FFT */
	fftwf_execute(p);

	/* FFT out is real and imaginary numbers - calc magnitude for each */
	calc_magnitude(bat, a, N);

	/* check data */
	err = check(bat, a, channel);

	fftwf_destroy_plan(p);

out4:
	fftwf_free(a->mag);
out3:
	fftwf_free(a->out);
out2:
	fftwf_free(a->in);
out1:
	return err;
}

static int calculate_noise_one_period(struct bat *bat,
		struct noise_analyzer *na, float *src,
		int length, int channel)
{
	int i, shift = 0;
	float tmp, rms, gain, residual;
	float a = 0.0, b = 1.0;

	/* step 1. phase compensation */

	if (length < 2 * na->nsamples)
		return -EINVAL;

	/* search for the beginning of a sine period */
	for (i = 0, tmp = 0.0, shift = -1; i < na->nsamples; i++) {
		/* find i where src[i] >= 0 && src[i+1] < 0 */
		if (src[i] < 0.0)
			continue;
		if (src[i + 1] < 0.0) {
			tmp = src[i] - src[i + 1];
			a = src[i] / tmp;
			b = -src[i + 1] / tmp;
			shift = i;
			break;
		}
	}

	/* didn't find the beginning of a sine period */
	if (shift == -1)
		return -EINVAL;

	/* shift sine waveform to source[0] = 0.0 */
	for (i = 0; i < na->nsamples; i++)
		na->source[i] = a * src[i + shift + 1] + b * src[i + shift];

	/* step 2. gain compensation */

	/* calculate rms of signal amplitude */
	for (i = 0, tmp = 0.0; i < na->nsamples; i++)
		tmp += na->source[i] * na->source[i];
	rms = sqrtf(tmp / na->nsamples);

	gain = na->rms_tgt / rms;

	for (i = 0; i < na->nsamples; i++)
		na->source[i] *= gain;

	/* step 3. calculate snr in dB */

	for (i = 0, tmp = 0.0, residual = 0.0; i < na->nsamples; i++) {
		tmp = fabsf(na->target[i] - na->source[i]);
		residual += tmp * tmp;
	}

	tmp = na->rms_tgt / sqrtf(residual / na->nsamples);
	na->snr_db = 20.0 * log10f(tmp);

	return 0;
}

static int calculate_noise(struct bat *bat, float *src, int channel)
{
	int err = 0;
	struct noise_analyzer na;
	float freq = bat->target_freq[channel];
	float tmp, sum_snr_pc, avg_snr_pc, avg_snr_db;
	int offset, i, cnt_noise, cnt_clean;
	/* num of samples in each sine period */
	int nsamples = (int) ceilf(bat->rate / freq);
	/* each section has 2 sine periods, the first one for locating
	 * and the second one for noise calculating */
	int nsamples_per_section = nsamples * 2;
	/* all sine periods will be calculated except the first and last one */
	int nsection = bat->frames / nsamples - 1;

	fprintf(bat->log, _("samples per period: %d\n"), nsamples);
	fprintf(bat->log, _("total sections to detect: %d\n"), nsection);
	na.source = (float *)malloc(sizeof(float) * nsamples);
	if (!na.source) {
		err = -ENOMEM;
		goto out1;
	}

	na.target = (float *)malloc(sizeof(float) * nsamples);
	if (!na.target) {
		err = -ENOMEM;
		goto out2;
	}

	/* generate standard single-tone signal */
	err = generate_sine_wave_raw_mono(bat, na.target, freq, nsamples);
	if (err < 0)
		goto out3;

	na.nsamples = nsamples;

	/* calculate rms of standard signal */
	for (i = 0, tmp = 0.0; i < nsamples; i++)
		tmp += na.target[i] * na.target[i];
	na.rms_tgt = sqrtf(tmp / nsamples);

	/* calculate average noise level */
	sum_snr_pc = 0.0;
	cnt_clean = cnt_noise = 0;
	for (i = 1, offset = nsamples; i < nsection; i++) {
		na.snr_db = SNR_DB_INVALID;

		err = calculate_noise_one_period(bat, &na, src + offset,
				nsamples_per_section, channel);
		if (err < 0)
			goto out3;

		if (na.snr_db > bat->snr_thd_db) {
			cnt_clean++;
			sum_snr_pc += 100.0 / powf(10.0, na.snr_db / 20.0);
		} else {
			cnt_noise++;
		}
		offset += nsamples;
	}

	if (cnt_noise > 0) {
		fprintf(bat->err, _("Noise detected at %d points.\n"),
				cnt_noise);
		err = -cnt_noise;
		if (cnt_clean == 0)
			goto out3;
	} else {
		fprintf(bat->log, _("No noise detected.\n"));
	}

	avg_snr_pc = sum_snr_pc / cnt_clean;
	avg_snr_db = 20.0 * log10f(100.0 / avg_snr_pc);
	fprintf(bat->log, _("Average SNR is %.2f dB (%.2f %%) at %d points.\n"),
			avg_snr_db, avg_snr_pc, cnt_clean);

out3:
	free(na.target);
out2:
	free(na.source);
out1:
	return err;
}

static int find_and_check_noise(struct bat *bat, void *buf, int channel)
{
	int err = 0;
	float *source;

	source = (float *)malloc(sizeof(float) * bat->frames);
	if (!source)
		return -ENOMEM;

	/* convert source PCM to floats */
	bat->convert_sample_to_float(buf, source, bat->frames);

	/* adjust waveform and calculate noise */
	err = calculate_noise(bat, source, channel);

	free(source);
	return err;
}

/**
 * Convert interleaved samples from channels in samples from a single channel
 */
static int reorder_data(struct bat *bat)
{
	char *p, *new_bat_buf;
	int ch, i, j;

	if (bat->channels == 1)
		return 0; /* No need for reordering */

	p = malloc(bat->frames * bat->frame_size);
	new_bat_buf = p;
	if (p == NULL)
		return -ENOMEM;

	for (ch = 0; ch < bat->channels; ch++) {
		for (j = 0; j < bat->frames; j++) {
			for (i = 0; i < bat->sample_size; i++) {
				*p++ = ((char *) (bat->buf))[j * bat->frame_size
						+ ch * bat->sample_size + i];
			}
		}
	}

	free(bat->buf);
	bat->buf = new_bat_buf;

	return 0;
}

/* truncate sample frames for faster FFT analysis process */
static int truncate_frames(struct bat *bat)
{
	int shift = SHIFT_MAX;

	for (; shift > SHIFT_MIN; shift--)
		if (bat->frames & (1 << shift)) {
			bat->frames = 1 << shift;
			return 0;
		}

	return -EINVAL;
}

int analyze_capture(struct bat *bat)
{
	int err = 0;
	size_t items;
	int c;
	struct analyze a;

	err = truncate_frames(bat);
	if (err < 0) {
		fprintf(bat->err, _("Invalid frame number for analysis: %d\n"),
				bat->frames);
		return err;
	}

	fprintf(bat->log, _("\nBAT analysis: signal has %d frames at %d Hz,"),
			bat->frames, bat->rate);
	fprintf(bat->log, _(" %d channels, %d bytes per sample.\n"),
			bat->channels, bat->sample_size);

	bat->buf = malloc(bat->frames * bat->frame_size);
	if (bat->buf == NULL)
		return -ENOMEM;

	bat->fp = fopen(bat->capture.file, "rb");
	err = -errno;
	if (bat->fp == NULL) {
		fprintf(bat->err, _("Cannot open file: %s %d\n"),
				bat->capture.file, err);
		goto exit1;
	}

	/* Skip header */
	err = read_wav_header(bat, bat->capture.file, bat->fp, true);
	if (err != 0)
		goto exit2;

	items = fread(bat->buf, bat->frame_size, bat->frames, bat->fp);
	if (items != bat->frames) {
		err = -EIO;
		goto exit2;
	}

	err = reorder_data(bat);
	if (err != 0)
		goto exit2;

	for (c = 0; c < bat->channels; c++) {
		fprintf(bat->log, _("\nChannel %i - "), c + 1);
		fprintf(bat->log, _("Checking for target frequency %2.2f Hz\n"),
				bat->target_freq[c]);
		a.buf = bat->buf +
				c * bat->frames * bat->frame_size
				/ bat->channels;
		if (!bat->standalone) {
			err = find_and_check_harmonics(bat, &a, c);
			if (err != 0)
				goto exit2;
		}

		if (snr_is_valid(bat->snr_thd_db)) {
			fprintf(bat->log, _("\nChecking for SNR: "));
			fprintf(bat->log, _("Threshold is %.2f dB (%.2f%%)\n"),
					bat->snr_thd_db, 100.0
					/ powf(10.0, bat->snr_thd_db / 20.0));
			err = find_and_check_noise(bat, a.buf, c);
			if (err != 0)
				goto exit2;
		}
	}

exit2:
	fclose(bat->fp);
exit1:
	free(bat->buf);

	return err;
}