avfilter: add audio surround upmixer
[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     float level_in;
34     float level_out;
35     int output_lfe;
36     int lowcutf;
37     int highcutf;
38
39     float lowcut;
40     float highcut;
41
42     uint64_t out_channel_layout;
43     int nb_in_channels;
44     int nb_out_channels;
45
46     AVFrame *input;
47     AVFrame *output;
48     AVFrame *overlap_buffer;
49
50     int buf_size;
51     int hop_size;
52     AVAudioFifo *fifo;
53     RDFTContext **rdft, **irdft;
54     float *window_func_lut;
55
56     int64_t pts;
57
58     void (*upmix)(AVFilterContext *ctx,
59                   float l_phase,
60                   float r_phase,
61                   float c_phase,
62                   float mag_total,
63                   float x, float y,
64                   int n);
65 } AudioSurroundContext;
66
67 static int query_formats(AVFilterContext *ctx)
68 {
69     AudioSurroundContext *s = ctx->priv;
70     AVFilterFormats *formats = NULL;
71     AVFilterChannelLayouts *layouts = NULL;
72     int ret;
73
74     ret = ff_add_format(&formats, AV_SAMPLE_FMT_FLTP);
75     if (ret)
76         return ret;
77     ret = ff_set_common_formats(ctx, formats);
78     if (ret)
79         return ret;
80
81     layouts = NULL;
82     ret = ff_add_channel_layout(&layouts, s->out_channel_layout);
83     if (ret)
84         return ret;
85
86     ret = ff_channel_layouts_ref(layouts, &ctx->outputs[0]->in_channel_layouts);
87     if (ret)
88         return ret;
89
90     layouts = NULL;
91     ret = ff_add_channel_layout(&layouts, AV_CH_LAYOUT_STEREO);
92     if (ret)
93         return ret;
94
95     ret = ff_channel_layouts_ref(layouts, &ctx->inputs[0]->out_channel_layouts);
96     if (ret)
97         return ret;
98
99     formats = ff_all_samplerates();
100     if (!formats)
101         return AVERROR(ENOMEM);
102     return ff_set_common_samplerates(ctx, formats);
103 }
104
105 static int config_input(AVFilterLink *inlink)
106 {
107     AVFilterContext *ctx = inlink->dst;
108     AudioSurroundContext *s = ctx->priv;
109     int ch;
110
111     s->rdft = av_calloc(inlink->channels, sizeof(*s->rdft));
112     if (!s->rdft)
113         return AVERROR(ENOMEM);
114
115     for (ch = 0; ch < inlink->channels; ch++) {
116         s->rdft[ch]  = av_rdft_init(ff_log2(s->buf_size), DFT_R2C);
117         if (!s->rdft[ch])
118             return AVERROR(ENOMEM);
119     }
120     s->nb_in_channels = inlink->channels;
121
122     s->input = ff_get_audio_buffer(inlink, s->buf_size * 2);
123     if (!s->input)
124         return AVERROR(ENOMEM);
125
126     s->fifo = av_audio_fifo_alloc(inlink->format, inlink->channels, s->buf_size);
127     if (!s->fifo)
128         return AVERROR(ENOMEM);
129
130     s->lowcut = 1.f * s->lowcutf / (inlink->sample_rate * 0.5) * (s->buf_size / 2);
131     s->highcut = 1.f * s->highcutf / (inlink->sample_rate * 0.5) * (s->buf_size / 2);
132
133     return 0;
134 }
135
136 static int config_output(AVFilterLink *outlink)
137 {
138     AVFilterContext *ctx = outlink->src;
139     AudioSurroundContext *s = ctx->priv;
140     int ch;
141
142     s->irdft = av_calloc(outlink->channels, sizeof(*s->irdft));
143     if (!s->irdft)
144         return AVERROR(ENOMEM);
145
146     for (ch = 0; ch < outlink->channels; ch++) {
147         s->irdft[ch] = av_rdft_init(ff_log2(s->buf_size), IDFT_C2R);
148         if (!s->irdft[ch])
149             return AVERROR(ENOMEM);
150     }
151     s->nb_out_channels = outlink->channels;
152
153     s->output = ff_get_audio_buffer(outlink, s->buf_size * 2);
154     s->overlap_buffer = ff_get_audio_buffer(outlink, s->buf_size * 2);
155     if (!s->overlap_buffer || !s->output)
156         return AVERROR(ENOMEM);
157
158     return 0;
159 }
160
161 static void stereo_position(float a, float p, float *x, float *y)
162 {
163       *x = av_clipf(a+FFMAX(0, sinf(p-M_PI_2))*FFDIFFSIGN(a,0), -1, 1);
164       *y = av_clipf(cosf(a*M_PI_2+M_PI)*cosf(M_PI_2-p/M_PI)*M_LN10+1, -1, 1);
165 }
166
167 static inline void get_lfe(int output_lfe, int n, float lowcut, float highcut,
168                            float *lfe_mag, float *mag_total)
169 {
170     if (output_lfe && n < highcut) {
171         *lfe_mag    = n < lowcut ? 1.f : .5f*(1.f+cosf(M_PI*(lowcut-n)/(lowcut-highcut)));
172         *lfe_mag   *= *mag_total;
173         *mag_total -= *lfe_mag;
174     } else {
175         *lfe_mag = 0.f;
176     }
177 }
178
179 static void upmix_1_0(AVFilterContext *ctx,
180                       float l_phase,
181                       float r_phase,
182                       float c_phase,
183                       float mag_total,
184                       float x, float y,
185                       int n)
186 {
187     AudioSurroundContext *s = ctx->priv;
188     float mag, *dst;
189
190     dst = (float *)s->output->extended_data[0];
191
192     mag = sqrtf(1.f - fabsf(x)) * ((y + 1.f) * .5f) * mag_total;
193
194     dst[2 * n    ] = mag * cosf(c_phase);
195     dst[2 * n + 1] = mag * sinf(c_phase);
196 }
197
198 static void upmix_stereo(AVFilterContext *ctx,
199                          float l_phase,
200                          float r_phase,
201                          float c_phase,
202                          float mag_total,
203                          float x, float y,
204                          int n)
205 {
206     AudioSurroundContext *s = ctx->priv;
207     float l_mag, r_mag, *dstl, *dstr;
208
209     dstl = (float *)s->output->extended_data[0];
210     dstr = (float *)s->output->extended_data[1];
211
212     l_mag = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
213     r_mag = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
214
215     dstl[2 * n    ] = l_mag * cosf(l_phase);
216     dstl[2 * n + 1] = l_mag * sinf(l_phase);
217
218     dstr[2 * n    ] = r_mag * cosf(r_phase);
219     dstr[2 * n + 1] = r_mag * sinf(r_phase);
220 }
221
222 static void upmix_2_1(AVFilterContext *ctx,
223                       float l_phase,
224                       float r_phase,
225                       float c_phase,
226                       float mag_total,
227                       float x, float y,
228                       int n)
229 {
230     AudioSurroundContext *s = ctx->priv;
231     float lfe_mag, l_mag, r_mag, *dstl, *dstr, *dstlfe;
232
233     dstl = (float *)s->output->extended_data[0];
234     dstr = (float *)s->output->extended_data[1];
235     dstlfe = (float *)s->output->extended_data[2];
236
237     get_lfe(s->output_lfe, n, s->lowcut, s->highcut, &lfe_mag, &mag_total);
238
239     l_mag = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
240     r_mag = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
241
242     dstl[2 * n    ] = l_mag * cosf(l_phase);
243     dstl[2 * n + 1] = l_mag * sinf(l_phase);
244
245     dstr[2 * n    ] = r_mag * cosf(r_phase);
246     dstr[2 * n + 1] = r_mag * sinf(r_phase);
247
248     dstlfe[2 * n    ] = lfe_mag * cosf(c_phase);
249     dstlfe[2 * n + 1] = lfe_mag * sinf(c_phase);
250 }
251
252 static void upmix_3_0(AVFilterContext *ctx,
253                       float l_phase,
254                       float r_phase,
255                       float c_phase,
256                       float mag_total,
257                       float x, float y,
258                       int n)
259 {
260     AudioSurroundContext *s = ctx->priv;
261     float l_mag, r_mag, c_mag, *dstc, *dstl, *dstr;
262
263     dstl = (float *)s->output->extended_data[0];
264     dstr = (float *)s->output->extended_data[1];
265     dstc = (float *)s->output->extended_data[2];
266
267     c_mag = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
268     l_mag = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
269     r_mag = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
270
271     dstl[2 * n    ] = l_mag * cosf(l_phase);
272     dstl[2 * n + 1] = l_mag * sinf(l_phase);
273
274     dstr[2 * n    ] = r_mag * cosf(r_phase);
275     dstr[2 * n + 1] = r_mag * sinf(r_phase);
276
277     dstc[2 * n    ] = c_mag * cosf(c_phase);
278     dstc[2 * n + 1] = c_mag * sinf(c_phase);
279 }
280
281 static void upmix_3_1(AVFilterContext *ctx,
282                       float l_phase,
283                       float r_phase,
284                       float c_phase,
285                       float mag_total,
286                       float x, float y,
287                       int n)
288 {
289     AudioSurroundContext *s = ctx->priv;
290     float lfe_mag, l_mag, r_mag, c_mag, *dstc, *dstl, *dstr, *dstlfe;
291
292     dstl = (float *)s->output->extended_data[0];
293     dstr = (float *)s->output->extended_data[1];
294     dstc = (float *)s->output->extended_data[2];
295     dstlfe = (float *)s->output->extended_data[3];
296
297     get_lfe(s->output_lfe, n, s->lowcut, s->highcut, &lfe_mag, &mag_total);
298
299     c_mag = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
300     l_mag = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
301     r_mag = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
302
303     dstl[2 * n    ] = l_mag * cosf(l_phase);
304     dstl[2 * n + 1] = l_mag * sinf(l_phase);
305
306     dstr[2 * n    ] = r_mag * cosf(r_phase);
307     dstr[2 * n + 1] = r_mag * sinf(r_phase);
308
309     dstc[2 * n    ] = c_mag * cosf(c_phase);
310     dstc[2 * n + 1] = c_mag * sinf(c_phase);
311
312     dstlfe[2 * n    ] = lfe_mag * cosf(c_phase);
313     dstlfe[2 * n + 1] = lfe_mag * sinf(c_phase);
314 }
315
316 static void upmix_4_0(AVFilterContext *ctx,
317                       float l_phase,
318                       float r_phase,
319                       float c_phase,
320                       float mag_total,
321                       float x, float y,
322                       int n)
323 {
324     float b_mag, l_mag, r_mag, c_mag, *dstc, *dstl, *dstr, *dstb;
325     AudioSurroundContext *s = ctx->priv;
326
327     dstl = (float *)s->output->extended_data[0];
328     dstr = (float *)s->output->extended_data[1];
329     dstc = (float *)s->output->extended_data[2];
330     dstb = (float *)s->output->extended_data[3];
331
332     c_mag = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
333     b_mag = sqrtf(1.f - fabsf(x))   * ((1.f - y) * .5f) * mag_total;
334     l_mag = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
335     r_mag = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
336
337     dstl[2 * n    ] = l_mag * cosf(l_phase);
338     dstl[2 * n + 1] = l_mag * sinf(l_phase);
339
340     dstr[2 * n    ] = r_mag * cosf(r_phase);
341     dstr[2 * n + 1] = r_mag * sinf(r_phase);
342
343     dstc[2 * n    ] = c_mag * cosf(c_phase);
344     dstc[2 * n + 1] = c_mag * sinf(c_phase);
345
346     dstb[2 * n    ] = b_mag * cosf(c_phase);
347     dstb[2 * n + 1] = b_mag * sinf(c_phase);
348 }
349
350 static void upmix_4_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     float lfe_mag, b_mag, l_mag, r_mag, c_mag, *dstc, *dstl, *dstr, *dstb, *dstlfe;
359     AudioSurroundContext *s = ctx->priv;
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     dstb = (float *)s->output->extended_data[4];
366
367     get_lfe(s->output_lfe, n, s->lowcut, s->highcut, &lfe_mag, &mag_total);
368
369     dstlfe[2 * n    ] = lfe_mag * cosf(c_phase);
370     dstlfe[2 * n + 1] = lfe_mag * sinf(c_phase);
371
372     c_mag = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
373     b_mag = sqrtf(1.f - fabsf(x))   * ((1.f - y) * .5f) * mag_total;
374     l_mag = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
375     r_mag = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
376
377     dstl[2 * n    ] = l_mag * cosf(l_phase);
378     dstl[2 * n + 1] = l_mag * sinf(l_phase);
379
380     dstr[2 * n    ] = r_mag * cosf(r_phase);
381     dstr[2 * n + 1] = r_mag * sinf(r_phase);
382
383     dstc[2 * n    ] = c_mag * cosf(c_phase);
384     dstc[2 * n + 1] = c_mag * sinf(c_phase);
385
386     dstb[2 * n    ] = b_mag * cosf(c_phase);
387     dstb[2 * n + 1] = b_mag * sinf(c_phase);
388 }
389
390 static void upmix_5_0_back(AVFilterContext *ctx,
391                            float l_phase,
392                            float r_phase,
393                            float c_phase,
394                            float mag_total,
395                            float x, float y,
396                            int n)
397 {
398     float l_mag, r_mag, ls_mag, rs_mag, c_mag, *dstc, *dstl, *dstr, *dstls, *dstrs;
399     AudioSurroundContext *s = ctx->priv;
400
401     dstl  = (float *)s->output->extended_data[0];
402     dstr  = (float *)s->output->extended_data[1];
403     dstc  = (float *)s->output->extended_data[2];
404     dstls = (float *)s->output->extended_data[3];
405     dstrs = (float *)s->output->extended_data[4];
406
407     c_mag  = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
408     l_mag  = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
409     r_mag  = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
410     ls_mag = sqrtf(.5f * ( x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
411     rs_mag = sqrtf(.5f * (-x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
412
413     dstl[2 * n    ] = l_mag * cosf(l_phase);
414     dstl[2 * n + 1] = l_mag * sinf(l_phase);
415
416     dstr[2 * n    ] = r_mag * cosf(r_phase);
417     dstr[2 * n + 1] = r_mag * sinf(r_phase);
418
419     dstc[2 * n    ] = c_mag * cosf(c_phase);
420     dstc[2 * n + 1] = c_mag * sinf(c_phase);
421
422     dstls[2 * n    ] = ls_mag * cosf(l_phase);
423     dstls[2 * n + 1] = ls_mag * sinf(l_phase);
424
425     dstrs[2 * n    ] = rs_mag * cosf(r_phase);
426     dstrs[2 * n + 1] = rs_mag * sinf(r_phase);
427 }
428
429 static void upmix_5_1_back(AVFilterContext *ctx,
430                            float l_phase,
431                            float r_phase,
432                            float c_phase,
433                            float mag_total,
434                            float x, float y,
435                            int n)
436 {
437     float lfe_mag, l_mag, r_mag, ls_mag, rs_mag, c_mag, *dstc, *dstl, *dstr, *dstls, *dstrs, *dstlfe;
438     AudioSurroundContext *s = ctx->priv;
439
440     dstl  = (float *)s->output->extended_data[0];
441     dstr  = (float *)s->output->extended_data[1];
442     dstc  = (float *)s->output->extended_data[2];
443     dstlfe = (float *)s->output->extended_data[3];
444     dstls = (float *)s->output->extended_data[4];
445     dstrs = (float *)s->output->extended_data[5];
446
447     get_lfe(s->output_lfe, n, s->lowcut, s->highcut, &lfe_mag, &mag_total);
448
449     c_mag  = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
450     l_mag  = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
451     r_mag  = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
452     ls_mag = sqrtf(.5f * ( x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
453     rs_mag = sqrtf(.5f * (-x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
454
455     dstl[2 * n    ] = l_mag * cosf(l_phase);
456     dstl[2 * n + 1] = l_mag * sinf(l_phase);
457
458     dstr[2 * n    ] = r_mag * cosf(r_phase);
459     dstr[2 * n + 1] = r_mag * sinf(r_phase);
460
461     dstc[2 * n    ] = c_mag * cosf(c_phase);
462     dstc[2 * n + 1] = c_mag * sinf(c_phase);
463
464     dstlfe[2 * n    ] = lfe_mag * cosf(c_phase);
465     dstlfe[2 * n + 1] = lfe_mag * sinf(c_phase);
466
467     dstls[2 * n    ] = ls_mag * cosf(l_phase);
468     dstls[2 * n + 1] = ls_mag * sinf(l_phase);
469
470     dstrs[2 * n    ] = rs_mag * cosf(r_phase);
471     dstrs[2 * n + 1] = rs_mag * sinf(r_phase);
472 }
473
474 static void upmix_7_0(AVFilterContext *ctx,
475                       float l_phase,
476                       float r_phase,
477                       float c_phase,
478                       float mag_total,
479                       float x, float y,
480                       int n)
481 {
482     float l_mag, r_mag, ls_mag, rs_mag, c_mag, lb_mag, rb_mag;
483     float *dstc, *dstl, *dstr, *dstls, *dstrs, *dstlb, *dstrb;
484     AudioSurroundContext *s = ctx->priv;
485
486     dstl  = (float *)s->output->extended_data[0];
487     dstr  = (float *)s->output->extended_data[1];
488     dstc  = (float *)s->output->extended_data[2];
489     dstlb = (float *)s->output->extended_data[3];
490     dstrb = (float *)s->output->extended_data[4];
491     dstls = (float *)s->output->extended_data[5];
492     dstrs = (float *)s->output->extended_data[6];
493
494     c_mag  = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
495     l_mag  = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
496     r_mag  = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
497     lb_mag = sqrtf(.5f * ( x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
498     rb_mag = sqrtf(.5f * (-x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
499     ls_mag = sqrtf(.5f * ( x + 1.f)) * (1.f - fabsf(y)) * mag_total;
500     rs_mag = sqrtf(.5f * (-x + 1.f)) * (1.f - fabsf(y)) * mag_total;
501
502     dstl[2 * n    ] = l_mag * cosf(l_phase);
503     dstl[2 * n + 1] = l_mag * sinf(l_phase);
504
505     dstr[2 * n    ] = r_mag * cosf(r_phase);
506     dstr[2 * n + 1] = r_mag * sinf(r_phase);
507
508     dstc[2 * n    ] = c_mag * cosf(c_phase);
509     dstc[2 * n + 1] = c_mag * sinf(c_phase);
510
511     dstlb[2 * n    ] = lb_mag * cosf(l_phase);
512     dstlb[2 * n + 1] = lb_mag * sinf(l_phase);
513
514     dstrb[2 * n    ] = rb_mag * cosf(r_phase);
515     dstrb[2 * n + 1] = rb_mag * sinf(r_phase);
516
517     dstls[2 * n    ] = ls_mag * cosf(l_phase);
518     dstls[2 * n + 1] = ls_mag * sinf(l_phase);
519
520     dstrs[2 * n    ] = rs_mag * cosf(r_phase);
521     dstrs[2 * n + 1] = rs_mag * sinf(r_phase);
522 }
523
524 static void upmix_7_1(AVFilterContext *ctx,
525                       float l_phase,
526                       float r_phase,
527                       float c_phase,
528                       float mag_total,
529                       float x, float y,
530                       int n)
531 {
532     float lfe_mag, l_mag, r_mag, ls_mag, rs_mag, c_mag, lb_mag, rb_mag;
533     float *dstc, *dstl, *dstr, *dstls, *dstrs, *dstlb, *dstrb, *dstlfe;
534     AudioSurroundContext *s = ctx->priv;
535
536     dstl  = (float *)s->output->extended_data[0];
537     dstr  = (float *)s->output->extended_data[1];
538     dstc  = (float *)s->output->extended_data[2];
539     dstlfe = (float *)s->output->extended_data[3];
540     dstlb = (float *)s->output->extended_data[4];
541     dstrb = (float *)s->output->extended_data[5];
542     dstls = (float *)s->output->extended_data[6];
543     dstrs = (float *)s->output->extended_data[7];
544
545     get_lfe(s->output_lfe, n, s->lowcut, s->highcut, &lfe_mag, &mag_total);
546
547     c_mag  = sqrtf(1.f - fabsf(x))   * ((y + 1.f) * .5f) * mag_total;
548     l_mag  = sqrtf(.5f * ( x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
549     r_mag  = sqrtf(.5f * (-x + 1.f)) * ((y + 1.f) * .5f) * mag_total;
550     lb_mag = sqrtf(.5f * ( x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
551     rb_mag = sqrtf(.5f * (-x + 1.f)) * (1.f - ((y + 1.f) * .5f)) * mag_total;
552     ls_mag = sqrtf(.5f * ( x + 1.f)) * (1.f - fabsf(y)) * mag_total;
553     rs_mag = sqrtf(.5f * (-x + 1.f)) * (1.f - fabsf(y)) * mag_total;
554
555     dstl[2 * n    ] = l_mag * cosf(l_phase);
556     dstl[2 * n + 1] = l_mag * sinf(l_phase);
557
558     dstr[2 * n    ] = r_mag * cosf(r_phase);
559     dstr[2 * n + 1] = r_mag * sinf(r_phase);
560
561     dstc[2 * n    ] = c_mag * cosf(c_phase);
562     dstc[2 * n + 1] = c_mag * sinf(c_phase);
563
564     dstlfe[2 * n    ] = lfe_mag * cosf(c_phase);
565     dstlfe[2 * n + 1] = lfe_mag * sinf(c_phase);
566
567     dstlb[2 * n    ] = lb_mag * cosf(l_phase);
568     dstlb[2 * n + 1] = lb_mag * sinf(l_phase);
569
570     dstrb[2 * n    ] = rb_mag * cosf(r_phase);
571     dstrb[2 * n + 1] = rb_mag * sinf(r_phase);
572
573     dstls[2 * n    ] = ls_mag * cosf(l_phase);
574     dstls[2 * n + 1] = ls_mag * sinf(l_phase);
575
576     dstrs[2 * n    ] = rs_mag * cosf(r_phase);
577     dstrs[2 * n + 1] = rs_mag * sinf(r_phase);
578 }
579
580 static int init(AVFilterContext *ctx)
581 {
582     AudioSurroundContext *s = ctx->priv;
583     float overlap;
584     int i;
585
586     if (!(s->out_channel_layout = av_get_channel_layout(s->out_channel_layout_str))) {
587         av_log(ctx, AV_LOG_ERROR, "Error parsing channel layout '%s'.\n",
588                s->out_channel_layout_str);
589         return AVERROR(EINVAL);
590     }
591
592     if (s->lowcutf >= s->highcutf) {
593         av_log(ctx, AV_LOG_ERROR, "Low cut-off '%d' should be less than high cut-off '%d'.\n",
594                s->lowcutf, s->highcutf);
595         return AVERROR(EINVAL);
596     }
597
598     switch (s->out_channel_layout) {
599     case AV_CH_LAYOUT_MONO:
600         s->upmix = upmix_1_0;
601         break;
602     case AV_CH_LAYOUT_STEREO:
603         s->upmix = upmix_stereo;
604         break;
605     case AV_CH_LAYOUT_2POINT1:
606         s->upmix = upmix_2_1;
607         break;
608     case AV_CH_LAYOUT_SURROUND:
609         s->upmix = upmix_3_0;
610         break;
611     case AV_CH_LAYOUT_3POINT1:
612         s->upmix = upmix_3_1;
613         break;
614     case AV_CH_LAYOUT_4POINT0:
615         s->upmix = upmix_4_0;
616         break;
617     case AV_CH_LAYOUT_4POINT1:
618         s->upmix = upmix_4_1;
619         break;
620     case AV_CH_LAYOUT_5POINT0_BACK:
621         s->upmix = upmix_5_0_back;
622         break;
623     case AV_CH_LAYOUT_5POINT1_BACK:
624         s->upmix = upmix_5_1_back;
625         break;
626     case AV_CH_LAYOUT_7POINT0:
627         s->upmix = upmix_7_0;
628         break;
629     case AV_CH_LAYOUT_7POINT1:
630         s->upmix = upmix_7_1;
631         break;
632     default:
633         av_log(ctx, AV_LOG_ERROR, "Unsupported output channel layout '%s'.\n",
634                s->out_channel_layout_str);
635         return AVERROR(EINVAL);
636     }
637
638     s->buf_size = 4096;
639     s->pts = AV_NOPTS_VALUE;
640
641     s->window_func_lut = av_calloc(s->buf_size, sizeof(*s->window_func_lut));
642     if (!s->window_func_lut)
643         return AVERROR(ENOMEM);
644
645     for (i = 0; i < s->buf_size; i++)
646         s->window_func_lut[i] = sqrtf(0.5 * (1 - cosf(2 * M_PI * i / s->buf_size)) / s->buf_size);
647     overlap = .5;
648     s->hop_size = s->buf_size * (1. - overlap);
649
650     return 0;
651 }
652
653 static int fft_channel(AVFilterContext *ctx, void *arg, int ch, int nb_jobs)
654 {
655     AudioSurroundContext *s = ctx->priv;
656     const float level_in = s->level_in;
657     float *dst;
658     int n;
659
660     memset(s->input->extended_data[ch] + s->buf_size * sizeof(float), 0, s->buf_size * sizeof(float));
661
662     dst = (float *)s->input->extended_data[ch];
663     for (n = 0; n < s->buf_size; n++) {
664         dst[n] *= s->window_func_lut[n] * level_in;
665     }
666
667     av_rdft_calc(s->rdft[ch], (float *)s->input->extended_data[ch]);
668
669     return 0;
670 }
671
672 static int ifft_channel(AVFilterContext *ctx, void *arg, int ch, int nb_jobs)
673 {
674     AudioSurroundContext *s = ctx->priv;
675     const float level_out = s->level_out;
676     AVFrame *out = arg;
677     float *dst, *ptr;
678     int n;
679
680     av_rdft_calc(s->irdft[ch], (float *)s->output->extended_data[ch]);
681
682     dst = (float *)s->output->extended_data[ch];
683     ptr = (float *)s->overlap_buffer->extended_data[ch];
684
685     memmove(s->overlap_buffer->extended_data[ch],
686             s->overlap_buffer->extended_data[ch] + s->hop_size * sizeof(float),
687             s->buf_size * sizeof(float));
688     memset(s->overlap_buffer->extended_data[ch] + s->buf_size * sizeof(float),
689            0, s->hop_size * sizeof(float));
690
691     for (n = 0; n < s->buf_size; n++) {
692         ptr[n] += dst[n] * s->window_func_lut[n] * level_out;
693     }
694
695     ptr = (float *)s->overlap_buffer->extended_data[ch];
696     dst = (float *)out->extended_data[ch];
697     memcpy(dst, ptr, s->hop_size * sizeof(float));
698
699     return 0;
700 }
701
702 static int filter_frame(AVFilterLink *inlink, AVFrame *in)
703 {
704     AVFilterContext *ctx = inlink->dst;
705     AVFilterLink *outlink = ctx->outputs[0];
706     AudioSurroundContext *s = ctx->priv;
707
708     av_audio_fifo_write(s->fifo, (void **)in->extended_data,
709                         in->nb_samples);
710
711     if (s->pts == AV_NOPTS_VALUE)
712         s->pts = in->pts;
713
714     av_frame_free(&in);
715
716     while (av_audio_fifo_size(s->fifo) >= s->buf_size) {
717         float *srcl, *srcr;
718         AVFrame *out;
719         int n, ret;
720
721         ret = av_audio_fifo_peek(s->fifo, (void **)s->input->extended_data, s->buf_size);
722         if (ret < 0)
723             return ret;
724
725         ctx->internal->execute(ctx, fft_channel, NULL, NULL, inlink->channels);
726
727         srcl = (float *)s->input->extended_data[0];
728         srcr = (float *)s->input->extended_data[1];
729
730         for (n = 0; n < s->buf_size; n++) {
731             float l_re = srcl[2 * n], r_re = srcr[2 * n];
732             float l_im = srcl[2 * n + 1], r_im = srcr[2 * n + 1];
733             float c_phase = atan2f(l_im + r_im, l_re + r_re);
734             float l_mag = hypotf(l_re, l_im);
735             float r_mag = hypotf(r_re, r_im);
736             float l_phase = atan2f(l_im, l_re);
737             float r_phase = atan2f(r_im, r_re);
738             float phase_dif = fabsf(l_phase - r_phase);
739             float mag_dif = (l_mag - r_mag) / (l_mag + r_mag);
740             float mag_total = hypotf(l_mag, r_mag);
741             float x, y;
742
743             if (phase_dif > M_PI)
744                 phase_dif = 2 * M_PI - phase_dif;
745
746             stereo_position(mag_dif, phase_dif, &x, &y);
747
748             s->upmix(ctx, l_phase, r_phase, c_phase, mag_total, x, y, n);
749         }
750
751         out = ff_get_audio_buffer(outlink, s->hop_size);
752         if (!out)
753             return AVERROR(ENOMEM);
754
755         ctx->internal->execute(ctx, ifft_channel, out, NULL, outlink->channels);
756
757         out->pts = s->pts;
758         if (s->pts != AV_NOPTS_VALUE)
759             s->pts += av_rescale_q(out->nb_samples, (AVRational){1, outlink->sample_rate}, outlink->time_base);
760         av_audio_fifo_drain(s->fifo, s->hop_size);
761         ret = ff_filter_frame(outlink, out);
762         if (ret < 0)
763             return ret;
764     }
765
766     return 0;
767 }
768
769 static av_cold void uninit(AVFilterContext *ctx)
770 {
771     AudioSurroundContext *s = ctx->priv;
772     int ch;
773
774     av_frame_free(&s->input);
775     av_frame_free(&s->output);
776     av_frame_free(&s->overlap_buffer);
777
778     for (ch = 0; ch < s->nb_in_channels; ch++) {
779         av_rdft_end(s->rdft[ch]);
780     }
781     for (ch = 0; ch < s->nb_out_channels; ch++) {
782         av_rdft_end(s->irdft[ch]);
783     }
784     av_freep(&s->rdft);
785     av_freep(&s->irdft);
786     av_audio_fifo_free(s->fifo);
787     av_freep(&s->window_func_lut);
788 }
789
790 #define OFFSET(x) offsetof(AudioSurroundContext, x)
791 #define FLAGS AV_OPT_FLAG_AUDIO_PARAM|AV_OPT_FLAG_FILTERING_PARAM
792
793 static const AVOption surround_options[] = {
794     { "chl_out",   "set output channel layout", OFFSET(out_channel_layout_str), AV_OPT_TYPE_STRING, {.str="5.1"}, 0,   0, FLAGS },
795     { "level_in",  "set input level",           OFFSET(level_in),               AV_OPT_TYPE_FLOAT,  {.dbl=1},     0,  10, FLAGS },
796     { "level_out", "set output level",          OFFSET(level_out),              AV_OPT_TYPE_FLOAT,  {.dbl=1},     0,  10, FLAGS },
797     { "lfe",       "output LFE",                OFFSET(output_lfe),             AV_OPT_TYPE_BOOL,   {.i64=1},     0,   1, FLAGS },
798     { "lfe_low",   "LFE low cut off",           OFFSET(lowcutf),                AV_OPT_TYPE_INT,    {.i64=128},   0, 256, FLAGS },
799     { "lfe_high",  "LFE high cut off",          OFFSET(highcutf),               AV_OPT_TYPE_INT,    {.i64=256},   0, 512, FLAGS },
800     { NULL }
801 };
802
803 AVFILTER_DEFINE_CLASS(surround);
804
805 static const AVFilterPad inputs[] = {
806     {
807         .name         = "default",
808         .type         = AVMEDIA_TYPE_AUDIO,
809         .filter_frame = filter_frame,
810         .config_props = config_input,
811     },
812     { NULL }
813 };
814
815 static const AVFilterPad outputs[] = {
816     {
817         .name         = "default",
818         .type         = AVMEDIA_TYPE_AUDIO,
819         .config_props = config_output,
820     },
821     { NULL }
822 };
823
824 AVFilter ff_af_surround = {
825     .name           = "surround",
826     .description    = NULL_IF_CONFIG_SMALL("Apply audio surround upmix filter."),
827     .query_formats  = query_formats,
828     .priv_size      = sizeof(AudioSurroundContext),
829     .priv_class     = &surround_class,
830     .init           = init,
831     .uninit         = uninit,
832     .inputs         = inputs,
833     .outputs        = outputs,
834     .flags          = AVFILTER_FLAG_SLICE_THREADS,
835 };