You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 

501 lines
11 KiB

/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2016
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 3 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.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "sound.h"
#include <math.h>
#include <endian.h>
#include <alsa/asoundlib.h>
#include <samplerate.h>
/* Our device handle */
static snd_pcm_t *pcm_handle_tx = NULL;
static snd_pcm_t *pcm_handle_rx = NULL;
static void (*sound_in_cb)(int16_t *samples_l, int16_t *samples_r, int nr_l, int nr_r);
static int channels_out = 1;
static int channels_in = 1;
struct sound_resample {
SRC_STATE *src;
int rate_in;
int rate_out;
double ratio;
float limit;
};
struct sound_resample *sound_resample_create(int rate_out, int rate_in)
{
int err;
struct sound_resample *sr = calloc(1, sizeof(struct sound_resample));
if (!sr)
goto err_sr;
sr->src = src_new(SRC_LINEAR, 1, &err);
if (!sr->src)
goto err_src;
sr->ratio = (double)rate_out / (double)rate_in;
sr->limit = 1.0;
return sr;
err_src:
free(sr);
err_sr:
return NULL;
}
void sound_resample_destroy(struct sound_resample *sr)
{
if (!sr)
return;
src_delete(sr->src);
free(sr);
}
int sound_resample_perform(struct sound_resample *sr, int16_t *out, int16_t *in, int nr_out, int nr_in)
{
float fl_in[nr_in], fl_out[nr_out];
SRC_DATA data;
data.data_in = fl_in;
data.data_out = fl_out;
data.input_frames = nr_in;
data.output_frames = nr_out;
data.end_of_input = 0;
data.src_ratio = sr->ratio;
src_short_to_float_array(in, fl_in, nr_in);
src_process(sr->src, &data);
src_float_to_short_array(fl_out, out, nr_out);
return 0;
}
#define GAIN_LP_LENGTH 16000
int sound_gain_limit(int16_t *samples, int nr, float gain, float *limit)
{
float limitgain = *limit;
bool newlimit = false;
int i;
float topval = 0.0;
for (i = 0; i < nr; i++) {
topval = fmaxf(topval, fabsf(samples[i]));
}
topval /= 32768;
float max = 1.0 / (gain * limitgain);
if (topval > max) {
limitgain = 1.0 / (topval * gain);
newlimit = true;
}
float realgain = gain * limitgain;
for (i = 0; i < nr; i++) {
float fl_out = samples[i];
fl_out *= realgain;
if (fl_out > 32767)
fl_out = 32767;
if (fl_out < -32768)
fl_out = -32768;
samples[i] = fl_out;
if (!newlimit)
limitgain = ((limitgain * (GAIN_LP_LENGTH - 1)) + 1.0) / GAIN_LP_LENGTH;
}
*limit = limitgain;
return 0;
}
int sound_resample_perform_gain_limit(struct sound_resample *sr, int16_t *out, int16_t *in, int nr_out, int nr_in, float gain)
{
float fl_in[nr_in], fl_out[nr_out];
SRC_DATA data;
data.data_in = fl_in;
data.data_out = fl_out;
data.input_frames = nr_in;
data.output_frames = nr_out;
data.end_of_input = 0;
data.src_ratio = sr->ratio;
bool newlimit = false;
int i;
src_short_to_float_array(in, fl_in, nr_in);
src_process(sr->src, &data);
float limitgain = sr->limit;
float topval = 0.0;
for (i = 0; i < nr_out; i++) {
topval = fmaxf(topval, fabsf(fl_out[i]));
}
float max = 1.0 / (gain * limitgain);
if (topval > max) {
limitgain = 1.0 / (topval * gain);
newlimit = true;
}
float realgain = gain * limitgain;
for (i = 0; i < nr_out; i++) {
fl_out[i] *= realgain;
if (!newlimit)
limitgain = ((limitgain * (GAIN_LP_LENGTH - 1)) + 1.0) / GAIN_LP_LENGTH;
}
src_float_to_short_array(fl_out, out, nr_out);
sr->limit = limitgain;
return 0;
}
int sound_resample_nr_out(struct sound_resample *sr, int nr_in)
{
return nr_in * sr->ratio;
}
int sound_resample_nr_in(struct sound_resample *sr, int nr_out)
{
return nr_out / sr->ratio;
}
int written;
int failed;
static int sound_out_alsa(int16_t *play_samples, int nr)
{
int r;
r = snd_pcm_writei (pcm_handle_tx, play_samples, nr);
// printf("alsa: %d\n", r);
if (r < 0) {
failed++;
printf("recover output %d %d\n", written, failed);
snd_pcm_recover(pcm_handle_tx, r, 1);
snd_pcm_writei (pcm_handle_tx, play_samples, nr);
}
written++;
return 0;
}
int sound_out_lr(int16_t *samples_l, int16_t *samples_r, int nr)
{
int16_t samples[nr * 2];
int i;
for (i = 0; i < nr; i++) {
samples[i * 2 + 0] = samples_l ? samples_l[i] : 0;
samples[i * 2 + 1] = samples_r ? samples_r[i] : 0;
}
return sound_out_alsa(samples, nr);
}
int sound_out(int16_t *samples, int nr, bool left, bool right)
{
int16_t *play_samples;
int16_t samples_2[nr * channels_out];
int i;
if (channels_out == 2) {
/* Output is 2 channels */
for (i = nr; i >= 0; i--) {
if (left)
samples_2[i * 2 + 0] = samples[i];
if (right)
samples_2[i * 2 + 1] = samples[i];
}
play_samples = samples_2;
} else {
play_samples = samples;
}
return sound_out_alsa(play_samples, nr);
}
int16_t *silence = NULL;
int silence_nr;
int sound_silence(void)
{
int r;
r = snd_pcm_writei (pcm_handle_tx, silence, silence_nr);
// printf("alsa: %d\n", r);
if (r < 0) {
printf("recover output\n");
snd_pcm_recover(pcm_handle_tx, r, 1);
snd_pcm_writei (pcm_handle_tx, silence, silence_nr);
}
return 0;
}
int sound_poll_count_tx(void)
{
return snd_pcm_poll_descriptors_count(pcm_handle_tx);
}
int sound_poll_fill_tx(struct pollfd *fds, int count)
{
if (snd_pcm_poll_descriptors(pcm_handle_tx, fds, count) >= 0)
return 0;
return -1;
}
bool sound_poll_out_tx(struct pollfd *fds, int count)
{
unsigned short revents;
snd_pcm_poll_descriptors_revents(pcm_handle_tx, fds, count, &revents);
if (revents & (POLLOUT | POLLERR))
return true;
else
return false;
}
int sound_poll_count_rx(void)
{
return snd_pcm_poll_descriptors_count(pcm_handle_rx);
}
int sound_poll_fill_rx(struct pollfd *fds, int count)
{
if (snd_pcm_poll_descriptors(pcm_handle_rx, fds, count) >= 0)
return 0;
return -1;
}
bool sound_poll_in_rx(struct pollfd *fds, int count)
{
unsigned short revents;
snd_pcm_poll_descriptors_revents(pcm_handle_rx, fds, count, &revents);
if (revents & (POLLIN | POLLERR))
return true;
else
return false;
}
static int nr;
int sound_rx(void)
{
int i;
int r;
int rec_nr = nr;
int16_t rec_samples[rec_nr * channels_in];
int16_t rec_samples_l[rec_nr];
int16_t rec_samples_r[rec_nr];
int16_t *samples_l, *samples_r;
r = snd_pcm_readi(pcm_handle_rx, rec_samples, rec_nr);
if (r <= 0) {
printf("recover input (nr=%d, r=%d)\n", nr, r);
snd_pcm_recover(pcm_handle_rx, r, 0);
snd_pcm_start(pcm_handle_rx);
return -1;
}
if (channels_in == 2) {
for (i = 0; i < r; i++) {
rec_samples_l[i] = rec_samples[i * 2];
rec_samples_r[i] = rec_samples[i * 2 + 1];
}
samples_l = rec_samples_l;
samples_r = rec_samples_r;
} else {
samples_l = rec_samples;
samples_r = rec_samples;
}
sound_in_cb(samples_l, samples_r, r, r);
return 0;
}
int sound_param(snd_pcm_t *pcm_handle, bool is_tx, int hw_rate, int force_channels)
{
int channels = !force_channels ? 1 : force_channels;
snd_pcm_hw_params_t *hw_params;
snd_pcm_hw_params_malloc (&hw_params);
snd_pcm_hw_params_any(pcm_handle, hw_params);
if (snd_pcm_hw_params_set_access (pcm_handle, hw_params, SND_PCM_ACCESS_RW_INTERLEAVED)) {
printf("Interleaved not supported\n");
}
if (htole16(0x1234) == 0x1234)
snd_pcm_hw_params_set_format (pcm_handle, hw_params, SND_PCM_FORMAT_S16_LE);
else
snd_pcm_hw_params_set_format (pcm_handle, hw_params, SND_PCM_FORMAT_S16_BE);
unsigned int rrate = hw_rate;
if (snd_pcm_hw_params_set_rate_near (pcm_handle, hw_params, &rrate, NULL)) {
printf("Could not set rate %d\n", rrate);
}
printf("requested rate: %d got rate: %d\n", hw_rate, rrate);
if (channels == 1 && snd_pcm_hw_params_set_channels (pcm_handle, hw_params, 1)) {
printf("Could not set channels to 1\n");
if (!force_channels)
channels = 2;
else
channels = 0;
}
if (channels == 2 && snd_pcm_hw_params_set_channels (pcm_handle, hw_params, 2)) {
printf("Could not set channels to 2\n");
channels = 0;
}
printf("Channels: %d\n", channels);
if (!channels)
return -1;
if (is_tx) {
channels_out = channels;
} else {
channels_in = channels;
}
snd_pcm_uframes_t buffer_size = rrate / 5;
snd_pcm_uframes_t period_size = rrate / 50;
snd_pcm_hw_params_set_buffer_size_near (pcm_handle, hw_params, &buffer_size);
snd_pcm_hw_params_set_period_size_near (pcm_handle, hw_params, &period_size, NULL);
if (snd_pcm_hw_params(pcm_handle, hw_params)) {
printf("Could not set HW params\n");
return -1;
}
snd_pcm_hw_params_free (hw_params);
return rrate;
}
int sound_buffer(snd_pcm_t *pcm_handle, int buffer_nr, bool is_tx)
{
snd_pcm_uframes_t period_size = buffer_nr * 1;
snd_pcm_sw_params_t *sw_params;
snd_pcm_sw_params_malloc (&sw_params);
snd_pcm_sw_params_current (pcm_handle, sw_params);
snd_pcm_sw_params_set_start_threshold(pcm_handle, sw_params, period_size);
snd_pcm_sw_params_set_avail_min(pcm_handle, sw_params, period_size);
if (snd_pcm_sw_params(pcm_handle, sw_params)) {
printf("Could not set SW params\n");
return -1;
}
snd_pcm_sw_params_free(sw_params);
return 0;
}
int sound_init(char *device,
void (*in_cb)(int16_t *samples_l, int16_t *samples_r, int nr_l, int nr_r),
int hw_rate, int force_channels_in, int force_channels_out)
{
int err;
int rrate_tx, rrate_rx;
/* The device name */
const char *device_name;
if (device)
device_name = device;
else
device_name = "default";
sound_in_cb = in_cb;
/* Open the device */
err = snd_pcm_open (&pcm_handle_tx, device_name, SND_PCM_STREAM_PLAYBACK, 0);
if (err < 0) {
printf("snd_pcm_open() err %d: %s\n", err, snd_strerror(err));
return -1;
}
if ((rrate_tx = sound_param(pcm_handle_tx, true, hw_rate, force_channels_out)) < 0)
return -1;
err = snd_pcm_open (&pcm_handle_rx, device_name, SND_PCM_STREAM_CAPTURE, 0);
if (err < 0) {
printf("snd_pcm_open() err %d: %s\n", err, snd_strerror(err));
return -1;
}
if ((rrate_rx = sound_param(pcm_handle_rx, false, hw_rate, force_channels_in)) < 0)
return -1;
if (rrate_rx != rrate_tx) {
printf("TX and RX sample rate do not match\n");
return -1;
}
return rrate_rx;
}
int sound_set_nr(int nr_set)
{
nr = nr_set;
if (sound_buffer(pcm_handle_tx, nr_set, true)) {
printf("Could not set sound settings for TX\n");
return -1;
}
if (sound_buffer(pcm_handle_rx, nr_set, false)) {
printf("Could not set sound settings for RX\n");
return -1;
}
silence_nr = nr_set;
free(silence);
silence = calloc(silence_nr * 2, sizeof(int16_t));
snd_pcm_start(pcm_handle_rx);
snd_pcm_prepare(pcm_handle_tx);
return 0;
}
int sound_gain(int16_t *samples, int nr, double gain)
{
int i;
for (i = 0; i < nr; i++) {
double sample = samples[i];
sample *= gain;
if (sample > 32767)
sample = 32767;
if (sample < -32768)
sample = -32768;
samples[i] = sample;
}
return 0;
}