git.sesse.net Git - ffmpeg/blob - 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 };