]> git.sesse.net Git - ffmpeg/blob - libavfilter/af_surround.c
avfilter/af_surround: allow user to change overlap and win_func
[ffmpeg] / libavfilter / af_surround.c
1 /*
2  * Copyright (c) 2017 Paul B Mahol
3  *
4  * This file is part of FFmpeg.
5  *
6  * FFmpeg is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU Lesser General Public
8  * License as published by the Free Software Foundation; either
9  * version 2.1 of the License, or (at your option) any later version.
10  *
11  * FFmpeg is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with FFmpeg; if not, write to the Free Software
18  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
19  */
20
21 #include "libavutil/audio_fifo.h"
22 #include "libavutil/channel_layout.h"
23 #include "libavutil/opt.h"
24 #include "libavcodec/avfft.h"
25 #include "avfilter.h"
26 #include "audio.h"
27 #include "formats.h"
28 #include "window_func.h"
29
30 typedef struct AudioSurroundContext {
31     const AVClass *class;
32
33     char *out_channel_layout_str;
34     char *in_channel_layout_str;
35
36     float level_in;
37     float level_out;
38     float fc_in;
39     float fc_out;
40     float lfe_in;
41     float lfe_out;
42     int   win_func;
43     float overlap;
44
45     float *input_levels;
46     float *output_levels;
47     int output_lfe;
48     int lowcutf;
49     int highcutf;
50
51     float lowcut;
52     float highcut;
53
54     uint64_t out_channel_layout;
55     uint64_t in_channel_layout;
56     int nb_in_channels;
57     int nb_out_channels;
58
59     AVFrame *input;
60     AVFrame *output;
61     AVFrame *overlap_buffer;
62
63     int buf_size;
64     int hop_size;
65     AVAudioFifo *fifo;
66     RDFTContext **rdft, **irdft;
67     float *window_func_lut;
68
69     int64_t pts;
70
71     void (*filter)(AVFilterContext *ctx);
72     void (*upmix_stereo)(AVFilterContext *ctx,
73                          float l_phase,
74                          float r_phase,
75                          float c_phase,
76                          float mag_total,
77                          float x, float y,
78                          int n);
79     void (*upmix_2_1)(AVFilterContext *ctx,
80                       float l_phase,
81                       float r_phase,
82                       float c_phase,
83                       float mag_total,
84                       float lfe_im,
85                       float lfe_re,
86                       float x, float y,
87                       int n);
88     void (*upmix_3_0)(AVFilterContext *ctx,
89                       float l_phase,
90                       float r_phase,
91                       float c_mag,
92                       float c_phase,
93                       float mag_total,
94                       float x, float y,
95                       int n);
96     void (*upmix_5_0)(AVFilterContext *ctx,
97                       float c_re, float c_im,
98                       float mag_totall, float mag_totalr,
99                       float fl_phase, float fr_phase,
100                       float bl_phase, float br_phase,
101                       float sl_phase, float sr_phase,
102                       float xl, float yl,
103                       float xr, float yr,
104                       int n);
105     void (*upmix_5_1)(AVFilterContext *ctx,
106                       float c_re, float c_im,
107                       float lfe_re, float lfe_im,
108                       float mag_totall, float mag_totalr,
109                       float fl_phase, float fr_phase,
110                       float bl_phase, float br_phase,
111                       float sl_phase, float sr_phase,
112                       float xl, float yl,
113                       float xr, float yr,
114                       int n);
115 } AudioSurroundContext;
116
117 static int query_formats(AVFilterContext *ctx)
118 {
119     AudioSurroundContext *s = ctx->priv;
120     AVFilterFormats *formats = NULL;
121     AVFilterChannelLayouts *layouts = NULL;
122     int ret;
123
124     ret = ff_add_format(&formats, AV_SAMPLE_FMT_FLTP);
125     if (ret)
126         return ret;
127     ret = ff_set_common_formats(ctx, formats);
128     if (ret)
129         return ret;
130
131     layouts = NULL;
132     ret = ff_add_channel_layout(&layouts, s->out_channel_layout);
133     if (ret)
134         return ret;
135
136     ret = ff_channel_layouts_ref(layouts, &ctx->outputs[0]->in_channel_layouts);
137     if (ret)
138         return ret;
139
140     layouts = NULL;
141     ret = ff_add_channel_layout(&layouts, s->in_channel_layout);
142     if (ret)
143         return ret;
144
145     ret = ff_channel_layouts_ref(layouts, &ctx->inputs[0]->out_channel_layouts);
146     if (ret)
147         return ret;
148
149     formats = ff_all_samplerates();
150     if (!formats)
151         return AVERROR(ENOMEM);
152     return ff_set_common_samplerates(ctx, formats);
153 }
154
155 static int config_input(AVFilterLink *inlink)
156 {
157     AVFilterContext *ctx = inlink->dst;
158     AudioSurroundContext *s = ctx->priv;
159     int ch;
160
161     s->rdft = av_calloc(inlink->channels, sizeof(*s->rdft));
162     if (!s->rdft)
163         return AVERROR(ENOMEM);
164
165     for (ch = 0; ch < inlink->channels; ch++) {
166         s->rdft[ch]  = av_rdft_init(ff_log2(s->buf_size), DFT_R2C);
167         if (!s->rdft[ch])
168             return AVERROR(ENOMEM);
169     }
170     s->nb_in_channels = inlink->channels;
171     s->input_levels = av_malloc_array(s->nb_in_channels, sizeof(*s->input_levels));
172     if (!s->input_levels)
173         return AVERROR(ENOMEM);
174     for (ch = 0;  ch < s->nb_in_channels; ch++)
175         s->input_levels[ch] = s->level_in;
176     ch = av_get_channel_layout_channel_index(inlink->channel_layout, AV_CH_FRONT_CENTER);
177     if (ch >= 0)
178         s->input_levels[ch] *= s->fc_in;
179     ch = av_get_channel_layout_channel_index(inlink->channel_layout, AV_CH_LOW_FREQUENCY);
180     if (ch >= 0)
181         s->input_levels[ch] *= s->lfe_in;
182
183     s->input = ff_get_audio_buffer(inlink, s->buf_size * 2);
184     if (!s->input)
185         return AVERROR(ENOMEM);
186
187     s->fifo = av_audio_fifo_alloc(inlink->format, inlink->channels, s->buf_size);
188     if (!s->fifo)
189         return AVERROR(ENOMEM);
190
191     s->lowcut = 1.f * s->lowcutf / (inlink->sample_rate * 0.5) * (s->buf_size / 2);
192     s->highcut = 1.f * s->highcutf / (inlink->sample_rate * 0.5) * (s->buf_size / 2);
193
194     return 0;
195 }
196
197 static int config_output(AVFilterLink *outlink)
198 {
199     AVFilterContext *ctx = outlink->src;
200     AudioSurroundContext *s = ctx->priv;
201     int ch;
202
203     s->irdft = av_calloc(outlink->channels, sizeof(*s->irdft));
204     if (!s->irdft)
205         return AVERROR(ENOMEM);
206
207     for (ch = 0; ch < outlink->channels; ch++) {
208         s->irdft[ch] = av_rdft_init(ff_log2(s->buf_size), IDFT_C2R);
209         if (!s->irdft[ch])
210             return AVERROR(ENOMEM);
211     }
212     s->nb_out_channels = outlink->channels;
213     s->output_levels = av_malloc_array(s->nb_out_channels, sizeof(*s->output_levels));
214     if (!s->output_levels)
215         return AVERROR(ENOMEM);
216     for (ch = 0;  ch < s->nb_out_channels; ch++)
217         s->output_levels[ch] = s->level_out;
218     ch = av_get_channel_layout_channel_index(outlink->channel_layout, AV_CH_FRONT_CENTER);
219     if (ch >= 0)
220         s->output_levels[ch] *= s->fc_out;
221     ch = av_get_channel_layout_channel_index(outlink->channel_layout, AV_CH_LOW_FREQUENCY);
222     if (ch >= 0)
223         s->output_levels[ch] *= s->lfe_out;
224
225     s->output = ff_get_audio_buffer(outlink, s->buf_size * 2);
226     s->overlap_buffer = ff_get_audio_buffer(outlink, s->buf_size * 2);
227     if (!s->overlap_buffer || !s->output)
228         return AVERROR(ENOMEM);
229
230     return 0;
231 }
232
233 static void stereo_position(float a, float p, float *x, float *y)
234 {
235     *x = av_clipf(a+FFMAX(0, sinf(p-M_PI_2))*FFDIFFSIGN(a,0), -1, 1);
236     *y = av_clipf(cosf(a*M_PI_2+M_PI)*cosf(M_PI_2-p/M_PI)*M_LN10+1, -1, 1);
237 }
238
239 static inline void get_lfe(int output_lfe, int n, float lowcut, float highcut,
240                            float *lfe_mag, float *mag_total)
241 {
242     if (output_lfe && n < highcut) {
243         *lfe_mag    = n < lowcut ? 1.f : .5f*(1.f+cosf(M_PI*(lowcut-n)/(lowcut-highcut)));
244         *lfe_mag   *= *mag_total;
245         *mag_total -= *lfe_mag;
246     } else {
247         *lfe_mag = 0.f;
248     }
249 }
250
251 static void upmix_1_0(AVFilterContext *ctx,
252                       float l_phase,
253                       float r_phase,
254                       float c_phase,
255                       float mag_total,
256                       float x, float y,
257                       int n)
258 {
259     AudioSurroundContext *s = ctx->priv;
260     float mag, *dst;
261
262     dst = (float *)s->output->extended_data[0];
263
264     mag = sqrtf(1.f - fabsf(x)) * ((y + 1.f) * .5f) * mag_total;
265
266     dst[2 * n    ] = mag * cosf(c_phase);
267     dst[2 * n + 1] = mag * sinf(c_phase);
268 }
269
270 static void upmix_stereo(AVFilterContext *ctx,
271                          float l_phase,
272                          float r_phase,
273                          float c_phase,
274                          float mag_total,
275                          float x, float y,
276                          int n)
277 {
278     AudioSurroundContext *s = ctx->priv;
279     float l_mag, r_mag, *dstl, *dstr;
280
281     dstl = (float *)s->output->extended_data[0];
282     dstr = (float *)s->output->extended_data[1];
283
284     l_mag = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
285     r_mag = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
286
287     dstl[2 * n    ] = l_mag * cosf(l_phase);
288     dstl[2 * n + 1] = l_mag * sinf(l_phase);
289
290     dstr[2 * n    ] = r_mag * cosf(r_phase);
291     dstr[2 * n + 1] = r_mag * sinf(r_phase);
292 }
293
294 static void upmix_2_1(AVFilterContext *ctx,
295                       float l_phase,
296                       float r_phase,
297                       float c_phase,
298                       float mag_total,
299                       float x, float y,
300                       int n)
301 {
302     AudioSurroundContext *s = ctx->priv;
303     float lfe_mag, l_mag, r_mag, *dstl, *dstr, *dstlfe;
304
305     dstl = (float *)s->output->extended_data[0];
306     dstr = (float *)s->output->extended_data[1];
307     dstlfe = (float *)s->output->extended_data[2];
308
309     get_lfe(s->output_lfe, n, s->lowcut, s->highcut, &lfe_mag, &mag_total);
310
311     l_mag = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
312     r_mag = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
313
314     dstl[2 * n    ] = l_mag * cosf(l_phase);
315     dstl[2 * n + 1] = l_mag * sinf(l_phase);
316
317     dstr[2 * n    ] = r_mag * cosf(r_phase);
318     dstr[2 * n + 1] = r_mag * sinf(r_phase);
319
320     dstlfe[2 * n    ] = lfe_mag * cosf(c_phase);
321     dstlfe[2 * n + 1] = lfe_mag * sinf(c_phase);
322 }
323
324 static void upmix_3_0(AVFilterContext *ctx,
325                       float l_phase,
326                       float r_phase,
327                       float c_phase,
328                       float mag_total,
329                       float x, float y,
330                       int n)
331 {
332     AudioSurroundContext *s = ctx->priv;
333     float l_mag, r_mag, c_mag, *dstc, *dstl, *dstr;
334
335     dstl = (float *)s->output->extended_data[0];
336     dstr = (float *)s->output->extended_data[1];
337     dstc = (float *)s->output->extended_data[2];
338
339     c_mag = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
340     l_mag = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
341     r_mag = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
342
343     dstl[2 * n    ] = l_mag * cosf(l_phase);
344     dstl[2 * n + 1] = l_mag * sinf(l_phase);
345
346     dstr[2 * n    ] = r_mag * cosf(r_phase);
347     dstr[2 * n + 1] = r_mag * sinf(r_phase);
348
349     dstc[2 * n    ] = c_mag * cosf(c_phase);
350     dstc[2 * n + 1] = c_mag * sinf(c_phase);
351 }
352
353 static void upmix_3_1(AVFilterContext *ctx,
354                       float l_phase,
355                       float r_phase,
356                       float c_phase,
357                       float mag_total,
358                       float x, float y,
359                       int n)
360 {
361     AudioSurroundContext *s = ctx->priv;
362     float lfe_mag, l_mag, r_mag, c_mag, *dstc, *dstl, *dstr, *dstlfe;
363
364     dstl = (float *)s->output->extended_data[0];
365     dstr = (float *)s->output->extended_data[1];
366     dstc = (float *)s->output->extended_data[2];
367     dstlfe = (float *)s->output->extended_data[3];
368
369     get_lfe(s->output_lfe, n, s->lowcut, s->highcut, &lfe_mag, &mag_total);
370
371     c_mag = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
372     l_mag = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
373     r_mag = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
374
375     dstl[2 * n    ] = l_mag * cosf(l_phase);
376     dstl[2 * n + 1] = l_mag * sinf(l_phase);
377
378     dstr[2 * n    ] = r_mag * cosf(r_phase);
379     dstr[2 * n + 1] = r_mag * sinf(r_phase);
380
381     dstc[2 * n    ] = c_mag * cosf(c_phase);
382     dstc[2 * n + 1] = c_mag * sinf(c_phase);
383
384     dstlfe[2 * n    ] = lfe_mag * cosf(c_phase);
385     dstlfe[2 * n + 1] = lfe_mag * sinf(c_phase);
386 }
387
388 static void upmix_3_1_surround(AVFilterContext *ctx,
389                                float l_phase,
390                                float r_phase,
391                                float c_phase,
392                                float c_mag,
393                                float mag_total,
394                                float x, float y,
395                                int n)
396 {
397     AudioSurroundContext *s = ctx->priv;
398     float lfe_mag, l_mag, r_mag, *dstc, *dstl, *dstr, *dstlfe;
399
400     dstl = (float *)s->output->extended_data[0];
401     dstr = (float *)s->output->extended_data[1];
402     dstc = (float *)s->output->extended_data[2];
403     dstlfe = (float *)s->output->extended_data[3];
404
405     get_lfe(s->output_lfe, n, s->lowcut, s->highcut, &lfe_mag, &c_mag);
406
407     l_mag = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
408     r_mag = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
409
410     dstl[2 * n    ] = l_mag * cosf(l_phase);
411     dstl[2 * n + 1] = l_mag * sinf(l_phase);
412
413     dstr[2 * n    ] = r_mag * cosf(r_phase);
414     dstr[2 * n + 1] = r_mag * sinf(r_phase);
415
416     dstc[2 * n    ] = c_mag * cosf(c_phase);
417     dstc[2 * n + 1] = c_mag * sinf(c_phase);
418
419     dstlfe[2 * n    ] = lfe_mag * cosf(c_phase);
420     dstlfe[2 * n + 1] = lfe_mag * sinf(c_phase);
421 }
422
423 static void upmix_4_0(AVFilterContext *ctx,
424                       float l_phase,
425                       float r_phase,
426                       float c_phase,
427                       float mag_total,
428                       float x, float y,
429                       int n)
430 {
431     float b_mag, l_mag, r_mag, c_mag, *dstc, *dstl, *dstr, *dstb;
432     AudioSurroundContext *s = ctx->priv;
433
434     dstl = (float *)s->output->extended_data[0];
435     dstr = (float *)s->output->extended_data[1];
436     dstc = (float *)s->output->extended_data[2];
437     dstb = (float *)s->output->extended_data[3];
438
439     c_mag = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
440     b_mag = sqrtf(1.f - fabsf(x))   * ((1.f - y) * .5f) * mag_total;
441     l_mag = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
442     r_mag = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
443
444     dstl[2 * n    ] = l_mag * cosf(l_phase);
445     dstl[2 * n + 1] = l_mag * sinf(l_phase);
446
447     dstr[2 * n    ] = r_mag * cosf(r_phase);
448     dstr[2 * n + 1] = r_mag * sinf(r_phase);
449
450     dstc[2 * n    ] = c_mag * cosf(c_phase);
451     dstc[2 * n + 1] = c_mag * sinf(c_phase);
452
453     dstb[2 * n    ] = b_mag * cosf(c_phase);
454     dstb[2 * n + 1] = b_mag * sinf(c_phase);
455 }
456
457 static void upmix_4_1(AVFilterContext *ctx,
458                       float l_phase,
459                       float r_phase,
460                       float c_phase,
461                       float mag_total,
462                       float x, float y,
463                       int n)
464 {
465     float lfe_mag, b_mag, l_mag, r_mag, c_mag, *dstc, *dstl, *dstr, *dstb, *dstlfe;
466     AudioSurroundContext *s = ctx->priv;
467
468     dstl = (float *)s->output->extended_data[0];
469     dstr = (float *)s->output->extended_data[1];
470     dstc = (float *)s->output->extended_data[2];
471     dstlfe = (float *)s->output->extended_data[3];
472     dstb = (float *)s->output->extended_data[4];
473
474     get_lfe(s->output_lfe, n, s->lowcut, s->highcut, &lfe_mag, &mag_total);
475
476     dstlfe[2 * n    ] = lfe_mag * cosf(c_phase);
477     dstlfe[2 * n + 1] = lfe_mag * sinf(c_phase);
478
479     c_mag = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
480     b_mag = sqrtf(1.f - fabsf(x))   * ((1.f - y) * .5f) * mag_total;
481     l_mag = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
482     r_mag = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
483
484     dstl[2 * n    ] = l_mag * cosf(l_phase);
485     dstl[2 * n + 1] = l_mag * sinf(l_phase);
486
487     dstr[2 * n    ] = r_mag * cosf(r_phase);
488     dstr[2 * n + 1] = r_mag * sinf(r_phase);
489
490     dstc[2 * n    ] = c_mag * cosf(c_phase);
491     dstc[2 * n + 1] = c_mag * sinf(c_phase);
492
493     dstb[2 * n    ] = b_mag * cosf(c_phase);
494     dstb[2 * n + 1] = b_mag * sinf(c_phase);
495 }
496
497 static void upmix_5_0_back(AVFilterContext *ctx,
498                            float l_phase,
499                            float r_phase,
500                            float c_phase,
501                            float mag_total,
502                            float x, float y,
503                            int n)
504 {
505     float l_mag, r_mag, ls_mag, rs_mag, c_mag, *dstc, *dstl, *dstr, *dstls, *dstrs;
506     AudioSurroundContext *s = ctx->priv;
507
508     dstl  = (float *)s->output->extended_data[0];
509     dstr  = (float *)s->output->extended_data[1];
510     dstc  = (float *)s->output->extended_data[2];
511     dstls = (float *)s->output->extended_data[3];
512     dstrs = (float *)s->output->extended_data[4];
513
514     c_mag  = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
515     l_mag  = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
516     r_mag  = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
517     ls_mag = sqrtf(.5f * ( x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
518     rs_mag = sqrtf(.5f * (-x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
519
520     dstl[2 * n    ] = l_mag * cosf(l_phase);
521     dstl[2 * n + 1] = l_mag * sinf(l_phase);
522
523     dstr[2 * n    ] = r_mag * cosf(r_phase);
524     dstr[2 * n + 1] = r_mag * sinf(r_phase);
525
526     dstc[2 * n    ] = c_mag * cosf(c_phase);
527     dstc[2 * n + 1] = c_mag * sinf(c_phase);
528
529     dstls[2 * n    ] = ls_mag * cosf(l_phase);
530     dstls[2 * n + 1] = ls_mag * sinf(l_phase);
531
532     dstrs[2 * n    ] = rs_mag * cosf(r_phase);
533     dstrs[2 * n + 1] = rs_mag * sinf(r_phase);
534 }
535
536 static void upmix_5_1_back(AVFilterContext *ctx,
537                            float l_phase,
538                            float r_phase,
539                            float c_phase,
540                            float mag_total,
541                            float x, float y,
542                            int n)
543 {
544     float lfe_mag, l_mag, r_mag, ls_mag, rs_mag, c_mag, *dstc, *dstl, *dstr, *dstls, *dstrs, *dstlfe;
545     AudioSurroundContext *s = ctx->priv;
546
547     dstl  = (float *)s->output->extended_data[0];
548     dstr  = (float *)s->output->extended_data[1];
549     dstc  = (float *)s->output->extended_data[2];
550     dstlfe = (float *)s->output->extended_data[3];
551     dstls = (float *)s->output->extended_data[4];
552     dstrs = (float *)s->output->extended_data[5];
553
554     get_lfe(s->output_lfe, n, s->lowcut, s->highcut, &lfe_mag, &mag_total);
555
556     c_mag  = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
557     l_mag  = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
558     r_mag  = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
559     ls_mag = sqrtf(.5f * ( x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
560     rs_mag = sqrtf(.5f * (-x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
561
562     dstl[2 * n    ] = l_mag * cosf(l_phase);
563     dstl[2 * n + 1] = l_mag * sinf(l_phase);
564
565     dstr[2 * n    ] = r_mag * cosf(r_phase);
566     dstr[2 * n + 1] = r_mag * sinf(r_phase);
567
568     dstc[2 * n    ] = c_mag * cosf(c_phase);
569     dstc[2 * n + 1] = c_mag * sinf(c_phase);
570
571     dstlfe[2 * n    ] = lfe_mag * cosf(c_phase);
572     dstlfe[2 * n + 1] = lfe_mag * sinf(c_phase);
573
574     dstls[2 * n    ] = ls_mag * cosf(l_phase);
575     dstls[2 * n + 1] = ls_mag * sinf(l_phase);
576
577     dstrs[2 * n    ] = rs_mag * cosf(r_phase);
578     dstrs[2 * n + 1] = rs_mag * sinf(r_phase);
579 }
580
581 static void upmix_5_1_back_surround(AVFilterContext *ctx,
582                                     float l_phase,
583                                     float r_phase,
584                                     float c_phase,
585                                     float c_mag,
586                                     float mag_total,
587                                     float x, float y,
588                                     int n)
589 {
590     AudioSurroundContext *s = ctx->priv;
591     float lfe_mag, l_mag, r_mag, *dstc, *dstl, *dstr, *dstlfe;
592     float ls_mag, rs_mag, *dstls, *dstrs;
593
594     dstl = (float *)s->output->extended_data[0];
595     dstr = (float *)s->output->extended_data[1];
596     dstc = (float *)s->output->extended_data[2];
597     dstlfe = (float *)s->output->extended_data[3];
598     dstls = (float *)s->output->extended_data[4];
599     dstrs = (float *)s->output->extended_data[5];
600
601     get_lfe(s->output_lfe, n, s->lowcut, s->highcut, &lfe_mag, &c_mag);
602
603     l_mag = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
604     r_mag = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
605     ls_mag = sqrtf(.5f * ( x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
606     rs_mag = sqrtf(.5f * (-x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
607
608     dstl[2 * n    ] = l_mag * cosf(l_phase);
609     dstl[2 * n + 1] = l_mag * sinf(l_phase);
610
611     dstr[2 * n    ] = r_mag * cosf(r_phase);
612     dstr[2 * n + 1] = r_mag * sinf(r_phase);
613
614     dstc[2 * n    ] = c_mag * cosf(c_phase);
615     dstc[2 * n + 1] = c_mag * sinf(c_phase);
616
617     dstlfe[2 * n    ] = lfe_mag * cosf(c_phase);
618     dstlfe[2 * n + 1] = lfe_mag * sinf(c_phase);
619
620     dstls[2 * n    ] = ls_mag * cosf(l_phase);
621     dstls[2 * n + 1] = ls_mag * sinf(l_phase);
622
623     dstrs[2 * n    ] = rs_mag * cosf(r_phase);
624     dstrs[2 * n + 1] = rs_mag * sinf(r_phase);
625 }
626
627 static void upmix_5_1_back_2_1(AVFilterContext *ctx,
628                                float l_phase,
629                                float r_phase,
630                                float c_phase,
631                                float mag_total,
632                                float lfe_re,
633                                float lfe_im,
634                                float x, float y,
635                                int n)
636 {
637     AudioSurroundContext *s = ctx->priv;
638     float c_mag, l_mag, r_mag, *dstc, *dstl, *dstr, *dstlfe;
639     float ls_mag, rs_mag, *dstls, *dstrs;
640
641     dstl = (float *)s->output->extended_data[0];
642     dstr = (float *)s->output->extended_data[1];
643     dstc = (float *)s->output->extended_data[2];
644     dstlfe = (float *)s->output->extended_data[3];
645     dstls = (float *)s->output->extended_data[4];
646     dstrs = (float *)s->output->extended_data[5];
647
648     c_mag  = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
649     l_mag  = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
650     r_mag  = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
651     ls_mag = sqrtf(.5f * ( x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
652     rs_mag = sqrtf(.5f * (-x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
653
654     dstl[2 * n    ] = l_mag * cosf(l_phase);
655     dstl[2 * n + 1] = l_mag * sinf(l_phase);
656
657     dstr[2 * n    ] = r_mag * cosf(r_phase);
658     dstr[2 * n + 1] = r_mag * sinf(r_phase);
659
660     dstc[2 * n    ] = c_mag * cosf(c_phase);
661     dstc[2 * n + 1] = c_mag * sinf(c_phase);
662
663     dstlfe[2 * n    ] = lfe_re;
664     dstlfe[2 * n + 1] = lfe_im;
665
666     dstls[2 * n    ] = ls_mag * cosf(l_phase);
667     dstls[2 * n + 1] = ls_mag * sinf(l_phase);
668
669     dstrs[2 * n    ] = rs_mag * cosf(r_phase);
670     dstrs[2 * n + 1] = rs_mag * sinf(r_phase);
671 }
672
673 static void upmix_7_0(AVFilterContext *ctx,
674                       float l_phase,
675                       float r_phase,
676                       float c_phase,
677                       float mag_total,
678                       float x, float y,
679                       int n)
680 {
681     float l_mag, r_mag, ls_mag, rs_mag, c_mag, lb_mag, rb_mag;
682     float *dstc, *dstl, *dstr, *dstls, *dstrs, *dstlb, *dstrb;
683     AudioSurroundContext *s = ctx->priv;
684
685     dstl  = (float *)s->output->extended_data[0];
686     dstr  = (float *)s->output->extended_data[1];
687     dstc  = (float *)s->output->extended_data[2];
688     dstlb = (float *)s->output->extended_data[3];
689     dstrb = (float *)s->output->extended_data[4];
690     dstls = (float *)s->output->extended_data[5];
691     dstrs = (float *)s->output->extended_data[6];
692
693     c_mag  = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
694     l_mag  = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
695     r_mag  = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
696     lb_mag = sqrtf(.5f * ( x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
697     rb_mag = sqrtf(.5f * (-x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
698     ls_mag = sqrtf(.5f * ( x + 1.f)) * (1.f - fabsf(y)) * mag_total;
699     rs_mag = sqrtf(.5f * (-x + 1.f)) * (1.f - fabsf(y)) * mag_total;
700
701     dstl[2 * n    ] = l_mag * cosf(l_phase);
702     dstl[2 * n + 1] = l_mag * sinf(l_phase);
703
704     dstr[2 * n    ] = r_mag * cosf(r_phase);
705     dstr[2 * n + 1] = r_mag * sinf(r_phase);
706
707     dstc[2 * n    ] = c_mag * cosf(c_phase);
708     dstc[2 * n + 1] = c_mag * sinf(c_phase);
709
710     dstlb[2 * n    ] = lb_mag * cosf(l_phase);
711     dstlb[2 * n + 1] = lb_mag * sinf(l_phase);
712
713     dstrb[2 * n    ] = rb_mag * cosf(r_phase);
714     dstrb[2 * n + 1] = rb_mag * sinf(r_phase);
715
716     dstls[2 * n    ] = ls_mag * cosf(l_phase);
717     dstls[2 * n + 1] = ls_mag * sinf(l_phase);
718
719     dstrs[2 * n    ] = rs_mag * cosf(r_phase);
720     dstrs[2 * n + 1] = rs_mag * sinf(r_phase);
721 }
722
723 static void upmix_7_1(AVFilterContext *ctx,
724                       float l_phase,
725                       float r_phase,
726                       float c_phase,
727                       float mag_total,
728                       float x, float y,
729                       int n)
730 {
731     float lfe_mag, l_mag, r_mag, ls_mag, rs_mag, c_mag, lb_mag, rb_mag;
732     float *dstc, *dstl, *dstr, *dstls, *dstrs, *dstlb, *dstrb, *dstlfe;
733     AudioSurroundContext *s = ctx->priv;
734
735     dstl  = (float *)s->output->extended_data[0];
736     dstr  = (float *)s->output->extended_data[1];
737     dstc  = (float *)s->output->extended_data[2];
738     dstlfe = (float *)s->output->extended_data[3];
739     dstlb = (float *)s->output->extended_data[4];
740     dstrb = (float *)s->output->extended_data[5];
741     dstls = (float *)s->output->extended_data[6];
742     dstrs = (float *)s->output->extended_data[7];
743
744     get_lfe(s->output_lfe, n, s->lowcut, s->highcut, &lfe_mag, &mag_total);
745
746     c_mag  = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
747     l_mag  = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
748     r_mag  = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
749     lb_mag = sqrtf(.5f * ( x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
750     rb_mag = sqrtf(.5f * (-x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
751     ls_mag = sqrtf(.5f * ( x + 1.f)) * (1.f - fabsf(y)) * mag_total;
752     rs_mag = sqrtf(.5f * (-x + 1.f)) * (1.f - fabsf(y)) * mag_total;
753
754     dstl[2 * n    ] = l_mag * cosf(l_phase);
755     dstl[2 * n + 1] = l_mag * sinf(l_phase);
756
757     dstr[2 * n    ] = r_mag * cosf(r_phase);
758     dstr[2 * n + 1] = r_mag * sinf(r_phase);
759
760     dstc[2 * n    ] = c_mag * cosf(c_phase);
761     dstc[2 * n + 1] = c_mag * sinf(c_phase);
762
763     dstlfe[2 * n    ] = lfe_mag * cosf(c_phase);
764     dstlfe[2 * n + 1] = lfe_mag * sinf(c_phase);
765
766     dstlb[2 * n    ] = lb_mag * cosf(l_phase);
767     dstlb[2 * n + 1] = lb_mag * sinf(l_phase);
768
769     dstrb[2 * n    ] = rb_mag * cosf(r_phase);
770     dstrb[2 * n + 1] = rb_mag * sinf(r_phase);
771
772     dstls[2 * n    ] = ls_mag * cosf(l_phase);
773     dstls[2 * n + 1] = ls_mag * sinf(l_phase);
774
775     dstrs[2 * n    ] = rs_mag * cosf(r_phase);
776     dstrs[2 * n + 1] = rs_mag * sinf(r_phase);
777 }
778
779 static void upmix_7_1_5_0_side(AVFilterContext *ctx,
780                                float c_re, float c_im,
781                                float mag_totall, float mag_totalr,
782                                float fl_phase, float fr_phase,
783                                float bl_phase, float br_phase,
784                                float sl_phase, float sr_phase,
785                                float xl, float yl,
786                                float xr, float yr,
787                                int n)
788 {
789     float fl_mag, fr_mag, ls_mag, rs_mag, lb_mag, rb_mag;
790     float *dstc, *dstl, *dstr, *dstls, *dstrs, *dstlb, *dstrb, *dstlfe;
791     float lfe_mag, c_phase, mag_total = (mag_totall + mag_totalr) * 0.5;
792     AudioSurroundContext *s = ctx->priv;
793
794     dstl  = (float *)s->output->extended_data[0];
795     dstr  = (float *)s->output->extended_data[1];
796     dstc  = (float *)s->output->extended_data[2];
797     dstlfe = (float *)s->output->extended_data[3];
798     dstlb = (float *)s->output->extended_data[4];
799     dstrb = (float *)s->output->extended_data[5];
800     dstls = (float *)s->output->extended_data[6];
801     dstrs = (float *)s->output->extended_data[7];
802
803     c_phase = atan2f(c_im, c_re);
804
805     get_lfe(s->output_lfe, n, s->lowcut, s->highcut, &lfe_mag, &mag_total);
806
807     fl_mag = sqrtf(.5f * (xl + 1.f)) * ((yl + 1.f) * .5f) * mag_totall;
808     fr_mag = sqrtf(.5f * (xr + 1.f)) * ((yr + 1.f) * .5f) * mag_totalr;
809     lb_mag = sqrtf(.5f * (-xl + 1.f)) * ((yl + 1.f) * .5f) * mag_totall;
810     rb_mag = sqrtf(.5f * (-xr + 1.f)) * ((yr + 1.f) * .5f) * mag_totalr;
811     ls_mag = sqrtf(1.f - fabsf(xl)) * ((yl + 1.f) * .5f) * mag_totall;
812     rs_mag = sqrtf(1.f - fabsf(xr)) * ((yr + 1.f) * .5f) * mag_totalr;
813
814     dstl[2 * n    ] = fl_mag * cosf(fl_phase);
815     dstl[2 * n + 1] = fl_mag * sinf(fl_phase);
816
817     dstr[2 * n    ] = fr_mag * cosf(fr_phase);
818     dstr[2 * n + 1] = fr_mag * sinf(fr_phase);
819
820     dstc[2 * n    ] = c_re;
821     dstc[2 * n + 1] = c_im;
822
823     dstlfe[2 * n    ] = lfe_mag * cosf(c_phase);
824     dstlfe[2 * n + 1] = lfe_mag * sinf(c_phase);
825
826     dstlb[2 * n    ] = lb_mag * cosf(bl_phase);
827     dstlb[2 * n + 1] = lb_mag * sinf(bl_phase);
828
829     dstrb[2 * n    ] = rb_mag * cosf(br_phase);
830     dstrb[2 * n + 1] = rb_mag * sinf(br_phase);
831
832     dstls[2 * n    ] = ls_mag * cosf(sl_phase);
833     dstls[2 * n + 1] = ls_mag * sinf(sl_phase);
834
835     dstrs[2 * n    ] = rs_mag * cosf(sr_phase);
836     dstrs[2 * n + 1] = rs_mag * sinf(sr_phase);
837 }
838
839 static void upmix_7_1_5_1(AVFilterContext *ctx,
840                           float c_re, float c_im,
841                           float lfe_re, float lfe_im,
842                           float mag_totall, float mag_totalr,
843                           float fl_phase, float fr_phase,
844                           float bl_phase, float br_phase,
845                           float sl_phase, float sr_phase,
846                           float xl, float yl,
847                           float xr, float yr,
848                           int n)
849 {
850     float fl_mag, fr_mag, ls_mag, rs_mag, lb_mag, rb_mag;
851     float *dstc, *dstl, *dstr, *dstls, *dstrs, *dstlb, *dstrb, *dstlfe;
852     AudioSurroundContext *s = ctx->priv;
853
854     dstl  = (float *)s->output->extended_data[0];
855     dstr  = (float *)s->output->extended_data[1];
856     dstc  = (float *)s->output->extended_data[2];
857     dstlfe = (float *)s->output->extended_data[3];
858     dstlb = (float *)s->output->extended_data[4];
859     dstrb = (float *)s->output->extended_data[5];
860     dstls = (float *)s->output->extended_data[6];
861     dstrs = (float *)s->output->extended_data[7];
862
863     fl_mag = sqrtf(.5f * (xl + 1.f)) * ((yl + 1.f) * .5f) * mag_totall;
864     fr_mag = sqrtf(.5f * (xr + 1.f)) * ((yr + 1.f) * .5f) * mag_totalr;
865     lb_mag = sqrtf(.5f * (-xl + 1.f)) * ((yl + 1.f) * .5f) * mag_totall;
866     rb_mag = sqrtf(.5f * (-xr + 1.f)) * ((yr + 1.f) * .5f) * mag_totalr;
867     ls_mag = sqrtf(1.f - fabsf(xl)) * ((yl + 1.f) * .5f) * mag_totall;
868     rs_mag = sqrtf(1.f - fabsf(xr)) * ((yr + 1.f) * .5f) * mag_totalr;
869
870     dstl[2 * n    ] = fl_mag * cosf(fl_phase);
871     dstl[2 * n + 1] = fl_mag * sinf(fl_phase);
872
873     dstr[2 * n    ] = fr_mag * cosf(fr_phase);
874     dstr[2 * n + 1] = fr_mag * sinf(fr_phase);
875
876     dstc[2 * n    ] = c_re;
877     dstc[2 * n + 1] = c_im;
878
879     dstlfe[2 * n    ] = lfe_re;
880     dstlfe[2 * n + 1] = lfe_im;
881
882     dstlb[2 * n    ] = lb_mag * cosf(bl_phase);
883     dstlb[2 * n + 1] = lb_mag * sinf(bl_phase);
884
885     dstrb[2 * n    ] = rb_mag * cosf(br_phase);
886     dstrb[2 * n + 1] = rb_mag * sinf(br_phase);
887
888     dstls[2 * n    ] = ls_mag * cosf(sl_phase);
889     dstls[2 * n + 1] = ls_mag * sinf(sl_phase);
890
891     dstrs[2 * n    ] = rs_mag * cosf(sr_phase);
892     dstrs[2 * n + 1] = rs_mag * sinf(sr_phase);
893 }
894
895 static void filter_stereo(AVFilterContext *ctx)
896 {
897     AudioSurroundContext *s = ctx->priv;
898     float *srcl, *srcr;
899     int n;
900
901     srcl = (float *)s->input->extended_data[0];
902     srcr = (float *)s->input->extended_data[1];
903
904     for (n = 0; n < s->buf_size; n++) {
905         float l_re = srcl[2 * n], r_re = srcr[2 * n];
906         float l_im = srcl[2 * n + 1], r_im = srcr[2 * n + 1];
907         float c_phase = atan2f(l_im + r_im, l_re + r_re);
908         float l_mag = hypotf(l_re, l_im);
909         float r_mag = hypotf(r_re, r_im);
910         float l_phase = atan2f(l_im, l_re);
911         float r_phase = atan2f(r_im, r_re);
912         float phase_dif = fabsf(l_phase - r_phase);
913         float mag_dif = (l_mag - r_mag) / (l_mag + r_mag);
914         float mag_total = hypotf(l_mag, r_mag);
915         float x, y;
916
917         if (phase_dif > M_PI)
918             phase_dif = 2 * M_PI - phase_dif;
919
920         stereo_position(mag_dif, phase_dif, &x, &y);
921
922         s->upmix_stereo(ctx, l_phase, r_phase, c_phase, mag_total, x, y, n);
923     }
924 }
925
926 static void filter_surround(AVFilterContext *ctx)
927 {
928     AudioSurroundContext *s = ctx->priv;
929     float *srcl, *srcr, *srcc;
930     int n;
931
932     srcl = (float *)s->input->extended_data[0];
933     srcr = (float *)s->input->extended_data[1];
934     srcc = (float *)s->input->extended_data[2];
935
936     for (n = 0; n < s->buf_size; n++) {
937         float l_re = srcl[2 * n], r_re = srcr[2 * n];
938         float l_im = srcl[2 * n + 1], r_im = srcr[2 * n + 1];
939         float c_re = srcc[2 * n], c_im = srcc[2 * n + 1];
940         float c_mag = hypotf(c_re, c_im);
941         float c_phase = atan2f(c_im, c_re);
942         float l_mag = hypotf(l_re, l_im);
943         float r_mag = hypotf(r_re, r_im);
944         float l_phase = atan2f(l_im, l_re);
945         float r_phase = atan2f(r_im, r_re);
946         float phase_dif = fabsf(l_phase - r_phase);
947         float mag_dif = (l_mag - r_mag) / (l_mag + r_mag);
948         float mag_total = hypotf(l_mag, r_mag);
949         float x, y;
950
951         if (phase_dif > M_PI)
952             phase_dif = 2 * M_PI - phase_dif;
953
954         stereo_position(mag_dif, phase_dif, &x, &y);
955
956         s->upmix_3_0(ctx, l_phase, r_phase, c_phase, c_mag, mag_total, x, y, n);
957     }
958 }
959
960 static void filter_2_1(AVFilterContext *ctx)
961 {
962     AudioSurroundContext *s = ctx->priv;
963     float *srcl, *srcr, *srclfe;
964     int n;
965
966     srcl = (float *)s->input->extended_data[0];
967     srcr = (float *)s->input->extended_data[1];
968     srclfe = (float *)s->input->extended_data[2];
969
970     for (n = 0; n < s->buf_size; n++) {
971         float l_re = srcl[2 * n], r_re = srcr[2 * n];
972         float l_im = srcl[2 * n + 1], r_im = srcr[2 * n + 1];
973         float lfe_re = srclfe[2 * n], lfe_im = srclfe[2 * n + 1];
974         float c_phase = atan2f(l_im + r_im, l_re + r_re);
975         float l_mag = hypotf(l_re, l_im);
976         float r_mag = hypotf(r_re, r_im);
977         float l_phase = atan2f(l_im, l_re);
978         float r_phase = atan2f(r_im, r_re);
979         float phase_dif = fabsf(l_phase - r_phase);
980         float mag_dif = (l_mag - r_mag) / (l_mag + r_mag);
981         float mag_total = hypotf(l_mag, r_mag);
982         float x, y;
983
984         if (phase_dif > M_PI)
985             phase_dif = 2 * M_PI - phase_dif;
986
987         stereo_position(mag_dif, phase_dif, &x, &y);
988
989         s->upmix_2_1(ctx, l_phase, r_phase, c_phase, mag_total, lfe_re, lfe_im, x, y, n);
990     }
991 }
992
993 static void filter_5_0_side(AVFilterContext *ctx)
994 {
995     AudioSurroundContext *s = ctx->priv;
996     float *srcl, *srcr, *srcc, *srcsl, *srcsr;
997     int n;
998
999     srcl = (float *)s->input->extended_data[0];
1000     srcr = (float *)s->input->extended_data[1];
1001     srcc = (float *)s->input->extended_data[2];
1002     srcsl = (float *)s->input->extended_data[3];
1003     srcsr = (float *)s->input->extended_data[4];
1004
1005     for (n = 0; n < s->buf_size; n++) {
1006         float fl_re = srcl[2 * n], fr_re = srcr[2 * n];
1007         float fl_im = srcl[2 * n + 1], fr_im = srcr[2 * n + 1];
1008         float c_re = srcc[2 * n], c_im = srcc[2 * n + 1];
1009         float sl_re = srcsl[2 * n], sl_im = srcsl[2 * n + 1];
1010         float sr_re = srcsr[2 * n], sr_im = srcsr[2 * n + 1];
1011         float fl_mag = hypotf(fl_re, fl_im);
1012         float fr_mag = hypotf(fr_re, fr_im);
1013         float fl_phase = atan2f(fl_im, fl_re);
1014         float fr_phase = atan2f(fr_im, fr_re);
1015         float sl_mag = hypotf(sl_re, sl_im);
1016         float sr_mag = hypotf(sr_re, sr_im);
1017         float sl_phase = atan2f(sl_im, sl_re);
1018         float sr_phase = atan2f(sr_im, sr_re);
1019         float phase_difl = fabsf(fl_phase - sl_phase);
1020         float phase_difr = fabsf(fr_phase - sr_phase);
1021         float mag_difl = (fl_mag - sl_mag) / (fl_mag + sl_mag);
1022         float mag_difr = (fr_mag - sr_mag) / (fr_mag + sr_mag);
1023         float mag_totall = hypotf(fl_mag, sl_mag);
1024         float mag_totalr = hypotf(fr_mag, sr_mag);
1025         float bl_phase = atan2f(fl_im + sl_im, fl_re + sl_re);
1026         float br_phase = atan2f(fr_im + sr_im, fr_re + sr_re);
1027         float xl, yl;
1028         float xr, yr;
1029
1030         if (phase_difl > M_PI)
1031             phase_difl = 2 * M_PI - phase_difl;
1032
1033         if (phase_difr > M_PI)
1034             phase_difr = 2 * M_PI - phase_difr;
1035
1036         stereo_position(mag_difl, phase_difl, &xl, &yl);
1037         stereo_position(mag_difr, phase_difr, &xr, &yr);
1038
1039         s->upmix_5_0(ctx, c_re, c_im,
1040                      mag_totall, mag_totalr,
1041                      fl_phase, fr_phase,
1042                      bl_phase, br_phase,
1043                      sl_phase, sr_phase,
1044                      xl, yl, xr, yr, n);
1045     }
1046 }
1047
1048 static void filter_5_1_side(AVFilterContext *ctx)
1049 {
1050     AudioSurroundContext *s = ctx->priv;
1051     float *srcl, *srcr, *srcc, *srclfe, *srcsl, *srcsr;
1052     int n;
1053
1054     srcl = (float *)s->input->extended_data[0];
1055     srcr = (float *)s->input->extended_data[1];
1056     srcc = (float *)s->input->extended_data[2];
1057     srclfe = (float *)s->input->extended_data[3];
1058     srcsl = (float *)s->input->extended_data[4];
1059     srcsr = (float *)s->input->extended_data[5];
1060
1061     for (n = 0; n < s->buf_size; n++) {
1062         float fl_re = srcl[2 * n], fr_re = srcr[2 * n];
1063         float fl_im = srcl[2 * n + 1], fr_im = srcr[2 * n + 1];
1064         float c_re = srcc[2 * n], c_im = srcc[2 * n + 1];
1065         float lfe_re = srclfe[2 * n], lfe_im = srclfe[2 * n + 1];
1066         float sl_re = srcsl[2 * n], sl_im = srcsl[2 * n + 1];
1067         float sr_re = srcsr[2 * n], sr_im = srcsr[2 * n + 1];
1068         float fl_mag = hypotf(fl_re, fl_im);
1069         float fr_mag = hypotf(fr_re, fr_im);
1070         float fl_phase = atan2f(fl_im, fl_re);
1071         float fr_phase = atan2f(fr_im, fr_re);
1072         float sl_mag = hypotf(sl_re, sl_im);
1073         float sr_mag = hypotf(sr_re, sr_im);
1074         float sl_phase = atan2f(sl_im, sl_re);
1075         float sr_phase = atan2f(sr_im, sr_re);
1076         float phase_difl = fabsf(fl_phase - sl_phase);
1077         float phase_difr = fabsf(fr_phase - sr_phase);
1078         float mag_difl = (fl_mag - sl_mag) / (fl_mag + sl_mag);
1079         float mag_difr = (fr_mag - sr_mag) / (fr_mag + sr_mag);
1080         float mag_totall = hypotf(fl_mag, sl_mag);
1081         float mag_totalr = hypotf(fr_mag, sr_mag);
1082         float bl_phase = atan2f(fl_im + sl_im, fl_re + sl_re);
1083         float br_phase = atan2f(fr_im + sr_im, fr_re + sr_re);
1084         float xl, yl;
1085         float xr, yr;
1086
1087         if (phase_difl > M_PI)
1088             phase_difl = 2 * M_PI - phase_difl;
1089
1090         if (phase_difr > M_PI)
1091             phase_difr = 2 * M_PI - phase_difr;
1092
1093         stereo_position(mag_difl, phase_difl, &xl, &yl);
1094         stereo_position(mag_difr, phase_difr, &xr, &yr);
1095
1096         s->upmix_5_1(ctx, c_re, c_im, lfe_re, lfe_im,
1097                      mag_totall, mag_totalr,
1098                      fl_phase, fr_phase,
1099                      bl_phase, br_phase,
1100                      sl_phase, sr_phase,
1101                      xl, yl, xr, yr, n);
1102     }
1103 }
1104
1105 static void filter_5_1_back(AVFilterContext *ctx)
1106 {
1107     AudioSurroundContext *s = ctx->priv;
1108     float *srcl, *srcr, *srcc, *srclfe, *srcbl, *srcbr;
1109     int n;
1110
1111     srcl = (float *)s->input->extended_data[0];
1112     srcr = (float *)s->input->extended_data[1];
1113     srcc = (float *)s->input->extended_data[2];
1114     srclfe = (float *)s->input->extended_data[3];
1115     srcbl = (float *)s->input->extended_data[4];
1116     srcbr = (float *)s->input->extended_data[5];
1117
1118     for (n = 0; n < s->buf_size; n++) {
1119         float fl_re = srcl[2 * n], fr_re = srcr[2 * n];
1120         float fl_im = srcl[2 * n + 1], fr_im = srcr[2 * n + 1];
1121         float c_re = srcc[2 * n], c_im = srcc[2 * n + 1];
1122         float lfe_re = srclfe[2 * n], lfe_im = srclfe[2 * n + 1];
1123         float bl_re = srcbl[2 * n], bl_im = srcbl[2 * n + 1];
1124         float br_re = srcbr[2 * n], br_im = srcbr[2 * n + 1];
1125         float fl_mag = hypotf(fl_re, fl_im);
1126         float fr_mag = hypotf(fr_re, fr_im);
1127         float fl_phase = atan2f(fl_im, fl_re);
1128         float fr_phase = atan2f(fr_im, fr_re);
1129         float bl_mag = hypotf(bl_re, bl_im);
1130         float br_mag = hypotf(br_re, br_im);
1131         float bl_phase = atan2f(bl_im, bl_re);
1132         float br_phase = atan2f(br_im, br_re);
1133         float phase_difl = fabsf(fl_phase - bl_phase);
1134         float phase_difr = fabsf(fr_phase - br_phase);
1135         float mag_difl = (fl_mag - bl_mag) / (fl_mag + bl_mag);
1136         float mag_difr = (fr_mag - br_mag) / (fr_mag + br_mag);
1137         float mag_totall = hypotf(fl_mag, bl_mag);
1138         float mag_totalr = hypotf(fr_mag, br_mag);
1139         float sl_phase = atan2f(fl_im + bl_im, fl_re + bl_re);
1140         float sr_phase = atan2f(fr_im + br_im, fr_re + br_re);
1141         float xl, yl;
1142         float xr, yr;
1143
1144         if (phase_difl > M_PI)
1145             phase_difl = 2 * M_PI - phase_difl;
1146
1147         if (phase_difr > M_PI)
1148             phase_difr = 2 * M_PI - phase_difr;
1149
1150         stereo_position(mag_difl, phase_difl, &xl, &yl);
1151         stereo_position(mag_difr, phase_difr, &xr, &yr);
1152
1153         s->upmix_5_1(ctx, c_re, c_im, lfe_re, lfe_im,
1154                      mag_totall, mag_totalr,
1155                      fl_phase, fr_phase,
1156                      bl_phase, br_phase,
1157                      sl_phase, sr_phase,
1158                      xl, yl, xr, yr, n);
1159     }
1160 }
1161
1162 static int init(AVFilterContext *ctx)
1163 {
1164     AudioSurroundContext *s = ctx->priv;
1165     float overlap;
1166     int i;
1167
1168     if (!(s->out_channel_layout = av_get_channel_layout(s->out_channel_layout_str))) {
1169         av_log(ctx, AV_LOG_ERROR, "Error parsing output channel layout '%s'.\n",
1170                s->out_channel_layout_str);
1171         return AVERROR(EINVAL);
1172     }
1173
1174     if (!(s->in_channel_layout = av_get_channel_layout(s->in_channel_layout_str))) {
1175         av_log(ctx, AV_LOG_ERROR, "Error parsing input channel layout '%s'.\n",
1176                s->in_channel_layout_str);
1177         return AVERROR(EINVAL);
1178     }
1179
1180     if (s->lowcutf >= s->highcutf) {
1181         av_log(ctx, AV_LOG_ERROR, "Low cut-off '%d' should be less than high cut-off '%d'.\n",
1182                s->lowcutf, s->highcutf);
1183         return AVERROR(EINVAL);
1184     }
1185
1186     switch (s->in_channel_layout) {
1187     case AV_CH_LAYOUT_STEREO:
1188         s->filter = filter_stereo;
1189         switch (s->out_channel_layout) {
1190         case AV_CH_LAYOUT_MONO:
1191             s->upmix_stereo = upmix_1_0;
1192             break;
1193         case AV_CH_LAYOUT_STEREO:
1194             s->upmix_stereo = upmix_stereo;
1195             break;
1196         case AV_CH_LAYOUT_2POINT1:
1197             s->upmix_stereo = upmix_2_1;
1198             break;
1199         case AV_CH_LAYOUT_SURROUND:
1200             s->upmix_stereo = upmix_3_0;
1201             break;
1202         case AV_CH_LAYOUT_3POINT1:
1203             s->upmix_stereo = upmix_3_1;
1204             break;
1205         case AV_CH_LAYOUT_4POINT0:
1206             s->upmix_stereo = upmix_4_0;
1207             break;
1208         case AV_CH_LAYOUT_4POINT1:
1209             s->upmix_stereo = upmix_4_1;
1210             break;
1211         case AV_CH_LAYOUT_5POINT0_BACK:
1212             s->upmix_stereo = upmix_5_0_back;
1213             break;
1214         case AV_CH_LAYOUT_5POINT1_BACK:
1215             s->upmix_stereo = upmix_5_1_back;
1216             break;
1217         case AV_CH_LAYOUT_7POINT0:
1218             s->upmix_stereo = upmix_7_0;
1219             break;
1220         case AV_CH_LAYOUT_7POINT1:
1221             s->upmix_stereo = upmix_7_1;
1222             break;
1223         default:
1224             goto fail;
1225         }
1226         break;
1227     case AV_CH_LAYOUT_2POINT1:
1228         s->filter = filter_2_1;
1229         switch (s->out_channel_layout) {
1230         case AV_CH_LAYOUT_5POINT1_BACK:
1231             s->upmix_2_1 = upmix_5_1_back_2_1;
1232             break;
1233         default:
1234             goto fail;
1235         }
1236         break;
1237     case AV_CH_LAYOUT_SURROUND:
1238         s->filter = filter_surround;
1239         switch (s->out_channel_layout) {
1240         case AV_CH_LAYOUT_3POINT1:
1241             s->upmix_3_0 = upmix_3_1_surround;
1242             break;
1243         case AV_CH_LAYOUT_5POINT1_BACK:
1244             s->upmix_3_0 = upmix_5_1_back_surround;
1245             break;
1246         default:
1247             goto fail;
1248         }
1249         break;
1250     case AV_CH_LAYOUT_5POINT0:
1251         s->filter = filter_5_0_side;
1252         switch (s->out_channel_layout) {
1253         case AV_CH_LAYOUT_7POINT1:
1254             s->upmix_5_0 = upmix_7_1_5_0_side;
1255             break;
1256         default:
1257             goto fail;
1258         }
1259         break;
1260     case AV_CH_LAYOUT_5POINT1:
1261         s->filter = filter_5_1_side;
1262         switch (s->out_channel_layout) {
1263         case AV_CH_LAYOUT_7POINT1:
1264             s->upmix_5_1 = upmix_7_1_5_1;
1265             break;
1266         default:
1267             goto fail;
1268         }
1269         break;
1270     case AV_CH_LAYOUT_5POINT1_BACK:
1271         s->filter = filter_5_1_back;
1272         switch (s->out_channel_layout) {
1273         case AV_CH_LAYOUT_7POINT1:
1274             s->upmix_5_1 = upmix_7_1_5_1;
1275             break;
1276         default:
1277             goto fail;
1278         }
1279         break;
1280     default:
1281 fail:
1282         av_log(ctx, AV_LOG_ERROR, "Unsupported upmix: '%s' -> '%s'.\n",
1283                s->in_channel_layout_str, s->out_channel_layout_str);
1284         return AVERROR(EINVAL);
1285     }
1286
1287     s->buf_size = 4096;
1288     s->pts = AV_NOPTS_VALUE;
1289
1290     s->window_func_lut = av_calloc(s->buf_size, sizeof(*s->window_func_lut));
1291     if (!s->window_func_lut)
1292         return AVERROR(ENOMEM);
1293
1294     generate_window_func(s->window_func_lut, s->buf_size, s->win_func, &overlap);
1295     if (s->overlap == 1)
1296         s->overlap = overlap;
1297
1298     for (i = 0; i < s->buf_size; i++)
1299         s->window_func_lut[i] = sqrtf(s->window_func_lut[i] / s->buf_size);
1300     s->hop_size = s->buf_size * (1. - s->overlap);
1301     if (s->hop_size <= 0)
1302         return AVERROR(EINVAL);
1303
1304     return 0;
1305 }
1306
1307 static int fft_channel(AVFilterContext *ctx, void *arg, int ch, int nb_jobs)
1308 {
1309     AudioSurroundContext *s = ctx->priv;
1310     const float level_in = s->input_levels[ch];
1311     float *dst;
1312     int n;
1313
1314     memset(s->input->extended_data[ch] + s->buf_size * sizeof(float), 0, s->buf_size * sizeof(float));
1315
1316     dst = (float *)s->input->extended_data[ch];
1317     for (n = 0; n < s->buf_size; n++) {
1318         dst[n] *= s->window_func_lut[n] * level_in;
1319     }
1320
1321     av_rdft_calc(s->rdft[ch], (float *)s->input->extended_data[ch]);
1322
1323     return 0;
1324 }
1325
1326 static int ifft_channel(AVFilterContext *ctx, void *arg, int ch, int nb_jobs)
1327 {
1328     AudioSurroundContext *s = ctx->priv;
1329     const float level_out = s->output_levels[ch];
1330     AVFrame *out = arg;
1331     float *dst, *ptr;
1332     int n;
1333
1334     av_rdft_calc(s->irdft[ch], (float *)s->output->extended_data[ch]);
1335
1336     dst = (float *)s->output->extended_data[ch];
1337     ptr = (float *)s->overlap_buffer->extended_data[ch];
1338
1339     memmove(s->overlap_buffer->extended_data[ch],
1340             s->overlap_buffer->extended_data[ch] + s->hop_size * sizeof(float),
1341             s->buf_size * sizeof(float));
1342     memset(s->overlap_buffer->extended_data[ch] + s->buf_size * sizeof(float),
1343            0, s->hop_size * sizeof(float));
1344
1345     for (n = 0; n < s->buf_size; n++) {
1346         ptr[n] += dst[n] * s->window_func_lut[n] * level_out;
1347     }
1348
1349     ptr = (float *)s->overlap_buffer->extended_data[ch];
1350     dst = (float *)out->extended_data[ch];
1351     memcpy(dst, ptr, s->hop_size * sizeof(float));
1352
1353     return 0;
1354 }
1355
1356 static int filter_frame(AVFilterLink *inlink, AVFrame *in)
1357 {
1358     AVFilterContext *ctx = inlink->dst;
1359     AVFilterLink *outlink = ctx->outputs[0];
1360     AudioSurroundContext *s = ctx->priv;
1361     int ret;
1362
1363     ret = av_audio_fifo_write(s->fifo, (void **)in->extended_data,
1364                               in->nb_samples);
1365     if (ret >= 0 && s->pts == AV_NOPTS_VALUE)
1366         s->pts = in->pts;
1367
1368     av_frame_free(&in);
1369     if (ret < 0)
1370         return ret;
1371
1372     while (av_audio_fifo_size(s->fifo) >= s->buf_size) {
1373         AVFrame *out;
1374
1375         ret = av_audio_fifo_peek(s->fifo, (void **)s->input->extended_data, s->buf_size);
1376         if (ret < 0)
1377             return ret;
1378
1379         ctx->internal->execute(ctx, fft_channel, NULL, NULL, inlink->channels);
1380
1381         s->filter(ctx);
1382
1383         out = ff_get_audio_buffer(outlink, s->hop_size);
1384         if (!out)
1385             return AVERROR(ENOMEM);
1386
1387         ctx->internal->execute(ctx, ifft_channel, out, NULL, outlink->channels);
1388
1389         out->pts = s->pts;
1390         if (s->pts != AV_NOPTS_VALUE)
1391             s->pts += av_rescale_q(out->nb_samples, (AVRational){1, outlink->sample_rate}, outlink->time_base);
1392         av_audio_fifo_drain(s->fifo, s->hop_size);
1393         ret = ff_filter_frame(outlink, out);
1394         if (ret < 0)
1395             return ret;
1396     }
1397
1398     return 0;
1399 }
1400
1401 static int request_frame(AVFilterLink *outlink)
1402 {
1403     AVFilterContext *ctx = outlink->src;
1404     AudioSurroundContext *s = ctx->priv;
1405     int ret = 0;
1406
1407     ret = ff_request_frame(ctx->inputs[0]);
1408
1409     if (ret == AVERROR_EOF && av_audio_fifo_size(s->fifo) > 0 && av_audio_fifo_size(s->fifo) < s->buf_size) {
1410         AVFrame *in;
1411
1412         in = ff_get_audio_buffer(outlink, s->buf_size - av_audio_fifo_size(s->fifo));
1413         if (!in)
1414             return AVERROR(ENOMEM);
1415         ret = filter_frame(ctx->inputs[0], in);
1416         av_audio_fifo_drain(s->fifo, s->buf_size);
1417     }
1418
1419     return ret;
1420 }
1421
1422 static av_cold void uninit(AVFilterContext *ctx)
1423 {
1424     AudioSurroundContext *s = ctx->priv;
1425     int ch;
1426
1427     av_frame_free(&s->input);
1428     av_frame_free(&s->output);
1429     av_frame_free(&s->overlap_buffer);
1430
1431     for (ch = 0; ch < s->nb_in_channels; ch++) {
1432         av_rdft_end(s->rdft[ch]);
1433     }
1434     for (ch = 0; ch < s->nb_out_channels; ch++) {
1435         av_rdft_end(s->irdft[ch]);
1436     }
1437     av_freep(&s->input_levels);
1438     av_freep(&s->output_levels);
1439     av_freep(&s->rdft);
1440     av_freep(&s->irdft);
1441     av_audio_fifo_free(s->fifo);
1442     av_freep(&s->window_func_lut);
1443 }
1444
1445 #define OFFSET(x) offsetof(AudioSurroundContext, x)
1446 #define FLAGS AV_OPT_FLAG_AUDIO_PARAM|AV_OPT_FLAG_FILTERING_PARAM
1447
1448 static const AVOption surround_options[] = {
1449     { "chl_out",   "set output channel layout", OFFSET(out_channel_layout_str), AV_OPT_TYPE_STRING, {.str="5.1"}, 0,   0, FLAGS },
1450     { "chl_in",    "set input channel layout",  OFFSET(in_channel_layout_str),  AV_OPT_TYPE_STRING, {.str="stereo"},0, 0, FLAGS },
1451     { "level_in",  "set input level",           OFFSET(level_in),               AV_OPT_TYPE_FLOAT,  {.dbl=1},     0,  10, FLAGS },
1452     { "level_out", "set output level",          OFFSET(level_out),              AV_OPT_TYPE_FLOAT,  {.dbl=1},     0,  10, FLAGS },
1453     { "lfe",       "output LFE",                OFFSET(output_lfe),             AV_OPT_TYPE_BOOL,   {.i64=1},     0,   1, FLAGS },
1454     { "lfe_low",   "LFE low cut off",           OFFSET(lowcutf),                AV_OPT_TYPE_INT,    {.i64=128},   0, 256, FLAGS },
1455     { "lfe_high",  "LFE high cut off",          OFFSET(highcutf),               AV_OPT_TYPE_INT,    {.i64=256},   0, 512, FLAGS },
1456     { "fc_in",     "set front center channel input level",  OFFSET(fc_in),      AV_OPT_TYPE_FLOAT,  {.dbl=1},     0,  10, FLAGS },
1457     { "fc_out",    "set front center channel output level", OFFSET(fc_out),     AV_OPT_TYPE_FLOAT,  {.dbl=1},     0,  10, FLAGS },
1458     { "lfe_in",    "set lfe channel input level",  OFFSET(lfe_in),              AV_OPT_TYPE_FLOAT,  {.dbl=1},     0,  10, FLAGS },
1459     { "lfe_out",   "set lfe channel output level", OFFSET(lfe_out),             AV_OPT_TYPE_FLOAT,  {.dbl=1},     0,  10, FLAGS },
1460     { "win_func", "set window function", OFFSET(win_func), AV_OPT_TYPE_INT, {.i64 = WFUNC_HANNING}, 0, NB_WFUNC-1, FLAGS, "win_func" },
1461         { "rect",     "Rectangular",      0, AV_OPT_TYPE_CONST, {.i64=WFUNC_RECT},     0, 0, FLAGS, "win_func" },
1462         { "bartlett", "Bartlett",         0, AV_OPT_TYPE_CONST, {.i64=WFUNC_BARTLETT}, 0, 0, FLAGS, "win_func" },
1463         { "hann",     "Hann",             0, AV_OPT_TYPE_CONST, {.i64=WFUNC_HANNING},  0, 0, FLAGS, "win_func" },
1464         { "hanning",  "Hanning",          0, AV_OPT_TYPE_CONST, {.i64=WFUNC_HANNING},  0, 0, FLAGS, "win_func" },
1465         { "hamming",  "Hamming",          0, AV_OPT_TYPE_CONST, {.i64=WFUNC_HAMMING},  0, 0, FLAGS, "win_func" },
1466         { "blackman", "Blackman",         0, AV_OPT_TYPE_CONST, {.i64=WFUNC_BLACKMAN}, 0, 0, FLAGS, "win_func" },
1467         { "welch",    "Welch",            0, AV_OPT_TYPE_CONST, {.i64=WFUNC_WELCH},    0, 0, FLAGS, "win_func" },
1468         { "flattop",  "Flat-top",         0, AV_OPT_TYPE_CONST, {.i64=WFUNC_FLATTOP},  0, 0, FLAGS, "win_func" },
1469         { "bharris",  "Blackman-Harris",  0, AV_OPT_TYPE_CONST, {.i64=WFUNC_BHARRIS},  0, 0, FLAGS, "win_func" },
1470         { "bnuttall", "Blackman-Nuttall", 0, AV_OPT_TYPE_CONST, {.i64=WFUNC_BNUTTALL}, 0, 0, FLAGS, "win_func" },
1471         { "bhann",    "Bartlett-Hann",    0, AV_OPT_TYPE_CONST, {.i64=WFUNC_BHANN},    0, 0, FLAGS, "win_func" },
1472         { "sine",     "Sine",             0, AV_OPT_TYPE_CONST, {.i64=WFUNC_SINE},     0, 0, FLAGS, "win_func" },
1473         { "nuttall",  "Nuttall",          0, AV_OPT_TYPE_CONST, {.i64=WFUNC_NUTTALL},  0, 0, FLAGS, "win_func" },
1474         { "lanczos",  "Lanczos",          0, AV_OPT_TYPE_CONST, {.i64=WFUNC_LANCZOS},  0, 0, FLAGS, "win_func" },
1475         { "gauss",    "Gauss",            0, AV_OPT_TYPE_CONST, {.i64=WFUNC_GAUSS},    0, 0, FLAGS, "win_func" },
1476         { "tukey",    "Tukey",            0, AV_OPT_TYPE_CONST, {.i64=WFUNC_TUKEY},    0, 0, FLAGS, "win_func" },
1477         { "dolph",    "Dolph-Chebyshev",  0, AV_OPT_TYPE_CONST, {.i64=WFUNC_DOLPH},    0, 0, FLAGS, "win_func" },
1478         { "cauchy",   "Cauchy",           0, AV_OPT_TYPE_CONST, {.i64=WFUNC_CAUCHY},   0, 0, FLAGS, "win_func" },
1479         { "parzen",   "Parzen",           0, AV_OPT_TYPE_CONST, {.i64=WFUNC_PARZEN},   0, 0, FLAGS, "win_func" },
1480         { "poisson",  "Poisson",          0, AV_OPT_TYPE_CONST, {.i64=WFUNC_POISSON},  0, 0, FLAGS, "win_func" },
1481         { "bohman",   "Bohman",           0, AV_OPT_TYPE_CONST, {.i64=WFUNC_BOHMAN},   0, 0, FLAGS, "win_func" },
1482     { "overlap", "set window overlap", OFFSET(overlap), AV_OPT_TYPE_FLOAT, {.dbl=0.5}, 0, 1, FLAGS },
1483     { NULL }
1484 };
1485
1486 AVFILTER_DEFINE_CLASS(surround);
1487
1488 static const AVFilterPad inputs[] = {
1489     {
1490         .name         = "default",
1491         .type         = AVMEDIA_TYPE_AUDIO,
1492         .filter_frame = filter_frame,
1493         .config_props = config_input,
1494     },
1495     { NULL }
1496 };
1497
1498 static const AVFilterPad outputs[] = {
1499     {
1500         .name          = "default",
1501         .type          = AVMEDIA_TYPE_AUDIO,
1502         .request_frame = request_frame,
1503         .config_props  = config_output,
1504     },
1505     { NULL }
1506 };
1507
1508 AVFilter ff_af_surround = {
1509     .name           = "surround",
1510     .description    = NULL_IF_CONFIG_SMALL("Apply audio surround upmix filter."),
1511     .query_formats  = query_formats,
1512     .priv_size      = sizeof(AudioSurroundContext),
1513     .priv_class     = &surround_class,
1514     .init           = init,
1515     .uninit         = uninit,
1516     .inputs         = inputs,
1517     .outputs        = outputs,
1518     .flags          = AVFILTER_FLAG_SLICE_THREADS,
1519 };