Binaural synthesis (confbridge): Adds binaural synthesis to bridge_softmix.
[asterisk/asterisk.git] / bridges / bridge_softmix / bridge_softmix_binaural.c
1 /*
2  * Asterisk -- An open source telephony toolkit.
3  *
4  * Copyright (C) 2016, Frank Haase, Dennis Guse
5  *
6  * Frank Haase <fra.haase@gmail.com>
7  * Dennis Guse <dennis.guse@alumni.tu-berlin.de>
8  *
9  * See http://www.asterisk.org for more information about
10  * the Asterisk project. Please do not directly contact
11  * any of the maintainers of this project for assistance;
12  * the project provides a web site, mailing lists and IRC
13  * channels for your use.
14  *
15  * This program is free software, distributed under the terms of
16  * the GNU General Public License Version 2. See the LICENSE file
17  * at the top of the source tree.
18  */
19
20 /*! \file
21  *
22  * \brief Multi-party software based binaural mixing
23  *
24  * \author Frank Haase <fra.haase@googlemail.com>
25  * \author Dennis Guse <dennis.guse@alumni.tu-berlin.de>
26  *
27  * \ingroup bridges
28  */
29
30 #include "include/bridge_softmix_internal.h"
31
32 #ifdef BINAURAL_RENDERING
33   #include "include/hrirs_configuration.h"
34 #endif
35
36 /*! The number of prealloced channels when a bridge will be created. */
37 #define CONVOLVE_CHANNEL_PREALLOC 3
38 /*! Max size of the convolve buffer. */
39 #define CONVOLVE_MAX_BUFFER 4096
40 /*! The default sample size in an binaural environment with a two-channel
41  * codec at 48kHz is 960 samples.
42  */
43 #define CONVOLUTION_SAMPLE_SIZE 960
44
45 #ifdef BINAURAL_RENDERING
46   #if SOFTMIX_BINAURAL_SAMPLE_RATE != HRIRS_SAMPLE_RATE
47           #error HRIRs are required to be SOFTMIX_BINAURAL_SAMPLE_RATE Hz. Please adjust hrirs.h accordingly.
48         #endif
49   #if CONVOLUTION_SAMPLE_SIZE < HRIRS_IMPULSE_LEN
50           #error HRIRS_IMPULSE_LEN cannot be longer than CONVOLUTION_SAMPLE_SIZE. Please adjust hrirs.h accordingly.
51         #endif
52 #endif
53
54 void reset_channel_pair(struct convolve_channel_pair *channel_pair,
55                 unsigned int default_sample_size)
56 {
57         memset(channel_pair->chan_left.overlap_add, 0, sizeof(float) * default_sample_size);
58         memset(channel_pair->chan_right.overlap_add, 0, sizeof(float) * default_sample_size);
59 }
60
61 void random_binaural_pos_change(struct softmix_bridge_data *softmix_data)
62 {
63         /*
64          * We perform a shuffle of all channels, even the ones that aren't used at the
65          * moment of shuffling now. This has the efect that new members will be placed
66          * randomly too.
67          */
68         unsigned int i;
69         unsigned int j;
70         struct convolve_channel_pair *tmp;
71
72         if (softmix_data->convolve.chan_size < 2) {
73                 return;
74         }
75
76         srand(time(NULL));
77         for (i = softmix_data->convolve.chan_size - 1; i > 0; i--) {
78                 j = rand() % (i + 1);
79                 tmp = softmix_data->convolve.cchan_pair[i];
80                 reset_channel_pair(tmp, softmix_data->default_sample_size);
81                 softmix_data->convolve.cchan_pair[i] = softmix_data->convolve.cchan_pair[j];
82                 softmix_data->convolve.cchan_pair[j] = tmp;
83         }
84 }
85
86 int do_convolve(struct convolve_channel *chan, int16_t *in_samples,
87                 unsigned int in_sample_size, unsigned int hrtf_length)
88 {
89 #ifdef BINAURAL_RENDERING
90         unsigned int i;
91
92         if (in_sample_size != CONVOLUTION_SAMPLE_SIZE) {
93                 return -1;
94         }
95
96         /* FFT setting real part */
97         for (i = 0; i < CONVOLUTION_SAMPLE_SIZE; i++) {
98                 chan->fftw_in[i] = in_samples[i] * (FLT_MAX / SHRT_MAX);
99         }
100
101         for (i = CONVOLUTION_SAMPLE_SIZE; i < hrtf_length; i++) {
102                 chan->fftw_in[i] = 0;
103         }
104         fftw_execute(chan->fftw_plan);
105
106         /* Imaginary mulitplication (frequency space). */
107         /* First FFTW result has never an imaginary part. */
108         chan->fftw_in[0] = chan->fftw_out[0] * chan->hrtf[0];
109         for (i = 1; i < (hrtf_length / 2); i++) {
110                 /* Real part */
111                 chan->fftw_in[i] = (chan->fftw_out[i] * chan->hrtf[i]) -
112                                 (chan->fftw_out[hrtf_length - i] * chan->hrtf[hrtf_length - i]);
113                 /* Imaginary part */
114                 chan->fftw_in[hrtf_length - i] = (chan->fftw_out[i] * chan->hrtf[hrtf_length - i]) +
115                                 (chan->fftw_out[hrtf_length - i] * chan->hrtf[i]);
116         }
117
118         /* The last (if even) FFTW result has never an imaginary part. */
119         if (hrtf_length % 2 == 0) {
120                 chan->fftw_in[hrtf_length / 2] = chan->fftw_out[hrtf_length / 2] *
121                                 chan->hrtf[hrtf_length / 2];
122         }
123
124         /* iFFT */
125         fftw_execute(chan->fftw_plan_inverse);
126         /* Remove signal increase due to iFFT. */
127         for (i = 0; i < hrtf_length; i++) {
128                 chan->fftw_out[i] = chan->fftw_out[i] / (hrtf_length / 2);
129         }
130
131         /* Save the block for overlapp add in the next itteration. */
132         for (i = 0; i < in_sample_size; i++) {
133                 chan->overlap_add[i] += chan->fftw_out[i];
134         }
135
136         /* Copy real part to the output, ignore the complex part. */
137         for (i = 0; i < in_sample_size; i++) {
138                 chan->out_data[i] = chan->overlap_add[i] * (SHRT_MAX / FLT_MAX);
139                 chan->overlap_add[i] = chan->fftw_out[i + in_sample_size];
140         }
141 #endif
142         return 0;
143 }
144
145 struct convolve_channel_pair *do_convolve_pair(struct convolve_data *data,
146                 unsigned int pos_id, int16_t *in_samples, unsigned int in_sample_size,
147                 const char *channel_name)
148 {
149         struct convolve_channel_pair *chan_pair;
150
151         /* If a position has no active member we will not convolve. */
152         if (data->pos_ids[pos_id] != 1) {
153                 ast_log(LOG_ERROR, "Channel %s: Channel pair has no active member! (pos id = %d)\n",
154                                 channel_name, pos_id);
155                 return NULL;
156         }
157
158         chan_pair = data->cchan_pair[pos_id];
159         if (do_convolve(&chan_pair->chan_left, in_samples, in_sample_size, data->hrtf_length)) {
160                 ast_log(LOG_ERROR, "Channel %s: Binaural processing failed.", channel_name);
161                 return NULL;
162         }
163
164         if (do_convolve(&chan_pair->chan_right, in_samples, in_sample_size, data->hrtf_length)) {
165                 ast_log(LOG_ERROR, "Channel %s: Binaural processing failed.", channel_name);
166                 return NULL;
167         }
168
169         return chan_pair;
170 }
171
172 float *get_hrir(unsigned int chan_pos, unsigned int chan_side)
173 {
174 #ifdef BINAURAL_RENDERING
175         if (chan_side == HRIRS_CHANNEL_LEFT) {
176                 return hrirs_left[ast_binaural_positions[chan_pos]];
177         } else if (chan_side == HRIRS_CHANNEL_RIGHT) {
178                 return hrirs_right[ast_binaural_positions[chan_pos]];
179         }
180 #else
181         ast_log(LOG_ERROR, "Requesting data for the binaural conference feature without "
182                         "it beeing active.\n");
183 #endif
184
185         return NULL;
186 }
187
188 int init_convolve_channel(struct convolve_channel *channel, unsigned int hrtf_len,
189                 unsigned int chan_pos, unsigned int chan_side, unsigned int default_sample_size)
190 {
191 #ifdef BINAURAL_RENDERING
192         unsigned int j;
193         float *hrir;
194
195         /* Prepare FFTW. */
196         channel->fftw_in = fftw_alloc_real(hrtf_len + 1);
197         if (channel->fftw_in == NULL) {
198                 return -1;
199         }
200
201         channel->fftw_out = fftw_alloc_real(hrtf_len + 1);
202         if (channel->fftw_out == NULL) {
203                 fftw_free(channel->fftw_in);
204                 return -1;
205         }
206
207         memset(channel->fftw_in, 0, sizeof(double) * (hrtf_len + 1));
208         memset(channel->fftw_out, 0, sizeof(double) * (hrtf_len + 1));
209
210         channel->fftw_plan = fftw_plan_r2r_1d(hrtf_len, channel->fftw_in, channel->fftw_out,
211                         FFTW_R2HC, FFTW_PATIENT);
212         channel->fftw_plan_inverse = fftw_plan_r2r_1d(hrtf_len, channel->fftw_in, channel->fftw_out,
213                         FFTW_HC2R, FFTW_PATIENT);
214         channel->out_data = ast_calloc(CONVOLVE_MAX_BUFFER, sizeof(int16_t));
215         if (channel->out_data == NULL) {
216                 fftw_free(channel->fftw_in);
217                 fftw_free(channel->fftw_out);
218                 return -1;
219         }
220
221         /* Reuse positions if all positions are already used. */
222         chan_pos = chan_pos % HRIRS_IMPULSE_SIZE;
223
224         /* Get HRTF for the channels spatial position. */
225         hrir = get_hrir(chan_pos, chan_side);
226         if (hrir == NULL) {
227                 fftw_free(channel->fftw_in);
228                 fftw_free(channel->fftw_out);
229                 ast_free(channel->out_data);
230                 return -1;
231         }
232
233         for (j = 0; j < HRIRS_IMPULSE_LEN; j++) {
234                 channel->fftw_in[j] = hrir[j];
235         }
236
237         for (j = HRIRS_IMPULSE_LEN; j < hrtf_len; j++) {
238                 channel->fftw_in[j] = 0;
239         }
240
241         fftw_execute(channel->fftw_plan);
242         channel->hrtf = fftw_alloc_real(hrtf_len);
243         if (channel->hrtf == NULL) {
244                 fftw_free(channel->fftw_in);
245                 fftw_free(channel->fftw_out);
246                 ast_free(channel->out_data);
247                 return -1;
248         }
249
250         for (j = 0; j < hrtf_len; j++) {
251                 channel->hrtf[j] = channel->fftw_out[j];
252         }
253         channel->overlap_add = ast_calloc(default_sample_size, sizeof(float));
254
255         return 0;
256 #endif
257         return -1;
258 }
259
260 int init_convolve_channel_pair(struct convolve_channel_pair *cchan_pair,
261                 unsigned int hrtf_len, unsigned int chan_pos, unsigned int default_sample_size)
262 {
263 #ifdef BINAURAL_RENDERING
264         unsigned int hrirs_pos = chan_pos * 2;
265         int success = 0;
266
267         ast_debug(3, "Binaural pos for the new channel pair will be L: %d R: %d (pos id = %d)\n",
268                         hrirs_pos, hrirs_pos + 1, chan_pos);
269         success = init_convolve_channel(&cchan_pair->chan_left, hrtf_len, chan_pos, HRIRS_CHANNEL_LEFT,
270                         default_sample_size);
271         if (success == -1) {
272                 return success;
273         }
274
275         success = init_convolve_channel(&cchan_pair->chan_right, hrtf_len, chan_pos,
276                         HRIRS_CHANNEL_RIGHT, default_sample_size);
277         if (success == -1) {
278                 free_convolve_channel(&cchan_pair->chan_left);
279         }
280
281         return success;
282 #else
283         ast_log(LOG_ERROR, "Requesting data for the binaural conference feature "
284                         "without it beeing active.\n");
285
286         return -1;
287 #endif
288 }
289
290 int init_convolve_data(struct convolve_data *data, unsigned int default_sample_size)
291 {
292         unsigned int i;
293         unsigned int j;
294         int success;
295         success = 0;
296
297         data->pos_ids = ast_calloc(sizeof(int), sizeof(int) * CONVOLVE_CHANNEL_PREALLOC);
298         if (data->pos_ids == NULL) {
299                 return -1;
300         }
301         data->chan_size = CONVOLVE_CHANNEL_PREALLOC;
302         data->number_channels = 0;
303         data->cchan_pair = ast_malloc(sizeof(struct convolve_channel_pair *) *
304                         CONVOLVE_CHANNEL_PREALLOC);
305         if (data->cchan_pair == NULL) {
306                 ast_free(data->pos_ids);
307                 return -1;
308         }
309
310         for (i = 0; i < CONVOLVE_CHANNEL_PREALLOC; i++) {
311                 data->cchan_pair[i] = ast_malloc(sizeof(struct convolve_channel_pair));
312                 if (data->cchan_pair[i] == NULL) {
313                         ast_free(data->pos_ids);
314                         for (j = 0; j < i; j++) {
315                                 ast_free(data->cchan_pair[j]);
316                         }
317                         ast_free(data->cchan_pair);
318                         return -1;
319                 }
320         }
321
322         data->hrtf_length = (default_sample_size * 2) - 1;
323         for (i = 0; i < CONVOLVE_CHANNEL_PREALLOC; i++) {
324                 success = init_convolve_channel_pair(data->cchan_pair[i], data->hrtf_length, i,
325                                 default_sample_size);
326                 if (success == -1) {
327                         ast_free(data->pos_ids);
328                         for (j = 0; j < i; j++) {
329                                 free_convolve_channel_pair(data->cchan_pair[j]);
330                         }
331                         for (j = 0; j < CONVOLVE_CHANNEL_PREALLOC; j++) {
332                                 ast_free(data->cchan_pair[j]);
333                         }
334                         return -1;
335                 }
336         }
337
338         return success;
339 }
340
341 void free_convolve_channel(struct convolve_channel *cchan)
342 {
343 #ifdef BINAURAL_RENDERING
344         fftw_free(cchan->fftw_out);
345         fftw_free(cchan->fftw_in);
346         fftw_free(cchan->hrtf);
347         ast_free(cchan->overlap_add);
348         ast_free(cchan->out_data);
349         fftw_destroy_plan(cchan->fftw_plan);
350         fftw_destroy_plan(cchan->fftw_plan_inverse);
351 #endif
352 }
353
354 void free_convolve_channel_pair(struct convolve_channel_pair *cchan_pair)
355 {
356         free_convolve_channel(&cchan_pair->chan_left);
357         free_convolve_channel(&cchan_pair->chan_right);
358 }
359
360 void free_convolve_data(struct convolve_data *data)
361 {
362         unsigned int i;
363         ast_free(data->pos_ids);
364         for (i = 0; i < data->chan_size; i++) {
365                 free_convolve_channel_pair(data->cchan_pair[i]);
366                 ast_free(data->cchan_pair[i]);
367         }
368         ast_free(data->cchan_pair);
369 }
370
371 int set_binaural_data_join(struct convolve_data *data, unsigned int default_sample_size)
372 {
373         struct convolve_channel_pair **cchan_pair_tmp;
374         unsigned int i;
375         int *pos_ids_tmp;
376
377         /* Raise the number of input channels. */
378         data->number_channels++;
379         /* We realloc another channel pair if we are out of prealloced ones. */
380         /* We have prealloced one at the beginning of a conference and if a member leaves. */
381         if (data->chan_size < data->number_channels)  {
382                 data->chan_size += 1;
383
384                 pos_ids_tmp = ast_realloc(data->pos_ids, data->chan_size * sizeof(int));
385                 if (pos_ids_tmp) {
386                         data->pos_ids = pos_ids_tmp;
387                 } else {
388                         goto binaural_join_fails;
389                 }
390
391                 data->pos_ids[data->chan_size - 1] = 0;
392                 cchan_pair_tmp = ast_realloc(data->cchan_pair,
393                                 data->chan_size * sizeof(struct convolve_channel_pair *));
394                 if (cchan_pair_tmp) {
395                         data->cchan_pair = cchan_pair_tmp;
396                 } else {
397                         goto binaural_join_fails;
398                 }
399
400                 data->cchan_pair[data->chan_size - 1] = ast_malloc(sizeof(struct convolve_channel_pair));
401                 if (data->cchan_pair[data->chan_size - 1] == NULL) {
402                         goto binaural_join_fails;
403                 }
404
405                 i = init_convolve_channel_pair(data->cchan_pair[data->chan_size - 1], data->hrtf_length,
406                                 data->chan_size - 1, default_sample_size);
407                 if (i == -1) {
408                         goto binaural_join_fails;
409                 }
410         }
411
412         for (i = 0; i < data->chan_size; i++) {
413                 if (data->pos_ids[i] == 0) {
414                         data->pos_ids[i] = 1;
415                         break;
416                 }
417         }
418
419         return i;
420
421 binaural_join_fails:
422         data->number_channels--;
423         data->chan_size -= 1;
424
425         return -1;
426 }
427
428 void set_binaural_data_leave(struct convolve_data *data, unsigned int pos,
429                 unsigned int default_sample_size)
430 {
431         if (pos >= data->chan_size || data->pos_ids[pos] == 0) {
432                 return;
433         }
434
435         reset_channel_pair(data->cchan_pair[pos], default_sample_size);
436         data->number_channels--;
437         data->pos_ids[pos] = 0;
438 }
439
440 void softmix_process_write_binaural_audio(struct softmix_channel *sc,
441                 unsigned int default_sample_size)
442 {
443         unsigned int i;
444
445         if (sc->write_frame.samples % default_sample_size != 0) {
446                 return;
447         }
448
449         /* If binaural is suspended, the source audio (mono) will be removed. */
450         if (sc->binaural_suspended) {
451                 for (i = 0; i < default_sample_size; i++) {
452                         ast_slinear_saturated_subtract(&sc->final_buf[i * 2], &sc->our_buf[i]);
453                         ast_slinear_saturated_subtract(&sc->final_buf[(i * 2) + 1], &sc->our_buf[i]);
454                 }
455                 return;
456         }
457
458   /* If binaural is NOT suspended, the source audio (binaural) will be removed. */
459         for (i = 0; i < default_sample_size; i++) {
460                 ast_slinear_saturated_subtract(&sc->final_buf[i * 2],
461                                 &sc->our_chan_pair->chan_left.out_data[i]);
462                 ast_slinear_saturated_subtract(&sc->final_buf[(i * 2) + 1],
463                                 &sc->our_chan_pair->chan_right.out_data[i]);
464         }
465 }
466
467 void check_binaural_position_change(struct ast_bridge *bridge,
468                 struct softmix_bridge_data *softmix_data, struct ast_bridge_channel *bridge_channel)
469 {
470         unsigned int pos_change;
471
472         /*
473          * We only check binaural things if binaural is activated by the config
474          * and at least one binaural channel joined.
475          */
476         if (!(bridge->softmix.binaural_active && softmix_data->convolve.binaural_active)) {
477                 return;
478         }
479         /*
480          * Before we pull any audio, we must check if any channel requests a
481          * change of binaural positions.
482          */
483         pos_change = 0;
484         AST_LIST_TRAVERSE(&bridge->channels, bridge_channel, entry) {
485                 if (!bridge_channel->binaural_pos_change) {
486                         continue;
487                 }
488                 ast_bridge_channel_lock_bridge(bridge_channel);
489                 bridge_channel->binaural_pos_change = 0;
490                 ast_bridge_unlock(bridge_channel->bridge);
491                 pos_change = 1;
492         }
493
494         if (pos_change) {
495                 random_binaural_pos_change(softmix_data);
496         }
497 }
498
499 void add_binaural_mixing(struct ast_bridge *bridge, struct softmix_bridge_data *softmix_data,
500                 unsigned int softmix_samples, struct softmix_mixing_array *mixing_array,
501                 struct softmix_channel *sc, const char *channel_name)
502 {
503         struct convolve_channel_pair *pair;
504
505         pair = NULL;
506         /* We only check binaural things if at least one binaural channel joined. */
507         if (!(bridge->softmix.binaural_active && softmix_data->convolve.binaural_active
508                         && (softmix_samples % CONVOLUTION_SAMPLE_SIZE) == 0)) {
509                 return;
510         }
511
512         if (!sc->is_announcement) {
513                 pair = do_convolve_pair(&softmix_data->convolve, sc->binaural_pos,
514                                 mixing_array->buffers[mixing_array->used_entries], softmix_samples, channel_name);
515         }
516         sc->our_chan_pair = pair;
517         mixing_array->chan_pairs[mixing_array->used_entries] = pair;
518 }
519
520 void binaural_mixing(struct ast_bridge *bridge, struct softmix_bridge_data *softmix_data,
521                 struct softmix_mixing_array *mixing_array, int16_t *bin_buf, int16_t *ann_buf)
522 {
523         unsigned int idx;
524         unsigned int x;
525
526         if (!(bridge->softmix.binaural_active && softmix_data->convolve.binaural_active)) {
527                 return;
528         }
529         /* mix it like crazy (binaural channels) */
530         memset(bin_buf, 0, MAX_DATALEN);
531         memset(ann_buf, 0, MAX_DATALEN);
532
533         for (idx = 0; idx < mixing_array->used_entries; idx++) {
534                 if (mixing_array->chan_pairs[idx] == NULL) {
535                         for (x = 0; x < softmix_data->default_sample_size; x++) {
536                                 ast_slinear_saturated_add(bin_buf + (x * 2), mixing_array->buffers[idx] + x);
537                                 ast_slinear_saturated_add(bin_buf + (x * 2) + 1, mixing_array->buffers[idx] + x);
538                                 ann_buf[x * 2] = mixing_array->buffers[idx][x];
539                                 ann_buf[(x * 2) + 1] = mixing_array->buffers[idx][x];
540                         }
541                 } else {
542                         for (x = 0; x < softmix_data->default_sample_size; x++) {
543                                 ast_slinear_saturated_add(bin_buf + (x * 2),
544                                                 mixing_array->chan_pairs[idx]->chan_left.out_data + x);
545                                 ast_slinear_saturated_add(bin_buf + (x * 2) + 1,
546                                                 mixing_array->chan_pairs[idx]->chan_right.out_data + x);
547                         }
548                 }
549         }
550 }
551
552 void create_binaural_frame(struct ast_bridge_channel *bridge_channel,
553                 struct softmix_channel *sc, int16_t *bin_buf, int16_t *ann_buf,
554                 unsigned int softmix_datalen, unsigned int softmix_samples, int16_t *buf)
555 {
556         unsigned int i;
557
558         sc->write_frame.datalen = softmix_datalen * 2;
559         sc->write_frame.samples = softmix_samples * 2;
560         if (!bridge_channel->binaural_suspended) {
561                 sc->binaural_suspended = 0;
562                 if (sc->is_announcement) {
563                         memcpy(sc->final_buf, ann_buf, softmix_datalen * 2);
564                 } else {
565                         memcpy(sc->final_buf, bin_buf, softmix_datalen * 2);
566                 }
567                 return;
568         }
569
570         /*
571          * Mark that binaural output is suspended, since we use two channel audio
572          * we copy the same signals into both channels.
573          */
574         sc->binaural_suspended = 1;
575         for (i = 0; i < softmix_samples; i++) {
576                 sc->final_buf[i * 2] = buf[i];
577                 sc->final_buf[(i * 2) + 1] = buf[i];
578         }
579 }