2012-03-27 23:52:15 +00:00
|
|
|
/***************************************************************************
|
|
|
|
* __________ __ ___.
|
|
|
|
* Open \______ \ ____ ____ | | _\_ |__ _______ ___
|
|
|
|
* Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
|
|
|
|
* Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
|
|
|
|
* Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
|
|
|
|
* \/ \/ \/ \/ \/
|
|
|
|
* $Id$
|
|
|
|
*
|
|
|
|
* Copyright (C) 2005 Miika Pekkarinen
|
|
|
|
* Copyright (C) 2012 Michael Sevakis
|
2012-05-07 07:12:56 +00:00
|
|
|
* Copyright (C) 2013 Michael Giacomelli
|
2012-03-27 23:52:15 +00:00
|
|
|
*
|
|
|
|
* 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.
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
2013-05-27 07:23:33 +00:00
|
|
|
#include "rbcodecconfig.h"
|
|
|
|
#include "platform.h"
|
2012-03-27 23:52:15 +00:00
|
|
|
#include "fracmul.h"
|
|
|
|
#include "fixedpoint.h"
|
|
|
|
#include "dsp_proc_entry.h"
|
2013-05-23 17:58:51 +00:00
|
|
|
#include "dsp_misc.h"
|
2022-12-03 15:33:15 +00:00
|
|
|
#include "resample.h"
|
2012-04-29 21:31:30 +00:00
|
|
|
#include <string.h>
|
2012-03-27 23:52:15 +00:00
|
|
|
|
|
|
|
/**
|
|
|
|
* Linear interpolation resampling that introduces a one sample delay because
|
|
|
|
* of our inability to look into the future at the end of a frame.
|
|
|
|
*/
|
|
|
|
|
2012-05-07 07:12:56 +00:00
|
|
|
#if 1 /* Set to '0' to enable debug messages */
|
2012-03-27 23:52:15 +00:00
|
|
|
#undef DEBUGF
|
|
|
|
#define DEBUGF(...)
|
|
|
|
#endif
|
|
|
|
|
|
|
|
#define RESAMPLE_BUF_COUNT 192 /* Per channel, per DSP */
|
|
|
|
|
2012-05-05 02:00:44 +00:00
|
|
|
/* CODEC_IDX_AUDIO = left and right, CODEC_IDX_VOICE = mono */
|
|
|
|
static int32_t resample_out_bufs[3][RESAMPLE_BUF_COUNT] IBSS_ATTR;
|
|
|
|
|
2012-03-27 23:52:15 +00:00
|
|
|
/* Data for each resampler on each DSP */
|
|
|
|
static struct resample_data
|
|
|
|
{
|
2012-05-07 07:12:56 +00:00
|
|
|
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 */
|
2013-05-23 17:58:51 +00:00
|
|
|
unsigned int frequency; /* Virtual input samplerate */
|
|
|
|
unsigned int frequency_out; /* Resampler output samplerate */
|
2012-03-27 23:52:15 +00:00
|
|
|
struct dsp_buffer resample_buf; /* Buffer descriptor for resampled data */
|
2013-05-23 17:58:51 +00:00
|
|
|
int32_t *resample_out_p[2]; /* Actual output buffer pointers */
|
2012-03-27 23:52:15 +00:00
|
|
|
} resample_data[DSP_COUNT] IBSS_ATTR;
|
|
|
|
|
|
|
|
/* Actual worker function. Implemented here or in target assembly code. */
|
2012-05-07 07:12:56 +00:00
|
|
|
int resample_hermite(struct resample_data *data, struct dsp_buffer *src,
|
|
|
|
struct dsp_buffer *dst);
|
2012-03-27 23:52:15 +00:00
|
|
|
|
2013-05-07 04:35:46 +00:00
|
|
|
static void resample_flush_data(struct resample_data *data)
|
2012-03-27 23:52:15 +00:00
|
|
|
{
|
|
|
|
data->phase = 0;
|
2012-05-07 07:12:56 +00:00
|
|
|
memset(&data->history, 0, sizeof (data->history));
|
2012-03-27 23:52:15 +00:00
|
|
|
}
|
|
|
|
|
2013-05-07 04:35:46 +00:00
|
|
|
static void resample_flush(struct dsp_proc_entry *this)
|
2012-03-27 23:52:15 +00:00
|
|
|
{
|
|
|
|
struct resample_data *data = (void *)this->data;
|
|
|
|
data->resample_buf.remcount = 0;
|
2013-05-07 04:35:46 +00:00
|
|
|
resample_flush_data(data);
|
2012-03-27 23:52:15 +00:00
|
|
|
}
|
|
|
|
|
2013-05-07 04:35:46 +00:00
|
|
|
static bool resample_new_delta(struct resample_data *data,
|
2013-05-23 17:58:51 +00:00
|
|
|
struct sample_format *format,
|
|
|
|
unsigned int fout)
|
2012-03-27 23:52:15 +00:00
|
|
|
{
|
2013-05-23 17:58:51 +00:00
|
|
|
unsigned int frequency = format->frequency; /* virtual samplerate */
|
2012-03-27 23:52:15 +00:00
|
|
|
|
|
|
|
data->frequency = frequency;
|
2013-05-23 17:58:51 +00:00
|
|
|
data->frequency_out = fout;
|
|
|
|
data->delta = fp_div(frequency, fout, 16);
|
2012-03-27 23:52:15 +00:00
|
|
|
|
2013-05-23 17:58:51 +00:00
|
|
|
if (frequency == data->frequency_out)
|
2012-03-27 23:52:15 +00:00
|
|
|
{
|
|
|
|
/* NOTE: If fully glitch-free transistions from no resampling to
|
2012-05-07 07:12:56 +00:00
|
|
|
resampling are desired, history should be maintained even when
|
|
|
|
not resampling. */
|
2013-05-07 04:35:46 +00:00
|
|
|
resample_flush_data(data);
|
2012-03-27 23:52:15 +00:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
#if !defined(CPU_COLDFIRE) && !defined(CPU_ARM)
|
2012-05-07 07:12:56 +00:00
|
|
|
int resample_hermite(struct resample_data *data, struct dsp_buffer *src,
|
|
|
|
struct dsp_buffer *dst)
|
2012-03-27 23:52:15 +00:00
|
|
|
{
|
|
|
|
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;
|
2012-05-20 05:11:52 +00:00
|
|
|
|
2012-03-27 23:52:15 +00:00
|
|
|
do
|
|
|
|
{
|
|
|
|
const int32_t *s = src->p32[ch];
|
|
|
|
|
|
|
|
d = dst->p32[ch];
|
|
|
|
int32_t *dmax = d + dst->bufcount;
|
|
|
|
|
2012-05-07 07:12:56 +00:00
|
|
|
/* Restore state */
|
2012-03-27 23:52:15 +00:00
|
|
|
phase = data->phase;
|
|
|
|
pos = phase >> 16;
|
|
|
|
pos = MIN(pos, count);
|
|
|
|
|
2012-05-07 07:12:56 +00:00
|
|
|
while (pos < count && d < dmax)
|
2012-03-27 23:52:15 +00:00
|
|
|
{
|
2012-05-07 07:12:56 +00:00
|
|
|
int x0, x1, x2, x3;
|
2012-03-27 23:52:15 +00:00
|
|
|
|
2012-05-07 07:12:56 +00:00
|
|
|
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];
|
2012-03-27 23:52:15 +00:00
|
|
|
}
|
2012-05-07 07:12:56 +00:00
|
|
|
else
|
2012-03-27 23:52:15 +00:00
|
|
|
{
|
2012-05-07 07:12:56 +00:00
|
|
|
x3 = s[pos-3];
|
|
|
|
x2 = s[pos-2];
|
|
|
|
x1 = s[pos-1];
|
2012-03-27 23:52:15 +00:00
|
|
|
}
|
2012-05-07 07:12:56 +00:00
|
|
|
|
|
|
|
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;
|
2012-03-27 23:52:15 +00:00
|
|
|
}
|
2012-05-20 05:11:52 +00:00
|
|
|
|
2012-05-07 07:12:56 +00:00
|
|
|
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];
|
2012-03-27 23:52:15 +00:00
|
|
|
}
|
|
|
|
while (--ch >= 0);
|
|
|
|
|
|
|
|
/* Wrap phase accumulator back to start of next frame. */
|
|
|
|
data->phase = phase - (pos << 16);
|
2012-05-20 05:11:52 +00:00
|
|
|
|
2012-03-27 23:52:15 +00:00
|
|
|
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. */
|
2013-05-07 04:35:46 +00:00
|
|
|
static void resample_process(struct dsp_proc_entry *this,
|
|
|
|
struct dsp_buffer **buf_p)
|
2012-03-27 23:52:15 +00:00
|
|
|
{
|
|
|
|
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;
|
2012-12-19 22:34:57 +00:00
|
|
|
dst->p32[0] = data->resample_out_p[0];
|
|
|
|
dst->p32[1] = data->resample_out_p[1];
|
2012-03-27 23:52:15 +00:00
|
|
|
|
|
|
|
if (src->remcount > 0)
|
|
|
|
{
|
|
|
|
dst->bufcount = RESAMPLE_BUF_COUNT;
|
|
|
|
|
2012-05-07 07:12:56 +00:00
|
|
|
int consumed = resample_hermite(data, src, dst);
|
2012-03-27 23:52:15 +00:00
|
|
|
|
|
|
|
/* 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 */
|
2013-05-07 04:35:46 +00:00
|
|
|
static intptr_t resample_new_format(struct dsp_proc_entry *this,
|
|
|
|
struct dsp_config *dsp,
|
|
|
|
struct sample_format *format)
|
2012-03-27 23:52:15 +00:00
|
|
|
{
|
|
|
|
struct resample_data *data = (void *)this->data;
|
|
|
|
struct dsp_buffer *dst = &data->resample_buf;
|
|
|
|
|
|
|
|
if (dst->remcount > 0)
|
2012-12-19 22:34:57 +00:00
|
|
|
return PROC_NEW_FORMAT_TRANSITION;
|
2012-03-27 23:52:15 +00:00
|
|
|
|
2012-12-19 22:34:57 +00:00
|
|
|
DSP_PRINT_FORMAT(DSP_PROC_RESAMPLE, *format);
|
2012-03-27 23:52:15 +00:00
|
|
|
|
2013-05-23 17:58:51 +00:00
|
|
|
unsigned int frequency = data->frequency;
|
|
|
|
unsigned int fout = dsp_get_output_frequency(dsp);
|
2012-03-27 23:52:15 +00:00
|
|
|
bool active = dsp_proc_active(dsp, DSP_PROC_RESAMPLE);
|
|
|
|
|
2013-05-23 17:58:51 +00:00
|
|
|
if ((unsigned int)format->frequency != frequency ||
|
|
|
|
data->frequency_out != fout)
|
2012-03-27 23:52:15 +00:00
|
|
|
{
|
2013-05-23 17:58:51 +00:00
|
|
|
DEBUGF(" DSP_PROC_RESAMPLE- new settings: %u %u\n",
|
|
|
|
format->frequency, fout);
|
|
|
|
active = resample_new_delta(data, format, fout);
|
2012-03-27 23:52:15 +00:00
|
|
|
dsp_proc_activate(dsp, DSP_PROC_RESAMPLE, active);
|
|
|
|
}
|
|
|
|
|
2013-05-23 17:58:51 +00:00
|
|
|
/* Everything after us is fout */
|
2012-12-19 22:34:57 +00:00
|
|
|
dst->format = *format;
|
2013-05-23 17:58:51 +00:00
|
|
|
dst->format.frequency = fout;
|
|
|
|
dst->format.codec_frequency = fout;
|
2012-03-27 23:52:15 +00:00
|
|
|
|
2012-12-19 22:34:57 +00:00
|
|
|
if (active)
|
|
|
|
return PROC_NEW_FORMAT_OK;
|
2012-03-27 23:52:15 +00:00
|
|
|
|
2012-12-19 22:34:57 +00:00
|
|
|
/* No longer needed */
|
|
|
|
DEBUGF(" DSP_PROC_RESAMPLE- deactivated\n");
|
|
|
|
return PROC_NEW_FORMAT_DEACTIVATED;
|
2012-03-27 23:52:15 +00:00
|
|
|
}
|
|
|
|
|
2022-12-03 15:33:15 +00:00
|
|
|
void dsp_resample_init(struct dsp_config *dsp, unsigned int dsp_id)
|
2012-05-05 02:00:44 +00:00
|
|
|
{
|
|
|
|
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? */
|
2022-12-03 15:24:41 +00:00
|
|
|
DEBUGF("DSP_PROC_RESAMPLE- unknown DSP %u\n", dsp_id);
|
2012-05-05 02:00:44 +00:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2012-12-19 22:34:57 +00:00
|
|
|
/* 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;
|
|
|
|
}
|
|
|
|
|
2022-12-03 15:04:06 +00:00
|
|
|
static void resample_proc_init(struct dsp_proc_entry *this,
|
|
|
|
struct dsp_config *dsp)
|
2012-12-19 22:34:57 +00:00
|
|
|
{
|
2013-05-23 17:58:51 +00:00
|
|
|
struct resample_data *data = &resample_data[dsp_get_id(dsp)];
|
|
|
|
this->data = (intptr_t)data;
|
2012-12-19 22:34:57 +00:00
|
|
|
dsp_proc_set_in_place(dsp, DSP_PROC_RESAMPLE, false);
|
2013-05-23 17:58:51 +00:00
|
|
|
data->frequency_out = DSP_OUT_DEFAULT_HZ;
|
2013-05-07 04:35:46 +00:00
|
|
|
this->process = resample_process;
|
2012-05-05 02:00:44 +00:00
|
|
|
}
|
|
|
|
|
2012-03-27 23:52:15 +00:00
|
|
|
/* DSP message hook */
|
2013-05-07 04:35:46 +00:00
|
|
|
static intptr_t resample_configure(struct dsp_proc_entry *this,
|
|
|
|
struct dsp_config *dsp,
|
|
|
|
unsigned int setting,
|
|
|
|
intptr_t value)
|
2012-03-27 23:52:15 +00:00
|
|
|
{
|
2012-12-19 22:34:57 +00:00
|
|
|
intptr_t retval = 0;
|
|
|
|
|
2012-03-27 23:52:15 +00:00
|
|
|
switch (setting)
|
|
|
|
{
|
|
|
|
case DSP_FLUSH:
|
2013-05-07 04:35:46 +00:00
|
|
|
resample_flush(this);
|
2012-03-27 23:52:15 +00:00
|
|
|
break;
|
|
|
|
|
|
|
|
case DSP_PROC_INIT:
|
2013-05-07 04:35:46 +00:00
|
|
|
resample_proc_init(this, dsp);
|
2012-03-27 23:52:15 +00:00
|
|
|
break;
|
|
|
|
|
|
|
|
case DSP_PROC_CLOSE:
|
|
|
|
/* This stage should be enabled at all times */
|
|
|
|
DEBUGF("DSP_PROC_RESAMPLE- Error: Closing!\n");
|
|
|
|
break;
|
2012-12-19 22:34:57 +00:00
|
|
|
|
|
|
|
case DSP_PROC_NEW_FORMAT:
|
2013-05-07 04:35:46 +00:00
|
|
|
retval = resample_new_format(this, dsp, (struct sample_format *)value);
|
2012-12-19 22:34:57 +00:00
|
|
|
break;
|
2013-05-23 17:58:51 +00:00
|
|
|
|
|
|
|
case DSP_SET_OUT_FREQUENCY:
|
|
|
|
dsp_proc_want_format_update(dsp, DSP_PROC_RESAMPLE);
|
|
|
|
break;
|
2012-03-27 23:52:15 +00:00
|
|
|
}
|
|
|
|
|
2012-12-19 22:34:57 +00:00
|
|
|
return retval;
|
2012-03-27 23:52:15 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/* Database entry */
|
|
|
|
DSP_PROC_DB_ENTRY(RESAMPLE,
|
2013-05-07 04:35:46 +00:00
|
|
|
resample_configure);
|