OpenSolaris_b135/cmd/audio/utilities/Resample.cc
/*
* CDDL HEADER START
*
* The contents of this file are subject to the terms of the
* Common Development and Distribution License, Version 1.0 only
* (the "License"). You may not use this file except in compliance
* with the License.
*
* You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
* or http://www.opensolaris.org/os/licensing.
* See the License for the specific language governing permissions
* and limitations under the License.
*
* When distributing Covered Code, include this CDDL HEADER in each
* file and include the License file at usr/src/OPENSOLARIS.LICENSE.
* If applicable, add the following below this CDDL HEADER, with the
* fields enclosed by brackets "[]" replaced with your own identifying
* information: Portions Copyright [yyyy] [name of copyright owner]
*
* CDDL HEADER END
*/
/*
* Copyright (c) 1992-2001 by Sun Microsystems, Inc.
* All rights reserved.
*/
#pragma ident "%Z%%M% %I% %E% SMI"
/*
* This is a impementation of sampling rate conversion for low-passed signals.
*
* To convert the input signal of sampling rate of fi to another rate of fo,
* the least common multiple of fi and fo is derived first:
* fm = L * fi = M * fo.
* Then the input signal is up-sampled to fm by inserting (L -1) zero valued
* samples after each input sample, low-pass filtered, and down-smapled by
* saving one sample for every M samples. The implementation takes advantages
* of the (L - 1) zero values in the filter input and the (M - 1) output
* samples to be disregarded.
*
* If L = 1 or M = 1, a simpler decimation or interpolation is used.
*/
#include <memory.h>
#include <math.h>
#include <Resample.h>
extern "C" {
char *bcopy(char *, char *, int);
char *memmove(char *, char *, int);
}
#define BCOPY(src, dest, num) memmove(dest, src, num)
// convolve(), short2double() and double2short() are defined in Fir.cc.
extern double convolve(double *, double *, int);
extern void short2double(double *, short *, int);
extern short double2short(double);
// generate truncated ideal LPF
static void sinc_coef(int fold, // sample rate change
int order, // LP FIR filter order
double *coef) // filter coefficients
{
int i;
float alpha;
double bandwidth = M_PI / fold; // digital bandwidth of pass band
int half = order >> 1;
if (order & 1) { // order is odd, center = half + 0.5
float center = half + 0.5;
for (i = 0; i <= half; i++) {
alpha = center - i;
coef[i] = sin(bandwidth * alpha) / (M_PI * alpha);
}
} else { // order is even, center = half
for (i = 0; i < half; i++) {
alpha = half - i;
coef[i] = sin(bandwidth * alpha) / (M_PI * alpha);
}
coef[i++] = bandwidth / M_PI;
}
for (; i <= order; i++) // symmetric FIR
coef[i] = coef[order - i];
}
/*
* poly_conv() = coef[0] * data[length - 1] +
* coef[inc_coef] * data[length - 2] +
* ...
* coef[(length - 1) * inc_coef] * data[0] +
*
* convolution of coef[order + 1] and data[length] up-sampled by a factor
* of inc_coef. The terms of coef[1, 2, ... inc_coef - 1] multiplying 0
* are ignored.
*/
// polyphase filter convolution
double
poly_conv(double *coef, // filter coef array
int order, // filter order
int inc_coef, // 1-to-L up-sample for data[length]
double *data, // data array
int length) // data array length
{
if ((order < 0) || (inc_coef < 1) || (length < 1))
return (0.0);
else {
double sum = 0.0;
double *coef_end = coef + order;
double *data_end = data + length;
while ((coef <= coef_end) && (data < data_end)) {
sum += *coef * *--data_end;
coef += inc_coef;
}
return (sum);
}
}
int
gcf(int a, int b) // greatest common factor between a and b
{
int remainder = a % b;
return (remainder == 0)? b : gcf(b, remainder);
}
void ResampleFilter::
updateState(
double *in,
int size)
{
if (up <= 1)
Fir::updateState(in, size);
else if (size >= num_state)
memcpy(state, in + size - num_state,
num_state * sizeof (double));
else {
int old = num_state - size;
BCOPY((char *)(state + size), (char *)state,
old * sizeof (double));
memcpy(state + old, in, size * sizeof (double));
}
}
ResampleFilter:: // constructor
ResampleFilter(
int rate_in, // sampling rate of input signal
int rate_out) // sampling rate of output signal
{
// filter sampling rate = common multiple of rate_in and rate_out
int commonfactor = gcf(rate_in, rate_out);
up = rate_out / commonfactor;
down = rate_in / commonfactor;
int fold = (up > down)? up : down; // take the bigger rate change
order = (fold << 4) - 2; // filter order = fold * 16 - 2
coef = new double[order + 1];
sinc_coef(fold, order, coef); // required bandwidth = PI/fold
if (up > 1) { // need (order/up) states
num_state = (order + up - 1) / up;
state = new double[num_state];
for (int i = 0; i < num_state; i++) // reset states
state[i] = 0.0;
} else {
num_state = order;
state = new double[order]; // need order states
resetState();
}
delay = (order + 1) >> 1; // assuming symmetric FIR
down_offset = 0;
up_offset = 0;
}
// down-to-1 decimation
int ResampleFilter::
decimate_noadjust(short *in,
int size,
short *out)
{
int i;
if (size <= 0)
return (0);
else if (down <= 1) // normal filter
return (Fir::filter_noadjust(in, size, out));
else if (size <= down_offset) { // just update states
update_short(in, size);
down_offset -= size;
return (0);
}
double *in_buf = new double[size];
short2double(in_buf, in, size);
// filter and decimate and output
short *out_ptr = out;
int init_size = (size <= order)? size : order;
for (i = down_offset; i < init_size; i += down)
*out_ptr++ = double2short(convolve(coef, in_buf, i + 1) +
convolve(coef + i + 1, state + i, order - i));
for (; i < size; i += down)
*out_ptr++ = double2short(convolve(coef, in_buf + i - order,
order + 1));
down_offset = i - size;
updateState(in_buf, size);
delete in_buf;
return (out_ptr - out);
}
// decimate with group delay adjusted to 0
int ResampleFilter::
decimate(short *in,
int size,
short *out)
{
if (delay <= 0)
return (decimate_noadjust(in, size, out));
else if (size <= delay) {
update_short(in, size);
delay -= size;
return (0);
} else {
update_short(in, delay);
in += delay;
size -= delay;
delay = 0;
return (decimate_noadjust(in, size, out));
}
}
// flush decimate filter
int ResampleFilter::
decimate_flush(short *out)
{
int num_in = Fir::getFlushSize();
short *in = new short[num_in];
memset(in, 0, num_in * sizeof (short));
int num_out = decimate_noadjust(in, num_in, out);
delay += num_in;
delete in;
return (num_out);
}
// 1-to-up interpolation
int ResampleFilter::
interpolate_noadjust(short *in,
int size,
short *out)
{
int i, j;
if (size <= 0)
return (0);
else if (up <= 1) // regular filter
return (Fir::filter_noadjust(in, size, out));
double *in_buf = new double[size];
short2double(in_buf, in, size);
short *out_ptr = out;
// befor the 1st input sample, generate "-up_offset" output samples
int coef_offset = up + up_offset;
for (j = up_offset; j < 0; j++) {
*out_ptr++ = double2short(up * poly_conv(coef + coef_offset,
order - coef_offset, up, state, num_state));
coef_offset++;
}
// for each of the rest input samples, generate up output samples
for (i = 1; i < size; i++) {
for (j = 0; j < up; j++) {
*out_ptr++ = double2short(up * (poly_conv(coef + j,
order - j, up, in_buf, i) + poly_conv(
coef + coef_offset, order - coef_offset, up, state,
num_state)));
coef_offset++;
}
}
// for the last input samples, generate "up_offset + up" output samples
for (j = 0; j < (up_offset + up); j++) {
*out_ptr++ = double2short(up * (poly_conv(coef + j,
order - j, up, in_buf, size) + poly_conv(
coef + coef_offset, order - coef_offset, up, state,
num_state)));
coef_offset++;
}
updateState(in_buf, size);
delete in_buf;
return (out_ptr - out);
}
// flush interpolate filter
int ResampleFilter::
interpolate_flush(short *out)
{
int num = (Fir::getFlushSize() + up - 1) / up;
short *in = new short[num];
memset(in, 0, num * sizeof (short));
int out_num = interpolate_noadjust(in, num, out);
delay += num * up;
delete in;
return (out_num);
}
// interpolate with delay adjusted
int ResampleFilter::
interpolate(short *in,
int size,
short *out)
{
if (size <= 0)
return (interpolate_flush(out));
else if (delay <= 0)
return (interpolate_noadjust(in, size, out));
else {
int delay_in = (delay + up - 1) / up;
if (size < delay_in)
delay_in = size;
double *in_buf = new double[delay_in];
short2double(in_buf, in, delay_in);
updateState(in_buf, delay_in);
delete in_buf;
delay -= delay_in * up;
up_offset = delay;
return (interpolate_noadjust(in + delay_in, size -
delay_in, out));
}
}
int ResampleFilter::
filter_noadjust(short *in, // non-integer sampling rate conversion
int size,
short *out)
{
int i, j;
if (size <= 0)
return (0);
else if (up <= 1)
return (decimate_noadjust(in, size, out));
else if (down <= 1)
return (interpolate_noadjust(in, size, out));
double *in_buf = new double[size];
short2double(in_buf, in, size);
short *init_out = out;
int coef_offset = up_offset + down_offset + up;
/*
* before the 1st input sample,
* start from "up_offset + down_offset"th up-sampled sample
*/
for (j = up_offset + down_offset; j < 0; j += down) {
*out++ = double2short(up * poly_conv(coef + coef_offset,
order - coef_offset, up, state, num_state));
coef_offset += down;
}
// process the input samples until the last one
for (i = 1; i < size; i++) {
for (; j < up; j += down) {
*out++ = double2short(up * (poly_conv(coef + j,
order - j, up, in_buf, i) + poly_conv(
coef + coef_offset, order - coef_offset, up, state,
num_state)));
coef_offset += down;
}
j -= up;
}
// for the last input sample, end at the "up + up_offset"th
for (; j < (up + up_offset); j += down) {
*out++ = double2short(up * (poly_conv(coef + j, order - j, up,
in_buf, size) + poly_conv(coef + coef_offset,
order - coef_offset, up, state, num_state)));
coef_offset += down;
}
down_offset = j - (up + up_offset);
updateState(in_buf, size);
delete in_buf;
return (out - init_out);
}
int ResampleFilter::
getFlushSize(void)
{
int num_in = (Fir::getFlushSize() + up - 1) / up;
return ((num_in * up + down - 1 - down_offset) / down);
}
int ResampleFilter::
flush(short *out) // flush resampling filter
{
if (down <= 1)
return (interpolate_flush(out));
else if (up <= 1)
return (decimate_flush(out));
int num = (Fir::getFlushSize() + up - 1) / up;
short *in = new short[num];
memset(in, 0, num * sizeof (short));
int out_num = filter_noadjust(in, num, out);
delete in;
delay += num * up;
return (out_num);
}
/*
* sampling rate conversion with filter delay adjusted
*/
int ResampleFilter::
filter(
short *in,
int size,
short *out)
{
if (size <= 0)
return (flush(out));
else if (up <= 1)
return (decimate(in, size, out));
else if (down <= 1)
return (interpolate(in, size, out));
else if (delay <= 0)
return (filter_noadjust(in, size, out));
else {
int delay_in = (delay + up - 1) / up;
if (size < delay_in)
delay_in = size;
double *in_buf = new double[delay_in];
short2double(in_buf, in, delay_in);
updateState(in_buf, delay_in);
delete in_buf;
delay -= up * delay_in;
if (delay <= 0) {
up_offset = delay;
down_offset = 0;
}
return (filter_noadjust(in + delay_in, size - delay_in, out));
}
}