summaryrefslogtreecommitdiffstats
path: root/lib/rbcodec/dsp/crossfeed.c
blob: 0428f58d8d32ae932171bc77be8e4c8406fc386e (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
/***************************************************************************
 *             __________               __   ___.
 *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
 *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
 *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
 *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
 *                     \/            \/     \/    \/            \/
 * $Id$
 *
 * Copyright (C) 2006 Thom Johansen
 * Copyright (C) 2010 Bertrik Sikken
 * Copyright (C) 2012 Michael Sevakis
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
 * KIND, either express or implied.
 *
 ****************************************************************************/
#include "rbcodecconfig.h"
#include "fixedpoint.h"
#include "fracmul.h"
#include "replaygain.h"
#include "dsp_misc.h"
#include "dsp_proc_entry.h"
#include "dsp_filter.h"
#include "crossfeed.h"
#include <string.h>

/* Implemented here or in target assembly code */
void crossfeed_process(struct dsp_proc_entry *this,
                       struct dsp_buffer **buf_p);
void crossfeed_meier_process(struct dsp_proc_entry *this,
                             struct dsp_buffer **buf_p);


/**
 * Applies crossfeed to the stereo signal.
 *
 * Crossfeed is a process where listening over speakers is simulated. This
 * is good for old hard panned stereo records, which might be quite fatiguing
 * to listen to on headphones with no crossfeed.
 */

#define DELAY_LEN(fs)   ((300*(fs) / 1000000)*2) /* ~300 uS */

/* Crossfeed */
static struct crossfeed_state
{
    union
    {
        struct /* Data for meier crossfeed */
        {
            int32_t reserved;   /* 00h: Reserved: overlaps gain */
            int32_t vcl;        /* 04h: Left filter output */
            int32_t vcr;        /* 08h: Right filter output */
            int32_t vdiff;      /* 0ch: L-R difference signal */
            int32_t coef1;      /* 10h: Left/right filter coef */
            int32_t coef2;      /* 14h: Crossfeed filter coef */
        };
        struct /* Data for custom crossfeed */
        {
            int32_t gain;       /* 00h: Direct path gain */
            int32_t coefs[3];   /* 04h: Filter coefficients: b0, b1, a1 */
            int32_t history[4]; /* 10h: Format is x[n - 1], y[n - 1] (L + R) */
            int32_t *index;     /* 20h: Current pointer into the delay line */
            int32_t *index_max; /* 24h: Current max pointer of delay line */
                                /* 28h: Delay line buffer (L + R interleaved) */
            int32_t delay[DELAY_LEN(DSP_OUT_MAX_HZ)]; /* Target-dependent size */
        };
    };
} crossfeed_state IBSS_ATTR;

static int crossfeed_type = CROSSFEED_TYPE_NONE;
/* Cached custom settings */
static long crossfeed_lf_gain;
static long crossfeed_hf_gain;
static long crossfeed_cutoff;

/* Discard the sample histories */
static void crossfeed_flush(struct dsp_proc_entry *this)
{
    struct crossfeed_state *state = (void *)this->data;

    if (crossfeed_type != CROSSFEED_TYPE_CUSTOM)
    {
        state->vcl = state->vcr = state->vdiff = 0;
    }
    else
    {
        memset(state->history, 0, sizeof (state->history));
        memset(state->delay, 0, sizeof (state->delay));
        state->index = state->delay;
    }
}

static void crossfeed_meier_update_filter(struct crossfeed_state *state,
                                          unsigned int fout)
{
    /* 1 / (F.Rforward.C) */
    state->coef1 = fp_div(2128, fout, 31);
    /* 1 / (F.Rcross.C) */
    state->coef2 = fp_div(1000, fout, 31);
}

static void crossfeed_custom_update_filter(struct crossfeed_state *state,
                                           unsigned int fout)
{
    long lf_gain = crossfeed_lf_gain;
    long hf_gain = crossfeed_hf_gain;
    long cutoff  = crossfeed_cutoff;
    int32_t *c = state->coefs;

    long scaler = get_replaygain_int(lf_gain * 10) << 7;

    cutoff = fp_div(cutoff, fout, 32);
    hf_gain -= lf_gain;
    /* Divide cutoff by sqrt(10^(hf_gain/20)) to place cutoff at the -3 dB
     * point instead of shelf midpoint. This is for compatibility with the old
     * crossfeed shelf filter and should be removed if crossfeed settings are
     * ever made incompatible for any other good reason.
     */
    cutoff = fp_div(cutoff, get_replaygain_int(hf_gain*5), 24);

    filter_shelf_coefs(cutoff, hf_gain, false, c);
    /* Scale coefs by LF gain and shift them to s0.31 format. We have no gains
     * over 1 and can do this safely
     */
    c[0] = FRACMUL_SHL(c[0], scaler, 4);
    c[1] = FRACMUL_SHL(c[1], scaler, 4);
    c[2] <<= 4;
}


/** DSP interface **/

/* Set the type of crossfeed to use */
void dsp_set_crossfeed_type(int type)
{
    if (type == crossfeed_type)
        return; /* No change */

    crossfeed_type = type;

    struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO);
    dsp_proc_enable(dsp, DSP_PROC_CROSSFEED, type != CROSSFEED_TYPE_NONE);
}

/* Set the gain of the dry mix */
void dsp_set_crossfeed_direct_gain(int gain)
{
    uint32_t gain32 = get_replaygain_int(gain * 10);
    crossfeed_state.gain =
        gain32 >= (0x80000000ul >> 7) ? 0x7ffffffful: (gain32 << 7);
}

/* Both gains should be below 0 dB */
void dsp_set_crossfeed_cross_params(long lf_gain, long hf_gain, long cutoff)
{
    crossfeed_lf_gain = lf_gain;
    crossfeed_hf_gain = hf_gain;
    crossfeed_cutoff  = cutoff;

    if (crossfeed_type != CROSSFEED_TYPE_CUSTOM)
        return;

    struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO);
    crossfeed_custom_update_filter(&crossfeed_state,
                                   dsp_get_output_frequency(dsp));
}

#if !defined(CPU_COLDFIRE) && !defined(CPU_ARM)
/* Apply the crossfade to the buffer in place */
void crossfeed_process(struct dsp_proc_entry *this, struct dsp_buffer **buf_p)
{
    struct crossfeed_state *state = (void *)this->data;
    struct dsp_buffer *buf = *buf_p;
   
    int32_t *hist_l = &state->history[0];
    int32_t *hist_r = &state->history[2];
    int32_t *delay = state->delay;
    int32_t *coefs = &state->coefs[0];
    int32_t gain = state->gain;
    int32_t *di = state->index;
    int32_t *di_max = state->index_max;

    int count = buf->remcount;

    for (int i = 0; i < count; i++)
    {
        int32_t left = buf->p32[0][i];
        int32_t right = buf->p32[1][i];

        /* Filter delayed sample from left speaker */
        int32_t acc = FRACMUL(*di, coefs[0]);
        acc += FRACMUL(hist_l[0], coefs[1]);
        acc += FRACMUL(hist_l[1], coefs[2]);
        /* Save filter history for left speaker */
        hist_l[1] = acc;
        hist_l[0] = *di;
        *di++ = left;
        /* Filter delayed sample from right speaker */
        acc = FRACMUL(*di, coefs[0]);
        acc += FRACMUL(hist_r[0], coefs[1]);
        acc += FRACMUL(hist_r[1], coefs[2]);
        /* Save filter history for right speaker */
        hist_r[1] = acc;
        hist_r[0] = *di;
        *di++ = right;
        /* Now add the attenuated direct sound and write to outputs */
        buf->p32[0][i] = FRACMUL(left, gain) + hist_r[1];
        buf->p32[1][i] = FRACMUL(right, gain) + hist_l[1];

        /* Wrap delay line index if bigger than delay line size */
        if (di >= di_max)
            di = delay;
    }

    /* Write back local copies of data we've modified */
    state->index = di;
}
#endif /* CPU */

#if !defined(CPU_COLDFIRE) && !defined(CPU_ARM)
/**
 * Implementation of the "simple" passive crossfeed circuit by Jan Meier.
 * See also: http://www.meier-audio.homepage.t-online.de/passivefilter.htm
 */

void crossfeed_meier_process(struct dsp_proc_entry *this,
                             struct dsp_buffer **buf_p)
{
    struct dsp_buffer *buf = *buf_p;

    /* Get filter state */
    struct crossfeed_state *state = (struct crossfeed_state *)this->data;
    int32_t vcl = state->vcl;
    int32_t vcr = state->vcr;
    int32_t vdiff = state->vdiff;
    int32_t coef1 = state->coef1;
    int32_t coef2 = state->coef2;

    int count = buf->remcount;

    for (int i = 0; i < count; i++)
    {
        /* Calculate new output */
        int32_t lout = buf->p32[0][i] + vcl;
        int32_t rout = buf->p32[1][i] + vcr;
        buf->p32[0][i] = lout;
        buf->p32[1][i] = rout;

        /* Update filter state */
        int32_t common = FRACMUL(vdiff, coef2);
        vcl -= FRACMUL(vcl, coef1) + common;
        vcr -= FRACMUL(vcr, coef1) - common;

        vdiff = lout - rout;
    }

    /* Store filter state */
    state->vcl = vcl;
    state->vcr = vcr;
    state->vdiff = vdiff;
}
#endif /* CPU */

/* Update the processing function according to crossfeed type */
static void update_process_fn(struct dsp_proc_entry *this,
                              struct dsp_config *dsp)
{
    struct crossfeed_state *state = (struct crossfeed_state *)this->data;
    dsp_proc_fn_type fn;

    unsigned int fout = dsp_get_output_frequency(dsp);

    if (crossfeed_type != CROSSFEED_TYPE_CUSTOM)
    {
        crossfeed_meier_update_filter(state, fout);
        fn = crossfeed_meier_process;
    }
    else
    {
        state->index_max = state->delay + DELAY_LEN(fout);
        crossfeed_custom_update_filter(state, fout);
        fn = crossfeed_process;
    }

    if (this->process != fn)
    {
        this->process = fn;    /* Set proper function */
        if (dsp_proc_active(dsp, DSP_PROC_CROSSFEED))
            crossfeed_flush(this);
    }
}

/* Crossfeed boot/format change function */
static intptr_t crossfeed_new_format(struct dsp_proc_entry *this,
                                     struct dsp_config *dsp,
                                     struct sample_format *format)
{
    DSP_PRINT_FORMAT(DSP_PROC_CROSSFEED, format);

    bool was_active = dsp_proc_active(dsp, DSP_PROC_CROSSFEED);
    bool active = format->num_channels >= 2;
    dsp_proc_activate(dsp, DSP_PROC_CROSSFEED, active);

    if (active)
    {
        if (!was_active)
            crossfeed_flush(this); /* Going online */

        return PROC_NEW_FORMAT_OK;
    }

    /* Can't do this. Sleep until next change */
    DEBUGF("  DSP_PROC_CROSSFEED- deactivated\n");
    return PROC_NEW_FORMAT_DEACTIVATED;
}

/* DSP message hook */
static intptr_t crossfeed_configure(struct dsp_proc_entry *this,
                                    struct dsp_config *dsp,
                                    unsigned int setting,
                                    intptr_t value)
{
    intptr_t retval = 0;

    switch (setting)
    {
    case DSP_PROC_INIT:
        if (value == 0)
            this->data = (intptr_t)&crossfeed_state;

    case DSP_SET_OUT_FREQUENCY:
        update_process_fn(this, dsp);
        break;

    case DSP_FLUSH:
        crossfeed_flush(this);
        break;

    case DSP_PROC_NEW_FORMAT:
        retval = crossfeed_new_format(this, dsp,
                                      (struct sample_format *)value);
        break;
    }

    return retval;
}

/* Database entry */
DSP_PROC_DB_ENTRY(
    CROSSFEED,
    crossfeed_configure);