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

/*
 * Copyright (C) 2015 Caleb Crome
 * 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.
 *
 */

/*
 * This is a general purpose sine wave generator that will stay stable
 * for a long time, and with a little renormalization, could stay stay
 * stable indefinitely
 */

#include "aconfig.h"

#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <stdint.h>
#include <stdbool.h>
#include <errno.h>

#include "gettext.h"
#include "common.h"
#include "signal.h"

/*
 * Initialize the sine wave generator.
 * sin_generator:  gets initialized by this call.
 * frequency:      the frequency for the sine wave.  must be < 0.5*sample_rate
 * sample_rate:    the sample rate...
 * returns 0 on success, -1 on error.
 */
int sin_generator_init(struct sin_generator *sg, float magnitude,
		float frequency, float sample_rate)
{
	/* angular frequency:  cycles/sec / (samp/sec) * rad/cycle = rad/samp */
	float w = frequency / sample_rate * 2 * M_PI;
	if (frequency >= sample_rate / 2)
		return -1;
	sg->phasor_real = cos(w);
	sg->phasor_imag = sin(w);
	sg->magnitude   = magnitude;
	sg->state_real  = 0.0;
	sg->state_imag  = magnitude;
	sg->frequency = frequency;
	sg->sample_rate = sample_rate;
	return 0;
}

/*
 * Generates the next sample in the sine wave.
 * should be much faster than calling a sin function
 * if it's inlined and optimized.
 *
 * returns the next value.  no possibility of error.
 */
float sin_generator_next_sample(struct sin_generator *sg)
{
	/* get shorthand to pointers */
	const double pr = sg->phasor_real;
	const double pi = sg->phasor_imag;
	const double sr = sg->state_real;
	const double si = sg->state_imag;
	/* step the phasor -- complex multiply */
	sg->state_real = sr * pr - si * pi;
	sg->state_imag = sr * pi + pr * si;
	/* return the input value so sine wave starts at exactly 0.0 */
	return (float)sr;
}

/* fills a vector with a sine wave */
void sin_generator_vfill(struct sin_generator *sg, float *buf, int n)
{
	int i;
	for (i = 0; i < n; i++)
		*buf++ = sin_generator_next_sample(sg);
}

static int reorder(struct bat *bat, float *val, int frames)
{
	float *new_buf = NULL;
	int i, c, bytes;

	bytes = frames * bat->channels * sizeof(float);

	new_buf = (float *) malloc(bytes);
	if (new_buf == NULL) {
		fprintf(bat->err, _("Not enough memory.\n"));
		return -ENOMEM;
	}

	memcpy(new_buf, val, bytes);
	for (i = 0; i < frames; i++)
		for (c = 0; c < bat->channels; c++)
			val[i * bat->channels + c] =
				new_buf[c * frames + i];
	free(new_buf);

	return 0;
}

static int adjust_waveform(struct bat *bat, float *val, int frames,
		int channels)
{
	int i, nsamples, max;
	float factor, offset = 0.0;

	switch (bat->format) {
	case BAT_PCM_FORMAT_U8:
		max = INT8_MAX;
		offset = max;	/* shift for unsigned format */
		break;
	case BAT_PCM_FORMAT_S16_LE:
		max  = INT16_MAX;
		break;
	case BAT_PCM_FORMAT_S24_3LE:
		max = (1 << 23) - 1;
		break;
	case BAT_PCM_FORMAT_S32_LE:
		max = INT32_MAX;
		break;
	default:
		fprintf(bat->err, _("Invalid PCM format: %d\n"), bat->format);
		return -EINVAL;
	}

	factor = max * RANGE_FACTOR;
	nsamples = channels * frames;

	for (i = 0; i < nsamples; i++)
		val[i] = val[i] * factor + offset;

	return 0;
}

int generate_sine_wave(struct bat *bat, int frames, void *buf)
{
	int err = 0;
	int c, nsamples;
	float *sinus_f = NULL;
	static struct sin_generator sg[MAX_CHANNELS];

	nsamples = bat->channels * frames;
	sinus_f = (float *) malloc(nsamples * sizeof(float));
	if (sinus_f == NULL) {
		fprintf(bat->err, _("Not enough memory.\n"));
		return -ENOMEM;
	}

	for (c = 0; c < bat->channels; c++) {
		/* initialize static struct at the first time */
		if (sg[c].frequency != bat->target_freq[c])
			sin_generator_init(&sg[c], 1.0, bat->target_freq[c],
					bat->rate);
		/* fill buffer for each channel */
		sin_generator_vfill(&sg[c], sinus_f + c * frames, frames);
	}

	/* reorder samples to interleaved mode */
	err = reorder(bat, sinus_f, frames);
	if (err != 0)
		goto exit;

	/* adjust amplitude and offset of waveform */
	err = adjust_waveform(bat, sinus_f, frames, bat->channels);
	if (err != 0)
		goto exit;

	bat->convert_float_to_sample(sinus_f, buf, frames, bat->channels);

exit:
	free(sinus_f);

	return err;
}

/* generate single channel sine waveform without sample conversion */
int generate_sine_wave_raw_mono(struct bat *bat, float *buf,
		float freq, int nsamples)
{
	int err = 0;
	struct sin_generator sg;

	err = sin_generator_init(&sg, 1.0, freq, bat->rate);
	if (err < 0)
		return err;
	sin_generator_vfill(&sg, buf, nsamples);

	/* adjust amplitude and offset of waveform */
	err = adjust_waveform(bat, buf, nsamples, 1);

	return err;
}