diff options
Diffstat (limited to 'bridges/bridge_softmix/bridge_softmix_binaural.c')
| -rw-r--r-- | bridges/bridge_softmix/bridge_softmix_binaural.c | 579 |
1 files changed, 579 insertions, 0 deletions
diff --git a/bridges/bridge_softmix/bridge_softmix_binaural.c b/bridges/bridge_softmix/bridge_softmix_binaural.c new file mode 100644 index 000000000..128e4640e --- /dev/null +++ b/bridges/bridge_softmix/bridge_softmix_binaural.c @@ -0,0 +1,579 @@ +/* + * Asterisk -- An open source telephony toolkit. + * + * Copyright (C) 2016, Frank Haase, Dennis Guse + * + * Frank Haase <fra.haase@gmail.com> + * Dennis Guse <dennis.guse@alumni.tu-berlin.de> + * + * 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 <fra.haase@googlemail.com> + * \author Dennis Guse <dennis.guse@alumni.tu-berlin.de> + * + * \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 = fftw_alloc_real(hrtf_len + 1); + if (channel->fftw_in == NULL) { + return -1; + } + + channel->fftw_out = fftw_alloc_real(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 = fftw_alloc_real(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, struct ast_bridge_channel *bridge_channel) +{ + unsigned int pos_change; + + /* + * 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]; + } +} |