summaryrefslogtreecommitdiffstats
path: root/lib/rbcodec/dsp/resample.c
blob: 6e7e5b7b454f569b1dd6622c5bb0b2683cb31944 (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
/***************************************************************************
 *             __________               __   ___.
 *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
 *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
 *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
 *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
 *                     \/            \/     \/    \/            \/
 * $Id$
 *
 * Copyright (C) 2005 Miika Pekkarinen
 * Copyright (C) 2012 Michael Sevakis
 * Copyright (C) 2013 Michael Giacomelli
 *
 * 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 "platform.h"
#include "fracmul.h"
#include "fixedpoint.h"
#include "dsp_proc_entry.h"
#include <string.h>

/**
 * Linear interpolation resampling that introduces a one sample delay because
 * of our inability to look into the future at the end of a frame.
 */

#if 1 /* Set to '0' to enable debug messages */
#undef DEBUGF
#define DEBUGF(...)
#endif

#define RESAMPLE_BUF_COUNT 192 /* Per channel, per DSP */

/* CODEC_IDX_AUDIO = left and right, CODEC_IDX_VOICE = mono */
static int32_t resample_out_bufs[3][RESAMPLE_BUF_COUNT] IBSS_ATTR;

/* Data for each resampler on each DSP */
static struct resample_data
{
    uint32_t delta;         /* 00h: Phase delta for each step in s15.16*/
    uint32_t phase;         /* 04h: Current phase [pos16|frac16] */
    int32_t  history[2][3]; /* 08h: Last samples for interpolation (L+R)
                                    0 = oldest, 2 = newest */
                            /* 20h */
    int32_t  frequency;     /* Virtual samplerate */
    struct dsp_buffer resample_buf; /* Buffer descriptor for resampled data */
    int32_t *resample_out_p[2]; /* Actual output buffer pointers */
} resample_data[DSP_COUNT] IBSS_ATTR;

/* Actual worker function. Implemented here or in target assembly code. */
int resample_hermite(struct resample_data *data, struct dsp_buffer *src,
                     struct dsp_buffer *dst);

static void resample_flush_data(struct resample_data *data)
{
    data->phase = 0;
    memset(&data->history, 0, sizeof (data->history));
}

static void resample_flush(struct dsp_proc_entry *this)
{
    struct resample_data *data = (void *)this->data;
    data->resample_buf.remcount = 0;
    resample_flush_data(data);
}

static bool resample_new_delta(struct resample_data *data,
                               struct sample_format *format)
{
    int32_t frequency = format->frequency; /* virtual samplerate */

    data->frequency = frequency;
    data->delta = fp_div(frequency, NATIVE_FREQUENCY, 16);

    if (frequency == NATIVE_FREQUENCY)
    {
        /* NOTE: If fully glitch-free transistions from no resampling to
           resampling are desired, history should be maintained even when
           not resampling. */
        resample_flush_data(data);
        return false;
    }

    return true;
}

#if !defined(CPU_COLDFIRE) && !defined(CPU_ARM)
int resample_hermite(struct resample_data *data, struct dsp_buffer *src,
                     struct dsp_buffer *dst)
{
    int ch = src->format.num_channels - 1;
    uint32_t count = MIN(src->remcount, 0x8000);
    uint32_t delta = data->delta;
    uint32_t phase, pos;
    int32_t *d;

    do
    {
        const int32_t *s = src->p32[ch];

        d = dst->p32[ch];
        int32_t *dmax = d + dst->bufcount;

        /* Restore state */
        phase = data->phase;
        pos = phase >> 16;
        pos = MIN(pos, count);

        while (pos < count && d < dmax)
        {
            int x0, x1, x2, x3;

            if (pos < 3)
            {
                x3 = data->history[ch][pos+0];
                x2 = pos < 2 ? data->history[ch][pos+1] : s[pos-2];
                x1 = pos < 1 ? data->history[ch][pos+2] : s[pos-1];
            }
            else
            {
                x3 = s[pos-3];
                x2 = s[pos-2];
                x1 = s[pos-1];
            }

            x0 = s[pos];

            int32_t frac = (phase & 0xffff) << 15;

            /* 4-point, 3rd-order Hermite/Catmull-Rom spline (x-form):
             * c1 = -0.5*x3 + 0.5*x1
             *    = 0.5*(x1 - x3)                <--
             *
             * v = x1 - x2, -v = x2 - x1
             * c2 = x3 - 2.5*x2 + 2*x1 - 0.5*x0
             *    = x3 + 2*(x1 - x2) - 0.5*(x0 + x2)
             *    = x3 + 2*v - 0.5*(x0 + x2)     <--
             *
             * c3 = -0.5*x3 + 1.5*x2 - 1.5*x1 + 0.5*x0
             *    = 0.5*x0 - 0.5*x3 + 0.5*(x2 - x1) + (x2 - x1)
             *    = 0.5*(x0 - x3 - v) - v        <--
             *
             * polynomial coefficients */
            int32_t c1 = (x1 - x3) >> 1;
            int32_t v = x1 - x2;
            int32_t c2 = x3 + 2*v - ((x0 + x2) >> 1);
            int32_t c3 = ((x0 - x3 - v) >> 1) - v;

            /* Evaluate polynomial at time 'frac'; Horner's rule. */
            int32_t acc;
            acc = FRACMUL(c3, frac) + c2;
            acc = FRACMUL(acc, frac) + c1;
            acc = FRACMUL(acc, frac) + x2;

            *d++ = acc;

            phase += delta;
            pos = phase >> 16;
        }

        pos = MIN(pos, count);

        /* Save delay samples for next time. Must do this even if pos was
         * clamped before loop in order to keep record up to date. */
        data->history[ch][0] = pos < 3 ? data->history[ch][pos+0] : s[pos-3];
        data->history[ch][1] = pos < 2 ? data->history[ch][pos+1] : s[pos-2];
        data->history[ch][2] = pos < 1 ? data->history[ch][pos+2] : s[pos-1];
    }
    while (--ch >= 0);

    /* Wrap phase accumulator back to start of next frame. */
    data->phase = phase - (pos << 16);

    dst->remcount = d - dst->p32[0];
    return pos;
}
#endif /* CPU */

/* Resample count stereo samples or stop when the destination is full.
 * Updates the src buffer and changes to its own output buffer to refer to
 * the resampled data. */
static void resample_process(struct dsp_proc_entry *this,
                             struct dsp_buffer **buf_p)
{
    struct resample_data *data = (void *)this->data;
    struct dsp_buffer *src = *buf_p;
    struct dsp_buffer *dst = &data->resample_buf;

    *buf_p = dst;

    if (dst->remcount > 0)
        return; /* data still remains */

    dst->remcount = 0;
    dst->p32[0] = data->resample_out_p[0];
    dst->p32[1] = data->resample_out_p[1];

    if (src->remcount > 0)
    {
        dst->bufcount = RESAMPLE_BUF_COUNT;

        int consumed = resample_hermite(data, src, dst);

        /* Advance src by consumed amount */
        if (consumed > 0)
            dsp_advance_buffer32(src, consumed);
    }
    /* else purged resample_buf */

    /* Inherit in-place processed mask from source buffer */
    dst->proc_mask = src->proc_mask;
}

/* Finish draining old samples then switch format or shut off */
static intptr_t resample_new_format(struct dsp_proc_entry *this,
                                    struct dsp_config *dsp,
                                    struct sample_format *format)
{
    struct resample_data *data = (void *)this->data;
    struct dsp_buffer *dst = &data->resample_buf;

    if (dst->remcount > 0)
        return PROC_NEW_FORMAT_TRANSITION;

    DSP_PRINT_FORMAT(DSP_PROC_RESAMPLE, *format);

    int32_t frequency = data->frequency;
    bool active = dsp_proc_active(dsp, DSP_PROC_RESAMPLE);

    if (format->frequency != frequency)
    {
        DEBUGF("  DSP_PROC_RESAMPLE- new delta\n");
        active = resample_new_delta(data, format);
        dsp_proc_activate(dsp, DSP_PROC_RESAMPLE, active);
    }

    /* Everything after us is NATIVE_FREQUENCY */
    dst->format = *format;
    dst->format.frequency = NATIVE_FREQUENCY;
    dst->format.codec_frequency = NATIVE_FREQUENCY;

    if (active)
        return PROC_NEW_FORMAT_OK;

    /* No longer needed */
    DEBUGF("  DSP_PROC_RESAMPLE- deactivated\n");
    return PROC_NEW_FORMAT_DEACTIVATED;
}

static void INIT_ATTR resample_dsp_init(struct dsp_config *dsp,
                                        enum dsp_ids dsp_id)
{
    int32_t *lbuf, *rbuf;

    switch (dsp_id)
    {
    case CODEC_IDX_AUDIO:
        lbuf = resample_out_bufs[0];
        rbuf = resample_out_bufs[1];
        break;

    case CODEC_IDX_VOICE:
        lbuf = rbuf = resample_out_bufs[2]; /* Always mono */
        break;

    default:
        /* huh? */
        DEBUGF("DSP_PROC_RESAMPLE- unknown DSP %d\n", (int)dsp_id);
        return;
    }

    /* Always enable resampler so that format changes may be monitored and
     * it self-activated when required */
    dsp_proc_enable(dsp, DSP_PROC_RESAMPLE, true);
    resample_data[dsp_id].resample_out_p[0] = lbuf;
    resample_data[dsp_id].resample_out_p[1] = rbuf;
}

static void INIT_ATTR resample_proc_init(struct dsp_proc_entry *this,
                                         struct dsp_config *dsp)
{
    dsp_proc_set_in_place(dsp, DSP_PROC_RESAMPLE, false);
    this->data = (intptr_t)&resample_data[dsp_get_id(dsp)];
    this->process = resample_process;
}

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

    switch (setting)
    {
    case DSP_INIT:
        resample_dsp_init(dsp, (enum dsp_ids)value);
        break;

    case DSP_FLUSH:
        resample_flush(this);
        break;

    case DSP_PROC_INIT:
        resample_proc_init(this, dsp);
        break;

    case DSP_PROC_CLOSE:
        /* This stage should be enabled at all times */
        DEBUGF("DSP_PROC_RESAMPLE- Error: Closing!\n");
        break;

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

    return retval;
}

/* Database entry */
DSP_PROC_DB_ENTRY(RESAMPLE,
                  resample_configure);