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 <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 multiplication (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 overlap add in the next iteration. */
    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];
    }
}