libaf/af_hrtf.c

   1 /* Experimental audio filter that mixes 5.1 and 5.1 with matrix
   2    encoded rear channels into headphone signal using FIR filtering
   3    with HRTF.
   4 */
   5 //#include <stdio.h>
   6 #include <stdlib.h>
   7 #include <string.h>
   8 #include <inttypes.h>
   9
  10 #include <math.h>
  11
  12 #include "af.h"
  13 #include "dsp.h"
  14
  15 /* HRTF filter coefficients and adjustable parameters */
  16 #include "af_hrtf.h"
  17
  18 typedef struct af_hrtf_s {
  19     /* Lengths */
  20     int dlbuflen, hrflen, basslen;
  21     /* L, C, R, Ls, Rs channels */
  22     float *lf, *rf, *lr, *rr, *cf, *cr;
  23     float *cf_ir, *af_ir, *of_ir, *ar_ir, *or_ir, *cr_ir;
  24     int cf_o, af_o, of_o, ar_o, or_o, cr_o;
  25     /* Bass */
  26     float *ba_l, *ba_r;
  27     float *ba_ir;
  28     /* Whether to matrix decode the rear center channel */
  29     int matrix_mode;
  30     /* How to decode the input:
  31        0 = 5/5+1 channels
  32        1 = 2 channels
  33        2 = matrix encoded 2 channels */
  34     int decode_mode;
  35     /* Full wave rectified (FWR) amplitudes and gain used to steer the
  36        active matrix decoding of front channels (variable names
  37        lpr/lmr means Lt + Rt, Lt - Rt) */
  38     float l_fwr, r_fwr, lpr_fwr, lmr_fwr;
  39     float adapt_l_gain, adapt_r_gain, adapt_lpr_gain, adapt_lmr_gain;
  40     /* Matrix input decoding require special FWR buffer, since the
  41        decoding is done in place. */
  42     float *fwrbuf_l, *fwrbuf_r, *fwrbuf_lr, *fwrbuf_rr;
  43     /* Rear channel delay buffer for matrix decoding */
  44     float *rear_dlbuf;
  45     /* Full wave rectified amplitude and gain used to steer the active
  46        matrix decoding of center rear channel */
  47     float lr_fwr, rr_fwr, lrprr_fwr, lrmrr_fwr;
  48     float adapt_lr_gain, adapt_rr_gain;
  49     float adapt_lrprr_gain, adapt_lrmrr_gain;
  50     /* Cyclic position on the ring buffer */
  51     int cyc_pos;
  52     int print_flag;
  53 } af_hrtf_t;
  54
  55 /* Convolution on a ring buffer
  56  *    nx:       length of the ring buffer
  57  *    nk:       length of the convolution kernel
  58  *    sx:       ring buffer
  59  *    sk:       convolution kernel
  60  *    offset:   offset on the ring buffer, can be
  61  */
  62 static float conv(const int nx, const int nk, float *sx, float *sk,
  63                   const int offset)
  64 {
  65     /* k = reminder of offset / nx */
  66     int k = offset >= 0 ? offset % nx : nx + (offset % nx);
  67
  68     if(nk + k <= nx)
  69         return af_filter_fir(nk, sx + k, sk);
  70     else
  71         return af_filter_fir(nk + k - nx, sx, sk + nx - k) +
  72             af_filter_fir(nx - k, sx + k, sk);
  73 }
  74
  75 /* Detect when the impulse response starts (significantly) */
  76 static int pulse_detect(float *sx)
  77 {
  78     /* nmax must be the reference impulse response length (128) minus
  79        s->hrflen */
  80     const int nmax = 128 - HRTFFILTLEN;
  81     const float thresh = IRTHRESH;
  82     int i;
  83
  84     for(i = 0; i < nmax; i++)
  85         if(fabs(sx[i]) > thresh)
  86             return i;
  87     return 0;
  88 }
  89
  90 /* Fuzzy matrix coefficient transfer function to "lock" the matrix on
  91    a effectively passive mode if the gain is approximately 1 */
  92 static inline float passive_lock(float x)
  93 {
  94    const float x1 = x - 1;
  95    const float ax1s = fabs(x - 1) * (1.0 / MATAGCLOCK);
  96
  97    return x1 - x1 / (1 + ax1s * ax1s) + 1;
  98 }
  99
 100 /* Unified active matrix decoder for 2 channel matrix encoded surround
 101    sources */
 102 static inline void matrix_decode(short *in, const int k, const int il,
 103                           const int ir, const int decode_rear,
 104                           const int dlbuflen,
 105                           float l_fwr, float r_fwr,
 106                           float lpr_fwr, float lmr_fwr,
 107                           float *adapt_l_gain, float *adapt_r_gain,
 108                           float *adapt_lpr_gain, float *adapt_lmr_gain,
 109                           float *lf, float *rf, float *lr,
 110                           float *rr, float *cf)
 111 {
 112    const int kr = (k + MATREARDELAY) % dlbuflen;
 113    float l_gain = (l_fwr + r_fwr) /
 114       (1 + l_fwr + l_fwr);
 115    float r_gain = (l_fwr + r_fwr) /
 116       (1 + r_fwr + r_fwr);
 117    /* The 2nd axis has strong gain fluctuations, and therefore require
 118       limits.  The factor corresponds to the 1 / amplification of (Lt
 119       - Rt) when (Lt, Rt) is strongly correlated. (e.g. during
 120       dialogues).  It should be bigger than -12 dB to prevent
 121       distortion. */
 122    float lmr_lim_fwr = lmr_fwr > M9_03DB * lpr_fwr ?
 123       lmr_fwr : M9_03DB * lpr_fwr;
 124    float lpr_gain = (lpr_fwr + lmr_lim_fwr) /
 125       (1 + lpr_fwr + lpr_fwr);
 126    float lmr_gain = (lpr_fwr + lmr_lim_fwr) /
 127       (1 + lmr_lim_fwr + lmr_lim_fwr);
 128    float lmr_unlim_gain = (lpr_fwr + lmr_fwr) /
 129       (1 + lmr_fwr + lmr_fwr);
 130    float lpr, lmr;
 131    float l_agc, r_agc, lpr_agc, lmr_agc;
 132    float f, d_gain, c_gain, c_agc_cfk;
 133
 134 #if 0
 135    static int counter = 0;
 136    static FILE *fp_out;
 137
 138    if(counter == 0)
 139       fp_out = fopen("af_hrtf.log", "w");
 140    if(counter % 240 == 0)
 141       fprintf(fp_out, "%g %g %g %g %g ", counter * (1.0 / 48000),
 142               l_gain, r_gain, lpr_gain, lmr_gain);
 143 #endif
 144
 145    /*** AXIS NO. 1: (Lt, Rt) -> (C, Ls, Rs) ***/
 146    /* AGC adaption */
 147    d_gain = (fabs(l_gain - *adapt_l_gain) +
 148              fabs(r_gain - *adapt_r_gain)) * 0.5;
 149    f = d_gain * (1.0 / MATAGCTRIG);
 150    f = MATAGCDECAY - MATAGCDECAY / (1 + f * f);
 151    *adapt_l_gain = (1 - f) * *adapt_l_gain + f * l_gain;
 152    *adapt_r_gain = (1 - f) * *adapt_r_gain + f * r_gain;
 153    /* Matrix */
 154    l_agc = in[il] * passive_lock(*adapt_l_gain);
 155    r_agc = in[ir] * passive_lock(*adapt_r_gain);
 156    cf[k] = (l_agc + r_agc) * M_SQRT1_2;
 157    if(decode_rear) {
 158       lr[kr] = rr[kr] = (l_agc - r_agc) * M_SQRT1_2;
 159       /* Stereo rear channel is steered with the same AGC steering as
 160          the decoding matrix. Note this requires a fast updating AGC
 161          at the order of 20 ms (which is the case here). */
 162       lr[kr] *= (l_fwr + l_fwr) /
 163          (1 + l_fwr + r_fwr);
 164       rr[kr] *= (r_fwr + r_fwr) /
 165          (1 + l_fwr + r_fwr);
 166    }
 167
 168    /*** AXIS NO. 2: (Lt + Rt, Lt - Rt) -> (L, R) ***/
 169    lpr = (in[il] + in[ir]) * M_SQRT1_2;
 170    lmr = (in[il] - in[ir]) * M_SQRT1_2;
 171    /* AGC adaption */
 172    d_gain = fabs(lmr_unlim_gain - *adapt_lmr_gain);
 173    f = d_gain * (1.0 / MATAGCTRIG);
 174    f = MATAGCDECAY - MATAGCDECAY / (1 + f * f);
 175    *adapt_lpr_gain = (1 - f) * *adapt_lpr_gain + f * lpr_gain;
 176    *adapt_lmr_gain = (1 - f) * *adapt_lmr_gain + f * lmr_gain;
 177    /* Matrix */
 178    lpr_agc = lpr * passive_lock(*adapt_lpr_gain);
 179    lmr_agc = lmr * passive_lock(*adapt_lmr_gain);
 180    lf[k] = (lpr_agc + lmr_agc) * M_SQRT1_2;
 181    rf[k] = (lpr_agc - lmr_agc) * M_SQRT1_2;
 182
 183    /*** CENTER FRONT CANCELLATION ***/
 184    /* A heuristic approach exploits that Lt + Rt gain contains the
 185       information about Lt, Rt correlation.  This effectively reshapes
 186       the front and rear "cones" to concentrate Lt + Rt to C and
 187       introduce Lt - Rt in L, R. */
 188    /* 0.67677 is the emprical lower bound for lpr_gain. */
 189    c_gain = 8 * (*adapt_lpr_gain - 0.67677);
 190    c_gain = c_gain > 0 ? c_gain : 0;
 191    /* c_gain should not be too high, not even reaching full
 192       cancellation (~ 0.50 - 0.55 at current AGC implementation), or
 193       the center will s0und too narrow. */
 194    c_gain = MATCOMPGAIN / (1 + c_gain * c_gain);
 195    c_agc_cfk = c_gain * cf[k];
 196    lf[k] -= c_agc_cfk;
 197    rf[k] -= c_agc_cfk;
 198    cf[k] += c_agc_cfk + c_agc_cfk;
 199 #if 0
 200    if(counter % 240 == 0)
 201       fprintf(fp_out, "%g %g %g %g %g\n",
 202               *adapt_l_gain, *adapt_r_gain,
 203               *adapt_lpr_gain, *adapt_lmr_gain,
 204               c_gain);
 205    counter++;
 206 #endif
 207 }
 208
 209 static inline void update_ch(af_hrtf_t *s, short *in, const int k)
 210 {
 211     const int fwr_pos = (k + FWRDURATION) % s->dlbuflen;
 212     /* Update the full wave rectified total amplitude */
 213     /* Input matrix decoder */
 214     if(s->decode_mode == HRTF_MIX_MATRIX2CH) {
 215        s->l_fwr += abs(in[0]) - fabs(s->fwrbuf_l[fwr_pos]);
 216        s->r_fwr += abs(in[1]) - fabs(s->fwrbuf_r[fwr_pos]);
 217        s->lpr_fwr += abs(in[0] + in[1]) -
 218           fabs(s->fwrbuf_l[fwr_pos] + s->fwrbuf_r[fwr_pos]);
 219        s->lmr_fwr += abs(in[0] - in[1]) -
 220           fabs(s->fwrbuf_l[fwr_pos] - s->fwrbuf_r[fwr_pos]);
 221     }
 222     /* Rear matrix decoder */
 223     if(s->matrix_mode) {
 224        s->lr_fwr += abs(in[2]) - fabs(s->fwrbuf_lr[fwr_pos]);
 225        s->rr_fwr += abs(in[3]) - fabs(s->fwrbuf_rr[fwr_pos]);
 226        s->lrprr_fwr += abs(in[2] + in[3]) -
 227           fabs(s->fwrbuf_lr[fwr_pos] + s->fwrbuf_rr[fwr_pos]);
 228        s->lrmrr_fwr += abs(in[2] - in[3]) -
 229           fabs(s->fwrbuf_lr[fwr_pos] - s->fwrbuf_rr[fwr_pos]);
 230     }
 231
 232     switch (s->decode_mode) {
 233     case HRTF_MIX_51:
 234        /* 5/5+1 channel sources */
 235        s->lf[k] = in[0];
 236        s->cf[k] = in[4];
 237        s->rf[k] = in[1];
 238        s->fwrbuf_lr[k] = s->lr[k] = in[2];
 239        s->fwrbuf_rr[k] = s->rr[k] = in[3];
 240        break;
 241     case HRTF_MIX_MATRIX2CH:
 242        /* Matrix encoded 2 channel sources */
 243        s->fwrbuf_l[k] = in[0];
 244        s->fwrbuf_r[k] = in[1];
 245        matrix_decode(in, k, 0, 1, 1, s->dlbuflen,
 246                      s->l_fwr, s->r_fwr,
 247                      s->lpr_fwr, s->lmr_fwr,
 248                      &(s->adapt_l_gain), &(s->adapt_r_gain),
 249                      &(s->adapt_lpr_gain), &(s->adapt_lmr_gain),
 250                      s->lf, s->rf, s->lr, s->rr, s->cf);
 251        break;
 252     case HRTF_MIX_STEREO:
 253        /* Stereo sources */
 254        s->lf[k] = in[0];
 255        s->rf[k] = in[1];
 256        s->cf[k] = s->lr[k] = s->rr[k] = 0;
 257        break;
 258     }
 259
 260     /* We need to update the bass compensation delay line, too. */
 261     s->ba_l[k] = in[0] + in[4] + in[2];
 262     s->ba_r[k] = in[4] + in[1] + in[3];
 263 }
 264
 265 /* Initialization and runtime control */
 266 static int control(struct af_instance_s *af, int cmd, void* arg)
 267 {
 268     af_hrtf_t *s = af->setup;
 269     int test_output_res;
 270     char mode;
 271
 272     switch(cmd) {
 273     case AF_CONTROL_REINIT:
 274         af->data->rate   = ((af_data_t*)arg)->rate;
 275         if(af->data->rate != 48000) {
 276             // automatic samplerate adjustment in the filter chain
 277             // is not yet supported.
 278             af_msg(AF_MSG_ERROR,
 279                    "[hrtf] ERROR: Sampling rate is not 48000 Hz (%d)!\n",
 280                    af->data->rate);
 281             return AF_ERROR;
 282         }
 283         af->data->nch    = ((af_data_t*)arg)->nch;
 284             if(af->data->nch == 2) {
 285                /* 2 channel input */
 286                if(s->decode_mode != HRTF_MIX_MATRIX2CH) {
 287                   /* Default behavior is stereo mixing. */
 288                   s->decode_mode = HRTF_MIX_STEREO;
 289                }
 290             }
 291             else if (af->data->nch < 5)
 292               af->data->nch = 5;
 293         af->data->format = AF_FORMAT_S16_NE;
 294         af->data->bps    = 2;
 295         test_output_res = af_test_output(af, (af_data_t*)arg);
 296         af->mul = 2.0 / af->data->nch;
 297         // after testing input set the real output format
 298         af->data->nch = 2;
 299         s->print_flag = 1;
 300         return test_output_res;
 301     case AF_CONTROL_COMMAND_LINE:
 302         sscanf((char*)arg, "%c", &mode);
 303         switch(mode) {
 304         case 'm':
 305             /* Use matrix rear decoding. */
 306             s->matrix_mode = 1;
 307             break;
 308         case 's':
 309             /* Input needs matrix decoding. */
 310             s->decode_mode = HRTF_MIX_MATRIX2CH;
 311             break;
 312         case '0':
 313             s->matrix_mode = 0;
 314             break;
 315         default:
 316             af_msg(AF_MSG_ERROR,
 317                    "[hrtf] Mode is neither 'm', 's', nor '0' (%c).\n",
 318                    mode);
 319             return AF_ERROR;
 320         }
 321         s->print_flag = 1;
 322         return AF_OK;
 323     }
 324
 325     return AF_UNKNOWN;
 326 }
 327
 328 /* Deallocate memory */
 329 static void uninit(struct af_instance_s *af)
 330 {
 331     if(af->setup) {
 332         af_hrtf_t *s = af->setup;
 333
 334         if(s->lf)
 335             free(s->lf);
 336         if(s->rf)
 337             free(s->rf);
 338         if(s->lr)
 339             free(s->lr);
 340         if(s->rr)
 341             free(s->rr);
 342         if(s->cf)
 343             free(s->cf);
 344         if(s->cr)
 345             free(s->cr);
 346         if(s->ba_l)
 347             free(s->ba_l);
 348         if(s->ba_r)
 349             free(s->ba_r);
 350         if(s->ba_ir)
 351             free(s->ba_ir);
 352         if(s->fwrbuf_l)
 353            free(s->fwrbuf_l);
 354         if(s->fwrbuf_r)
 355            free(s->fwrbuf_r);
 356         if(s->fwrbuf_lr)
 357            free(s->fwrbuf_lr);
 358         if(s->fwrbuf_rr)
 359            free(s->fwrbuf_rr);
 360         free(af->setup);
 361     }
 362     if(af->data)
 363         free(af->data->audio);
 364     free(af->data);
 365 }
 366
 367 /* Filter data through filter
 368
 369 Two "tricks" are used to compensate the "color" of the KEMAR data:
 370
 371 1. The KEMAR data is refiltered to ensure that the front L, R channels
 372 on the same side of the ear are equalized (especially in the high
 373 frequencies).
 374
 375 2. A bass compensation is introduced to ensure that 0-200 Hz are not
 376 damped (without any real 3D acoustical image, however).
 377 */
 378 static af_data_t* play(struct af_instance_s *af, af_data_t *data)
 379 {
 380     af_hrtf_t *s = af->setup;
 381     short *in = data->audio; // Input audio data
 382     short *out = NULL; // Output audio data
 383     short *end = in + data->len / sizeof(short); // Loop end
 384     float common, left, right, diff, left_b, right_b;
 385     const int dblen = s->dlbuflen, hlen = s->hrflen, blen = s->basslen;
 386
 387     if(AF_OK != RESIZE_LOCAL_BUFFER(af, data))
 388         return NULL;
 389
 390     if(s->print_flag) {
 391         s->print_flag = 0;
 392         switch (s->decode_mode) {
 393         case HRTF_MIX_51:
 394           af_msg(AF_MSG_INFO,
 395                  "[hrtf] Using HRTF to mix %s discrete surround into "
 396                  "L, R channels\n", s->matrix_mode ? "5+1" : "5");
 397           break;
 398         case HRTF_MIX_STEREO:
 399           af_msg(AF_MSG_INFO,
 400                  "[hrtf] Using HRTF to mix stereo into "
 401                  "L, R channels\n");
 402           break;
 403         case HRTF_MIX_MATRIX2CH:
 404           af_msg(AF_MSG_INFO,
 405                  "[hrtf] Using active matrix to decode 2 channel "
 406                  "input, HRTF to mix %s matrix surround into "
 407                  "L, R channels\n", "3/2");
 408           break;
 409         default:
 410           af_msg(AF_MSG_WARN,
 411                  "[hrtf] bogus decode_mode: %d\n", s->decode_mode);
 412           break;
 413         }
 414
 415        if(s->matrix_mode)
 416           af_msg(AF_MSG_INFO,
 417                  "[hrtf] Using active matrix to decode rear center "
 418                  "channel\n");
 419     }
 420
 421     out = af->data->audio;
 422
 423     /* MPlayer's 5 channel layout (notation for the variable):
 424      *
 425      * 0: L (LF), 1: R (RF), 2: Ls (LR), 3: Rs (RR), 4: C (CF), matrix
 426      * encoded: Cs (CR)
 427      *
 428      * or: L = left, C = center, R = right, F = front, R = rear
 429      *
 430      * Filter notation:
 431      *
 432      *      CF
 433      * OF        AF
 434      *      Ear->
 435      * OR        AR
 436      *      CR
 437      *
 438      * or: C = center, A = same side, O = opposite, F = front, R = rear
 439      */
 440
 441     while(in < end) {
 442         const int k = s->cyc_pos;
 443
 444         update_ch(s, in, k);
 445
 446         /* Simulate a 7.5 ms -20 dB echo of the center channel in the
 447            front channels (like reflection from a room wall) - a kind of
 448            psycho-acoustically "cheating" to focus the center front
 449            channel, which is normally hard to be perceived as front */
 450         s->lf[k] += CFECHOAMPL * s->cf[(k + CFECHODELAY) % s->dlbuflen];
 451         s->rf[k] += CFECHOAMPL * s->cf[(k + CFECHODELAY) % s->dlbuflen];
 452
 453         switch (s->decode_mode) {
 454         case HRTF_MIX_51:
 455         case HRTF_MIX_MATRIX2CH:
 456            /* Mixer filter matrix */
 457            common = conv(dblen, hlen, s->cf, s->cf_ir, k + s->cf_o);
 458            if(s->matrix_mode) {
 459               /* In matrix decoding mode, the rear channel gain must be
 460                  renormalized, as there is an additional channel. */
 461               matrix_decode(in, k, 2, 3, 0, s->dlbuflen,
 462                             s->lr_fwr, s->rr_fwr,
 463                             s->lrprr_fwr, s->lrmrr_fwr,
 464                             &(s->adapt_lr_gain), &(s->adapt_rr_gain),
 465                             &(s->adapt_lrprr_gain), &(s->adapt_lrmrr_gain),
 466                             s->lr, s->rr, NULL, NULL, s->cr);
 467               common +=
 468                  conv(dblen, hlen, s->cr, s->cr_ir, k + s->cr_o) *
 469                  M1_76DB;
 470               left    =
 471                  ( conv(dblen, hlen, s->lf, s->af_ir, k + s->af_o) +
 472                    conv(dblen, hlen, s->rf, s->of_ir, k + s->of_o) +
 473                    (conv(dblen, hlen, s->lr, s->ar_ir, k + s->ar_o) +
 474                     conv(dblen, hlen, s->rr, s->or_ir, k + s->or_o)) *
 475                    M1_76DB + common);
 476               right   =
 477                  ( conv(dblen, hlen, s->rf, s->af_ir, k + s->af_o) +
 478                    conv(dblen, hlen, s->lf, s->of_ir, k + s->of_o) +
 479                    (conv(dblen, hlen, s->rr, s->ar_ir, k + s->ar_o) +
 480                     conv(dblen, hlen, s->lr, s->or_ir, k + s->or_o)) *
 481                    M1_76DB + common);
 482            } else {
 483               left    =
 484                  ( conv(dblen, hlen, s->lf, s->af_ir, k + s->af_o) +
 485                    conv(dblen, hlen, s->rf, s->of_ir, k + s->of_o) +
 486                    conv(dblen, hlen, s->lr, s->ar_ir, k + s->ar_o) +
 487                    conv(dblen, hlen, s->rr, s->or_ir, k + s->or_o) +
 488                    common);
 489               right   =
 490                  ( conv(dblen, hlen, s->rf, s->af_ir, k + s->af_o) +
 491                    conv(dblen, hlen, s->lf, s->of_ir, k + s->of_o) +
 492                    conv(dblen, hlen, s->rr, s->ar_ir, k + s->ar_o) +
 493                    conv(dblen, hlen, s->lr, s->or_ir, k + s->or_o) +
 494                    common);
 495            }
 496            break;
 497         case HRTF_MIX_STEREO:
 498            left    =
 499               ( conv(dblen, hlen, s->lf, s->af_ir, k + s->af_o) +
 500                 conv(dblen, hlen, s->rf, s->of_ir, k + s->of_o));
 501            right   =
 502               ( conv(dblen, hlen, s->rf, s->af_ir, k + s->af_o) +
 503                 conv(dblen, hlen, s->lf, s->of_ir, k + s->of_o));
 504            break;
 505         default:
 506             /* make gcc happy */
 507             left = 0.0;
 508             right = 0.0;
 509             break;
 510         }
 511
 512         /* Bass compensation for the lower frequency cut of the HRTF.  A
 513            cross talk of the left and right channel is introduced to
 514            match the directional characteristics of higher frequencies.
 515            The bass will not have any real 3D perception, but that is
 516            OK (note at 180 Hz, the wavelength is about 2 m, and any
 517            spatial perception is impossible). */
 518         left_b  = conv(dblen, blen, s->ba_l, s->ba_ir, k);
 519         right_b = conv(dblen, blen, s->ba_r, s->ba_ir, k);
 520         left  += (1 - BASSCROSS) * left_b  + BASSCROSS * right_b;
 521         right += (1 - BASSCROSS) * right_b + BASSCROSS * left_b;
 522         /* Also mix the LFE channel (if available) */
 523         if(data->nch >= 6) {
 524             left  += in[5] * M3_01DB;
 525             right += in[5] * M3_01DB;
 526         }
 527
 528         /* Amplitude renormalization. */
 529         left  *= AMPLNORM;
 530         right *= AMPLNORM;
 531
 532         switch (s->decode_mode) {
 533         case HRTF_MIX_51:
 534         case HRTF_MIX_STEREO:
 535            /* "Cheating": linear stereo expansion to amplify the 3D
 536               perception.  Note: Too much will destroy the acoustic space
 537               and may even result in headaches. */
 538            diff = STEXPAND2 * (left - right);
 539            out[0] = (int16_t)(left  + diff);
 540            out[1] = (int16_t)(right - diff);
 541            break;
 542         case HRTF_MIX_MATRIX2CH:
 543            /* Do attempt any stereo expansion with matrix encoded
 544               sources.  The L, R channels are already stereo expanded
 545               by the steering, any further stereo expansion will sound
 546               very unnatural. */
 547            out[0] = (int16_t)left;
 548            out[1] = (int16_t)right;
 549            break;
 550         }
 551
 552         /* Next sample... */
 553         in = &in[data->nch];
 554         out = &out[af->data->nch];
 555         (s->cyc_pos)--;
 556         if(s->cyc_pos < 0)
 557             s->cyc_pos += dblen;
 558     }
 559
 560     /* Set output data */
 561     data->audio = af->data->audio;
 562     data->len   = data->len / data->nch * 2;
 563     data->nch   = 2;
 564
 565     return data;
 566 }
 567
 568 static int allocate(af_hrtf_t *s)
 569 {
 570     if ((s->lf = malloc(s->dlbuflen * sizeof(float))) == NULL) return -1;
 571     if ((s->rf = malloc(s->dlbuflen * sizeof(float))) == NULL) return -1;
 572     if ((s->lr = malloc(s->dlbuflen * sizeof(float))) == NULL) return -1;
 573     if ((s->rr = malloc(s->dlbuflen * sizeof(float))) == NULL) return -1;
 574     if ((s->cf = malloc(s->dlbuflen * sizeof(float))) == NULL) return -1;
 575     if ((s->cr = malloc(s->dlbuflen * sizeof(float))) == NULL) return -1;
 576     if ((s->ba_l = malloc(s->dlbuflen * sizeof(float))) == NULL) return -1;
 577     if ((s->ba_r = malloc(s->dlbuflen * sizeof(float))) == NULL) return -1;
 578     if ((s->fwrbuf_l =
 579          malloc(s->dlbuflen * sizeof(float))) == NULL) return -1;
 580     if ((s->fwrbuf_r =
 581          malloc(s->dlbuflen * sizeof(float))) == NULL) return -1;
 582     if ((s->fwrbuf_lr =
 583          malloc(s->dlbuflen * sizeof(float))) == NULL) return -1;
 584     if ((s->fwrbuf_rr =
 585          malloc(s->dlbuflen * sizeof(float))) == NULL) return -1;
 586     return 0;
 587 }
 588
 589 /* Allocate memory and set function pointers */
 590 static int af_open(af_instance_t* af)
 591 {
 592     int i;
 593     af_hrtf_t *s;
 594     float fc;
 595
 596     af->control = control;
 597     af->uninit = uninit;
 598     af->play = play;
 599     af->mul = 1;
 600     af->data = calloc(1, sizeof(af_data_t));
 601     af->setup = calloc(1, sizeof(af_hrtf_t));
 602     if((af->data == NULL) || (af->setup == NULL))
 603         return AF_ERROR;
 604
 605     s = af->setup;
 606
 607     s->dlbuflen = DELAYBUFLEN;
 608     s->hrflen = HRTFFILTLEN;
 609     s->basslen = BASSFILTLEN;
 610
 611     s->cyc_pos = s->dlbuflen - 1;
 612     /* With a full (two axis) steering matrix decoder, s->matrix_mode
 613        should not be enabled lightly (it will also steer the Ls, Rs
 614        channels). */
 615     s->matrix_mode = 0;
 616     s->decode_mode = HRTF_MIX_51;
 617
 618     s->print_flag = 1;
 619
 620     if (allocate(s) != 0) {
 621         af_msg(AF_MSG_ERROR, "[hrtf] Memory allocation error.\n");
 622         return AF_ERROR;
 623     }
 624
 625     for(i = 0; i < s->dlbuflen; i++)
 626         s->lf[i] = s->rf[i] = s->lr[i] = s->rr[i] = s->cf[i] =
 627             s->cr[i] = 0;
 628
 629     s->lr_fwr =
 630         s->rr_fwr = 0;
 631
 632     s->cf_ir = cf_filt + (s->cf_o = pulse_detect(cf_filt));
 633     s->af_ir = af_filt + (s->af_o = pulse_detect(af_filt));
 634     s->of_ir = of_filt + (s->of_o = pulse_detect(of_filt));
 635     s->ar_ir = ar_filt + (s->ar_o = pulse_detect(ar_filt));
 636     s->or_ir = or_filt + (s->or_o = pulse_detect(or_filt));
 637     s->cr_ir = cr_filt + (s->cr_o = pulse_detect(cr_filt));
 638
 639     if((s->ba_ir = malloc(s->basslen * sizeof(float))) == NULL) {
 640         af_msg(AF_MSG_ERROR, "[hrtf] Memory allocation error.\n");
 641         return AF_ERROR;
 642     }
 643     fc = 2.0 * BASSFILTFREQ / (float)af->data->rate;
 644     if(af_filter_design_fir(s->basslen, s->ba_ir, &fc, LP | KAISER, 4 * M_PI) ==
 645        -1) {
 646         af_msg(AF_MSG_ERROR, "[hrtf] Unable to design low-pass "
 647                "filter.\n");
 648         return AF_ERROR;
 649     }
 650     for(i = 0; i < s->basslen; i++)
 651         s->ba_ir[i] *= BASSGAIN;
 652
 653     return AF_OK;
 654 }
 655
 656 /* Description of this filter */
 657 af_info_t af_info_hrtf = {
 658     "HRTF Headphone",
 659     "hrtf",
 660     "ylai",
 661     "",
 662     AF_FLAGS_REENTRANT,
 663     af_open
 664 };