/*
* 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"
#include <memory.h>
#include <stddef.h>
#include <sys/types.h>
#include <Fir.h>
extern "C" {
char *bcopy(char *, char *, int);
char *memmove(char *, char *, int);
}
#define BCOPY(src, dest, num) memmove(dest, src, num)
/*
* convolve()
* returns the convolution of coef[length] and in_buf[length]:
*
* convolution = coef[0] * in_buf[length - 1] +
* coef[1] * in_buf[length - 2] +
* ...
* coef[length - 1] * in_buf[0]
*/
double
convolve(
double *coefs,
double *in_buf,
int length)
{
if (length <= 0)
return (0.0);
else {
in_buf += --length;
double sum = *coefs * *in_buf;
while (length--)
sum += *++coefs * *--in_buf;
return (sum);
}
}
void // convert short to double
short2double(
double *out,
short *in,
int size)
{
while (size-- > 0)
*out++ = (double)*in++;
}
short
double2short(double in) // limit double to short
{
if (in <= -32768.0)
return (-32768);
else if (in >= 32767.0)
return (32767);
else
return ((short)in);
}
void Fir:: // update state with data[size]
updateState(
double *data,
int size)
{
if (size >= order)
memcpy(state, data + size - order, order * sizeof (double));
else {
int old = order - size;
BCOPY((char *)(state + size), (char *)state,
old * sizeof (double));
memcpy(state + order - size, data, size * sizeof (double));
}
}
void Fir::
update_short(
short *in,
int size)
{
double *in_buf = new double[size];
short2double(in_buf, in, size);
updateState(in_buf, size);
delete in_buf;
}
void Fir::
resetState(void) // reset state to all zero
{
for (int i = 0; i < order; i++)
state[i] = 0.0;
}
Fir::
Fir(void)
{
}
Fir::
Fir(int order_in): order(order_in) // construct Fir object
{
state = new double[order];
resetState();
coef = new double[order + 1];
delay = (order + 1) >> 1; // assuming symmetric FIR
}
Fir::
~Fir() // destruct Fir object
{
delete coef;
delete state;
}
int Fir::
getOrder(void) // returns filter order
{
return (order);
}
int Fir::
getNumCoefs(void) // returns number of filter coefficients
{
return (order + 1);
}
void Fir::
putCoef(double *coef_in) // copy coef_in in filter coefficients
{
memcpy(coef, coef_in, (order + 1) * sizeof (double));
}
void Fir::
getCoef(double *coef_out) // returns filter coefs in coef_out
{
memcpy(coef_out, coef, (order + 1) * sizeof (double));
}
int Fir:: // filter in[size], and updates the state.
filter_noadjust(
short *in,
int size,
short *out)
{
if (size <= 0)
return (0);
double *in_buf = new double[size];
short2double(in_buf, in, size); // convert short input to double
int i;
int init_size = (size <= order)? size : order;
int init_order = order;
double *state_ptr = state;
short *out_ptr = out;
// the first "order" outputs need state in convolution
for (i = 1; i <= init_size; i++)
*out_ptr++ = double2short(convolve(coef, in_buf, i) +
convolve(coef + i, state_ptr++, init_order--));
// starting from "order + 1"th output, state is no longer needed
state_ptr = in_buf;
while (i++ <= size)
*out_ptr++ =
double2short(convolve(coef, state_ptr++, order + 1));
updateState(in_buf, size);
delete in_buf;
return (out_ptr - out);
}
int Fir::
getFlushSize(void)
{
int group_delay = (order + 1) >> 1;
return ((delay < group_delay)? group_delay - delay : 0);
}
int Fir::
flush(short *out) // zero input response of Fir
{
int num = getFlushSize();
if (num > 0) {
short *in = new short[num];
memset(in, 0, num * sizeof (short));
num = filter_noadjust(in, num, out);
delete in;
}
return (num);
}
/*
* filter() filters in[size] with filter delay adjusted to 0
*
* All FIR filters introduce a delay of "order" samples between input and
* output sequences. Most FIR filters are symmetric filters to keep the
* linear phase responses. For those FIR fitlers the group delay is
* "(order + 1) / 2". So filter_nodelay adjusts the group delay in the
* output sequence such that the output is aligned with the input and
* direct comparison between them is possible.
*
* The first call of filter returns "size - group_delay" output samples.
* After all the input samples have been filtered, filter() needs
* to be called with size = 0 to get the residual output samples to make
* the output sequence the same length as the input.
*
*/
int Fir::
filter(
short *in,
int size,
short *out)
{
if ((size <= 0) || (in == NULL))
return (flush(out));
else if (delay <= 0)
return (filter_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 (filter_noadjust(in, size, out));
}
}