/*************************************************************************** * __________ __ ___. * 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 "dsp_misc.h" #include "resample.h" #include /** * 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 */ unsigned int frequency; /* Virtual input samplerate */ unsigned int frequency_out; /* Resampler output 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, unsigned int fout) { unsigned int frequency = format->frequency; /* virtual samplerate */ data->frequency = frequency; data->frequency_out = fout; data->delta = fp_div(frequency, fout, 16); if (frequency == data->frequency_out) { /* 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); unsigned int frequency = data->frequency; unsigned int fout = dsp_get_output_frequency(dsp); bool active = dsp_proc_active(dsp, DSP_PROC_RESAMPLE); if ((unsigned int)format->frequency != frequency || data->frequency_out != fout) { DEBUGF(" DSP_PROC_RESAMPLE- new settings: %u %u\n", format->frequency, fout); active = resample_new_delta(data, format, fout); dsp_proc_activate(dsp, DSP_PROC_RESAMPLE, active); } /* Everything after us is fout */ dst->format = *format; dst->format.frequency = fout; dst->format.codec_frequency = fout; if (active) return PROC_NEW_FORMAT_OK; /* No longer needed */ DEBUGF(" DSP_PROC_RESAMPLE- deactivated\n"); return PROC_NEW_FORMAT_DEACTIVATED; } void dsp_resample_init(struct dsp_config *dsp, unsigned int 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 %u\n", 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 resample_proc_init(struct dsp_proc_entry *this, struct dsp_config *dsp) { struct resample_data *data = &resample_data[dsp_get_id(dsp)]; this->data = (intptr_t)data; dsp_proc_set_in_place(dsp, DSP_PROC_RESAMPLE, false); data->frequency_out = DSP_OUT_DEFAULT_HZ; 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_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; case DSP_SET_OUT_FREQUENCY: dsp_proc_want_format_update(dsp, DSP_PROC_RESAMPLE); break; } return retval; } /* Database entry */ DSP_PROC_DB_ENTRY(RESAMPLE, resample_configure);