bridge_softmix_binaural.c

Go to the documentation of this file.

/*
 * Asterisk -- An open source telephony toolkit.
 *
 * Copyright (C) 2016, Frank Haase, Dennis Guse
 *
 * Frank Haase <[email protected]>
 * Dennis Guse <[email protected]>
 *
 * See http://www.asterisk.org for more information about
 * the Asterisk project. Please do not directly contact
 * any of the maintainers of this project for assistance;
 * the project provides a web site, mailing lists and IRC
 * channels for your use.
 *
 * This program is free software, distributed under the terms of
 * the GNU General Public License Version 2. See the LICENSE file
 * at the top of the source tree.
 */

/*! \file
 *
 * \brief Multi-party software based binaural mixing
 *
 * \author Frank Haase <[email protected]>
 * \author Dennis Guse <[email protected]>
 *
 * \ingroup bridges
 */

#include "include/bridge_softmix_internal.h"

#ifdef BINAURAL_RENDERING
 #include "include/hrirs_configuration.h"
#endif

/*! The number of prealloced channels when a bridge will be created. */
#define CONVOLVE_CHANNEL_PREALLOC 3
/*! Max size of the convolve buffer. */
#define CONVOLVE_MAX_BUFFER 4096
/*! The default sample size in an binaural environment with a two-channel
 * codec at 48kHz is 960 samples.
 */
#define CONVOLUTION_SAMPLE_SIZE 960

#ifdef BINAURAL_RENDERING
 #if SOFTMIX_BINAURAL_SAMPLE_RATE != HRIRS_SAMPLE_RATE
 #error HRIRs are required to be SOFTMIX_BINAURAL_SAMPLE_RATE Hz. Please adjust hrirs.h accordingly.
 #endif
 #if CONVOLUTION_SAMPLE_SIZE < HRIRS_IMPULSE_LEN
 #error HRIRS_IMPULSE_LEN cannot be longer than CONVOLUTION_SAMPLE_SIZE. Please adjust hrirs.h accordingly.
 #endif
#endif

void reset_channel_pair(struct convolve_channel_pair *channel_pair,
 unsigned int default_sample_size)
{
 memset(channel_pair->chan_left.overlap_add, 0, sizeof(float) * default_sample_size);
 memset(channel_pair->chan_right.overlap_add, 0, sizeof(float) * default_sample_size);
}

void random_binaural_pos_change(struct softmix_bridge_data *softmix_data)
{
 /*
 * We perform a shuffle of all channels, even the ones that aren't used at the
 * moment of shuffling now. This has the efect that new members will be placed
 * randomly too.
 */
 unsigned int i;
 unsigned int j;
 struct convolve_channel_pair *tmp;

 if (softmix_data->convolve.chan_size < 2) {
 return;
 }

 srand(time(NULL));
 for (i = softmix_data->convolve.chan_size - 1; i > 0; i--) {
 j = rand() % (i + 1);
 tmp = softmix_data->convolve.cchan_pair[i];
 reset_channel_pair(tmp, softmix_data->default_sample_size);
 softmix_data->convolve.cchan_pair[i] = softmix_data->convolve.cchan_pair[j];
 softmix_data->convolve.cchan_pair[j] = tmp;
 }
}

int do_convolve(struct convolve_channel *chan, int16_t *in_samples,
 unsigned int in_sample_size, unsigned int hrtf_length)
{
#ifdef BINAURAL_RENDERING
 unsigned int i;

 if (in_sample_size != CONVOLUTION_SAMPLE_SIZE) {
 return -1;
 }

 /* FFT setting real part */
 for (i = 0; i < CONVOLUTION_SAMPLE_SIZE; i++) {
 chan->fftw_in[i] = in_samples[i] * (FLT_MAX / SHRT_MAX);
 }

 for (i = CONVOLUTION_SAMPLE_SIZE; i < hrtf_length; i++) {
 chan->fftw_in[i] = 0;
 }
 fftw_execute(chan->fftw_plan);

 /* Imaginary mulitplication (frequency space). */
 /* First FFTW result has never an imaginary part. */
 chan->fftw_in[0] = chan->fftw_out[0] * chan->hrtf[0];
 for (i = 1; i < (hrtf_length / 2); i++) {
 /* Real part */
 chan->fftw_in[i] = (chan->fftw_out[i] * chan->hrtf[i]) -
 (chan->fftw_out[hrtf_length - i] * chan->hrtf[hrtf_length - i]);
 /* Imaginary part */
 chan->fftw_in[hrtf_length - i] = (chan->fftw_out[i] * chan->hrtf[hrtf_length - i]) +
 (chan->fftw_out[hrtf_length - i] * chan->hrtf[i]);
 }

 /* The last (if even) FFTW result has never an imaginary part. */
 if (hrtf_length % 2 == 0) {
 chan->fftw_in[hrtf_length / 2] = chan->fftw_out[hrtf_length / 2] *
 chan->hrtf[hrtf_length / 2];
 }

 /* iFFT */
 fftw_execute(chan->fftw_plan_inverse);
 /* Remove signal increase due to iFFT. */
 for (i = 0; i < hrtf_length; i++) {
 chan->fftw_out[i] = chan->fftw_out[i] / (hrtf_length / 2);
 }

 /* Save the block for overlapp add in the next itteration. */
 for (i = 0; i < in_sample_size; i++) {
 chan->overlap_add[i] += chan->fftw_out[i];
 }

 /* Copy real part to the output, ignore the complex part. */
 for (i = 0; i < in_sample_size; i++) {
 chan->out_data[i] = chan->overlap_add[i] * (SHRT_MAX / FLT_MAX);
 chan->overlap_add[i] = chan->fftw_out[i + in_sample_size];
 }
#endif
 return 0;
}

struct convolve_channel_pair *do_convolve_pair(struct convolve_data *data,
 unsigned int pos_id, int16_t *in_samples, unsigned int in_sample_size,
 const char *channel_name)
{
 struct convolve_channel_pair *chan_pair;

 /* If a position has no active member we will not convolve. */
 if (data->pos_ids[pos_id] != 1) {
 ast_log(LOG_ERROR, "Channel %s: Channel pair has no active member! (pos id = %d)\n",
 channel_name, pos_id);
 return NULL;
 }

 chan_pair = data->cchan_pair[pos_id];
 if (do_convolve(&chan_pair->chan_left, in_samples, in_sample_size, data->hrtf_length)) {
 ast_log(LOG_ERROR, "Channel %s: Binaural processing failed.", channel_name);
 return NULL;
 }

 if (do_convolve(&chan_pair->chan_right, in_samples, in_sample_size, data->hrtf_length)) {
 ast_log(LOG_ERROR, "Channel %s: Binaural processing failed.", channel_name);
 return NULL;
 }

 return chan_pair;
}

float *get_hrir(unsigned int chan_pos, unsigned int chan_side)
{
#ifdef BINAURAL_RENDERING
 if (chan_side == HRIRS_CHANNEL_LEFT) {
 return hrirs_left[ast_binaural_positions[chan_pos]];
 } else if (chan_side == HRIRS_CHANNEL_RIGHT) {
 return hrirs_right[ast_binaural_positions[chan_pos]];
 }
#else
 ast_log(LOG_ERROR, "Requesting data for the binaural conference feature without "
 "it beeing active.\n");
#endif

 return NULL;
}

int init_convolve_channel(struct convolve_channel *channel, unsigned int hrtf_len,
 unsigned int chan_pos, unsigned int chan_side, unsigned int default_sample_size)
{
#ifdef BINAURAL_RENDERING
 unsigned int j;
 float *hrir;

 /* Prepare FFTW. */
 channel->fftw_in = (double *) fftw_malloc(sizeof(double) * (hrtf_len + 1));
 if (channel->fftw_in == NULL) {
 return -1;
 }

 channel->fftw_out = (double *) fftw_malloc(sizeof(double) * (hrtf_len + 1));
 if (channel->fftw_out == NULL) {
 fftw_free(channel->fftw_in);
 return -1;
 }

 memset(channel->fftw_in, 0, sizeof(double) * (hrtf_len + 1));
 memset(channel->fftw_out, 0, sizeof(double) * (hrtf_len + 1));

 channel->fftw_plan = fftw_plan_r2r_1d(hrtf_len, channel->fftw_in, channel->fftw_out,
 FFTW_R2HC, FFTW_PATIENT);
 channel->fftw_plan_inverse = fftw_plan_r2r_1d(hrtf_len, channel->fftw_in, channel->fftw_out,
 FFTW_HC2R, FFTW_PATIENT);
 channel->out_data = ast_calloc(CONVOLVE_MAX_BUFFER, sizeof(int16_t));
 if (channel->out_data == NULL) {
 fftw_free(channel->fftw_in);
 fftw_free(channel->fftw_out);
 return -1;
 }

 /* Reuse positions if all positions are already used. */
 chan_pos = chan_pos % HRIRS_IMPULSE_SIZE;

 /* Get HRTF for the channels spatial position. */
 hrir = get_hrir(chan_pos, chan_side);
 if (hrir == NULL) {
 fftw_free(channel->fftw_in);
 fftw_free(channel->fftw_out);
 ast_free(channel->out_data);
 return -1;
 }

 for (j = 0; j < HRIRS_IMPULSE_LEN; j++) {
 channel->fftw_in[j] = hrir[j];
 }

 for (j = HRIRS_IMPULSE_LEN; j < hrtf_len; j++) {
 channel->fftw_in[j] = 0;
 }

 fftw_execute(channel->fftw_plan);
 channel->hrtf = (double *) fftw_malloc(sizeof(double) * hrtf_len);
 if (channel->hrtf == NULL) {
 fftw_free(channel->fftw_in);
 fftw_free(channel->fftw_out);
 ast_free(channel->out_data);
 return -1;
 }

 for (j = 0; j < hrtf_len; j++) {
 channel->hrtf[j] = channel->fftw_out[j];
 }
 channel->overlap_add = ast_calloc(default_sample_size, sizeof(float));

 return 0;
#endif
 return -1;
}

int init_convolve_channel_pair(struct convolve_channel_pair *cchan_pair,
 unsigned int hrtf_len, unsigned int chan_pos, unsigned int default_sample_size)
{
#ifdef BINAURAL_RENDERING
 unsigned int hrirs_pos = chan_pos * 2;
 int success = 0;

 ast_debug(3, "Binaural pos for the new channel pair will be L: %d R: %d (pos id = %d)\n",
 hrirs_pos, hrirs_pos + 1, chan_pos);
 success = init_convolve_channel(&cchan_pair->chan_left, hrtf_len, chan_pos, HRIRS_CHANNEL_LEFT,
 default_sample_size);
 if (success == -1) {
 return success;
 }

 success = init_convolve_channel(&cchan_pair->chan_right, hrtf_len, chan_pos,
 HRIRS_CHANNEL_RIGHT, default_sample_size);
 if (success == -1) {
 free_convolve_channel(&cchan_pair->chan_left);
 }

 return success;
#else
 ast_log(LOG_ERROR, "Requesting data for the binaural conference feature "
 "without it beeing active.\n");

 return -1;
#endif
}

int init_convolve_data(struct convolve_data *data, unsigned int default_sample_size)
{
 unsigned int i;
 unsigned int j;
 int success;
 success = 0;

 data->pos_ids = ast_calloc(sizeof(int), sizeof(int) * CONVOLVE_CHANNEL_PREALLOC);
 if (data->pos_ids == NULL) {
 return -1;
 }
 data->chan_size = CONVOLVE_CHANNEL_PREALLOC;
 data->number_channels = 0;
 data->cchan_pair = ast_malloc(sizeof(struct convolve_channel_pair *) *
 CONVOLVE_CHANNEL_PREALLOC);
 if (data->cchan_pair == NULL) {
 ast_free(data->pos_ids);
 return -1;
 }

 for (i = 0; i < CONVOLVE_CHANNEL_PREALLOC; i++) {
 data->cchan_pair[i] = ast_malloc(sizeof(struct convolve_channel_pair));
 if (data->cchan_pair[i] == NULL) {
 ast_free(data->pos_ids);
 for (j = 0; j < i; j++) {
 ast_free(data->cchan_pair[j]);
 }
 ast_free(data->cchan_pair);
 return -1;
 }
 }

 data->hrtf_length = (default_sample_size * 2) - 1;
 for (i = 0; i < CONVOLVE_CHANNEL_PREALLOC; i++) {
 success = init_convolve_channel_pair(data->cchan_pair[i], data->hrtf_length, i,
 default_sample_size);
 if (success == -1) {
 ast_free(data->pos_ids);
 for (j = 0; j < i; j++) {
 free_convolve_channel_pair(data->cchan_pair[j]);
 }
 for (j = 0; j < CONVOLVE_CHANNEL_PREALLOC; j++) {
 ast_free(data->cchan_pair[j]);
 }
 return -1;
 }
 }

 return success;
}

void free_convolve_channel(struct convolve_channel *cchan)
{
#ifdef BINAURAL_RENDERING
 fftw_free(cchan->fftw_out);
 fftw_free(cchan->fftw_in);
 fftw_free(cchan->hrtf);
 ast_free(cchan->overlap_add);
 ast_free(cchan->out_data);
 fftw_destroy_plan(cchan->fftw_plan);
 fftw_destroy_plan(cchan->fftw_plan_inverse);
#endif
}

void free_convolve_channel_pair(struct convolve_channel_pair *cchan_pair)
{
 free_convolve_channel(&cchan_pair->chan_left);
 free_convolve_channel(&cchan_pair->chan_right);
}

void free_convolve_data(struct convolve_data *data)
{
 unsigned int i;
 ast_free(data->pos_ids);
 for (i = 0; i < data->chan_size; i++) {
 free_convolve_channel_pair(data->cchan_pair[i]);
 ast_free(data->cchan_pair[i]);
 }
 ast_free(data->cchan_pair);
}

int set_binaural_data_join(struct convolve_data *data, unsigned int default_sample_size)
{
 struct convolve_channel_pair **cchan_pair_tmp;
 unsigned int i;
 int *pos_ids_tmp;

 /* Raise the number of input channels. */
 data->number_channels++;
 /* We realloc another channel pair if we are out of prealloced ones. */
 /* We have prealloced one at the beginning of a conference and if a member leaves. */
 if (data->chan_size < data->number_channels) {
 data->chan_size += 1;

 pos_ids_tmp = ast_realloc(data->pos_ids, data->chan_size * sizeof(int));
 if (pos_ids_tmp) {
 data->pos_ids = pos_ids_tmp;
 } else {
 goto binaural_join_fails;
 }

 data->pos_ids[data->chan_size - 1] = 0;
 cchan_pair_tmp = ast_realloc(data->cchan_pair,
 data->chan_size * sizeof(struct convolve_channel_pair *));
 if (cchan_pair_tmp) {
 data->cchan_pair = cchan_pair_tmp;
 } else {
 goto binaural_join_fails;
 }

 data->cchan_pair[data->chan_size - 1] = ast_malloc(sizeof(struct convolve_channel_pair));
 if (data->cchan_pair[data->chan_size - 1] == NULL) {
 goto binaural_join_fails;
 }

 i = init_convolve_channel_pair(data->cchan_pair[data->chan_size - 1], data->hrtf_length,
 data->chan_size - 1, default_sample_size);
 if (i == -1) {
 goto binaural_join_fails;
 }
 }

 for (i = 0; i < data->chan_size; i++) {
 if (data->pos_ids[i] == 0) {
 data->pos_ids[i] = 1;
 break;
 }
 }

 return i;

binaural_join_fails:
 data->number_channels--;
 data->chan_size -= 1;

 return -1;
}

void set_binaural_data_leave(struct convolve_data *data, unsigned int pos,
 unsigned int default_sample_size)
{
 if (pos >= data->chan_size || data->pos_ids[pos] == 0) {
 return;
 }

 reset_channel_pair(data->cchan_pair[pos], default_sample_size);
 data->number_channels--;
 data->pos_ids[pos] = 0;
}

void softmix_process_write_binaural_audio(struct softmix_channel *sc,
 unsigned int default_sample_size)
{
 unsigned int i;

 if (sc->write_frame.samples % default_sample_size != 0) {
 return;
 }

 /* If binaural is suspended, the source audio (mono) will be removed. */
 if (sc->binaural_suspended) {
 for (i = 0; i < default_sample_size; i++) {
 ast_slinear_saturated_subtract(&sc->final_buf[i * 2], &sc->our_buf[i]);
 ast_slinear_saturated_subtract(&sc->final_buf[(i * 2) + 1], &sc->our_buf[i]);
 }
 return;
 }

 /* If binaural is NOT suspended, the source audio (binaural) will be removed. */
 for (i = 0; i < default_sample_size; i++) {
 ast_slinear_saturated_subtract(&sc->final_buf[i * 2],
 &sc->our_chan_pair->chan_left.out_data[i]);
 ast_slinear_saturated_subtract(&sc->final_buf[(i * 2) + 1],
 &sc->our_chan_pair->chan_right.out_data[i]);
 }
}

void check_binaural_position_change(struct ast_bridge *bridge,
 struct softmix_bridge_data *softmix_data)
{
 unsigned int pos_change;
 struct ast_bridge_channel *bridge_channel;

 /*
 * We only check binaural things if binaural is activated by the config
 * and at least one binaural channel joined.
 */
 if (!(bridge->softmix.binaural_active && softmix_data->convolve.binaural_active)) {
 return;
 }
 /*
 * Before we pull any audio, we must check if any channel requests a
 * change of binaural positions.
 */
 pos_change = 0;
 AST_LIST_TRAVERSE(&bridge->channels, bridge_channel, entry) {
 if (!bridge_channel->binaural_pos_change) {
 continue;
 }
 ast_bridge_channel_lock_bridge(bridge_channel);
 bridge_channel->binaural_pos_change = 0;
 ast_bridge_unlock(bridge_channel->bridge);
 pos_change = 1;
 }

 if (pos_change) {
 random_binaural_pos_change(softmix_data);
 }
}

void add_binaural_mixing(struct ast_bridge *bridge, struct softmix_bridge_data *softmix_data,
 unsigned int softmix_samples, struct softmix_mixing_array *mixing_array,
 struct softmix_channel *sc, const char *channel_name)
{
 struct convolve_channel_pair *pair;

 pair = NULL;
 /* We only check binaural things if at least one binaural channel joined. */
 if (!(bridge->softmix.binaural_active && softmix_data->convolve.binaural_active
 && (softmix_samples % CONVOLUTION_SAMPLE_SIZE) == 0)) {
 return;
 }

 if (!sc->is_announcement) {
 pair = do_convolve_pair(&softmix_data->convolve, sc->binaural_pos,
 mixing_array->buffers[mixing_array->used_entries], softmix_samples, channel_name);
 }
 sc->our_chan_pair = pair;
 mixing_array->chan_pairs[mixing_array->used_entries] = pair;
}

void binaural_mixing(struct ast_bridge *bridge, struct softmix_bridge_data *softmix_data,
 struct softmix_mixing_array *mixing_array, int16_t *bin_buf, int16_t *ann_buf)
{
 unsigned int idx;
 unsigned int x;

 if (!(bridge->softmix.binaural_active && softmix_data->convolve.binaural_active)) {
 return;
 }
 /* mix it like crazy (binaural channels) */
 memset(bin_buf, 0, MAX_DATALEN);
 memset(ann_buf, 0, MAX_DATALEN);

 for (idx = 0; idx < mixing_array->used_entries; idx++) {
 if (mixing_array->chan_pairs[idx] == NULL) {
 for (x = 0; x < softmix_data->default_sample_size; x++) {
 ast_slinear_saturated_add(bin_buf + (x * 2), mixing_array->buffers[idx] + x);
 ast_slinear_saturated_add(bin_buf + (x * 2) + 1, mixing_array->buffers[idx] + x);
 ann_buf[x * 2] = mixing_array->buffers[idx][x];
 ann_buf[(x * 2) + 1] = mixing_array->buffers[idx][x];
 }
 } else {
 for (x = 0; x < softmix_data->default_sample_size; x++) {
 ast_slinear_saturated_add(bin_buf + (x * 2),
 mixing_array->chan_pairs[idx]->chan_left.out_data + x);
 ast_slinear_saturated_add(bin_buf + (x * 2) + 1,
 mixing_array->chan_pairs[idx]->chan_right.out_data + x);
 }
 }
 }
}

void create_binaural_frame(struct ast_bridge_channel *bridge_channel,
 struct softmix_channel *sc, int16_t *bin_buf, int16_t *ann_buf,
 unsigned int softmix_datalen, unsigned int softmix_samples, int16_t *buf)
{
 unsigned int i;

 sc->write_frame.datalen = softmix_datalen * 2;
 sc->write_frame.samples = softmix_samples * 2;
 if (!bridge_channel->binaural_suspended) {
 sc->binaural_suspended = 0;
 if (sc->is_announcement) {
 memcpy(sc->final_buf, ann_buf, softmix_datalen * 2);
 } else {
 memcpy(sc->final_buf, bin_buf, softmix_datalen * 2);
 }
 return;
 }

 /*
 * Mark that binaural output is suspended, since we use two channel audio
 * we copy the same signals into both channels.
 */
 sc->binaural_suspended = 1;
 for (i = 0; i < softmix_samples; i++) {
 sc->final_buf[i * 2] = buf[i];
 sc->final_buf[(i * 2) + 1] = buf[i];
 }
}