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