17c478bd9Sstevel@tonic-gate /*
27c478bd9Sstevel@tonic-gate * CDDL HEADER START
37c478bd9Sstevel@tonic-gate *
47c478bd9Sstevel@tonic-gate * The contents of this file are subject to the terms of the
57c478bd9Sstevel@tonic-gate * Common Development and Distribution License, Version 1.0 only
67c478bd9Sstevel@tonic-gate * (the "License"). You may not use this file except in compliance
77c478bd9Sstevel@tonic-gate * with the License.
87c478bd9Sstevel@tonic-gate *
97c478bd9Sstevel@tonic-gate * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
107c478bd9Sstevel@tonic-gate * or http://www.opensolaris.org/os/licensing.
117c478bd9Sstevel@tonic-gate * See the License for the specific language governing permissions
127c478bd9Sstevel@tonic-gate * and limitations under the License.
137c478bd9Sstevel@tonic-gate *
147c478bd9Sstevel@tonic-gate * When distributing Covered Code, include this CDDL HEADER in each
157c478bd9Sstevel@tonic-gate * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
167c478bd9Sstevel@tonic-gate * If applicable, add the following below this CDDL HEADER, with the
177c478bd9Sstevel@tonic-gate * fields enclosed by brackets "[]" replaced with your own identifying
187c478bd9Sstevel@tonic-gate * information: Portions Copyright [yyyy] [name of copyright owner]
197c478bd9Sstevel@tonic-gate *
207c478bd9Sstevel@tonic-gate * CDDL HEADER END
217c478bd9Sstevel@tonic-gate */
227c478bd9Sstevel@tonic-gate /*
237c478bd9Sstevel@tonic-gate * Copyright (c) 1992-2001 by Sun Microsystems, Inc.
247c478bd9Sstevel@tonic-gate * All rights reserved.
257c478bd9Sstevel@tonic-gate */
267c478bd9Sstevel@tonic-gate
277c478bd9Sstevel@tonic-gate #include <memory.h>
287c478bd9Sstevel@tonic-gate #include <stddef.h>
297c478bd9Sstevel@tonic-gate #include <sys/types.h>
307c478bd9Sstevel@tonic-gate #include <Fir.h>
317c478bd9Sstevel@tonic-gate
327c478bd9Sstevel@tonic-gate extern "C" {
337c478bd9Sstevel@tonic-gate char *bcopy(char *, char *, int);
347c478bd9Sstevel@tonic-gate char *memmove(char *, char *, int);
357c478bd9Sstevel@tonic-gate }
367c478bd9Sstevel@tonic-gate
377c478bd9Sstevel@tonic-gate #define BCOPY(src, dest, num) memmove(dest, src, num)
387c478bd9Sstevel@tonic-gate
397c478bd9Sstevel@tonic-gate /*
407c478bd9Sstevel@tonic-gate * convolve()
417c478bd9Sstevel@tonic-gate * returns the convolution of coef[length] and in_buf[length]:
427c478bd9Sstevel@tonic-gate *
437c478bd9Sstevel@tonic-gate * convolution = coef[0] * in_buf[length - 1] +
447c478bd9Sstevel@tonic-gate * coef[1] * in_buf[length - 2] +
457c478bd9Sstevel@tonic-gate * ...
467c478bd9Sstevel@tonic-gate * coef[length - 1] * in_buf[0]
477c478bd9Sstevel@tonic-gate */
487c478bd9Sstevel@tonic-gate double
convolve(double * coefs,double * in_buf,int length)497c478bd9Sstevel@tonic-gate convolve(
507c478bd9Sstevel@tonic-gate double *coefs,
517c478bd9Sstevel@tonic-gate double *in_buf,
527c478bd9Sstevel@tonic-gate int length)
537c478bd9Sstevel@tonic-gate {
547c478bd9Sstevel@tonic-gate if (length <= 0)
557c478bd9Sstevel@tonic-gate return (0.0);
567c478bd9Sstevel@tonic-gate else {
577c478bd9Sstevel@tonic-gate in_buf += --length;
587c478bd9Sstevel@tonic-gate double sum = *coefs * *in_buf;
597c478bd9Sstevel@tonic-gate while (length--)
607c478bd9Sstevel@tonic-gate sum += *++coefs * *--in_buf;
617c478bd9Sstevel@tonic-gate return (sum);
627c478bd9Sstevel@tonic-gate }
637c478bd9Sstevel@tonic-gate }
647c478bd9Sstevel@tonic-gate
657c478bd9Sstevel@tonic-gate void // convert short to double
short2double(double * out,short * in,int size)667c478bd9Sstevel@tonic-gate short2double(
677c478bd9Sstevel@tonic-gate double *out,
687c478bd9Sstevel@tonic-gate short *in,
697c478bd9Sstevel@tonic-gate int size)
707c478bd9Sstevel@tonic-gate {
717c478bd9Sstevel@tonic-gate while (size-- > 0)
727c478bd9Sstevel@tonic-gate *out++ = (double)*in++;
737c478bd9Sstevel@tonic-gate }
747c478bd9Sstevel@tonic-gate
757c478bd9Sstevel@tonic-gate short
double2short(double in)767c478bd9Sstevel@tonic-gate double2short(double in) // limit double to short
777c478bd9Sstevel@tonic-gate {
787c478bd9Sstevel@tonic-gate if (in <= -32768.0)
797c478bd9Sstevel@tonic-gate return (-32768);
807c478bd9Sstevel@tonic-gate else if (in >= 32767.0)
817c478bd9Sstevel@tonic-gate return (32767);
827c478bd9Sstevel@tonic-gate else
837c478bd9Sstevel@tonic-gate return ((short)in);
847c478bd9Sstevel@tonic-gate }
857c478bd9Sstevel@tonic-gate
867c478bd9Sstevel@tonic-gate void Fir:: // update state with data[size]
updateState(double * data,int size)877c478bd9Sstevel@tonic-gate updateState(
887c478bd9Sstevel@tonic-gate double *data,
897c478bd9Sstevel@tonic-gate int size)
907c478bd9Sstevel@tonic-gate {
917c478bd9Sstevel@tonic-gate if (size >= order)
927c478bd9Sstevel@tonic-gate memcpy(state, data + size - order, order * sizeof (double));
937c478bd9Sstevel@tonic-gate else {
947c478bd9Sstevel@tonic-gate int old = order - size;
957c478bd9Sstevel@tonic-gate BCOPY((char *)(state + size), (char *)state,
967c478bd9Sstevel@tonic-gate old * sizeof (double));
977c478bd9Sstevel@tonic-gate memcpy(state + order - size, data, size * sizeof (double));
987c478bd9Sstevel@tonic-gate }
997c478bd9Sstevel@tonic-gate }
1007c478bd9Sstevel@tonic-gate
1017c478bd9Sstevel@tonic-gate void Fir::
update_short(short * in,int size)1027c478bd9Sstevel@tonic-gate update_short(
1037c478bd9Sstevel@tonic-gate short *in,
1047c478bd9Sstevel@tonic-gate int size)
1057c478bd9Sstevel@tonic-gate {
1067c478bd9Sstevel@tonic-gate double *in_buf = new double[size];
1077c478bd9Sstevel@tonic-gate short2double(in_buf, in, size);
1087c478bd9Sstevel@tonic-gate updateState(in_buf, size);
109*7cad4b84SToomas Soome delete[] in_buf;
1107c478bd9Sstevel@tonic-gate }
1117c478bd9Sstevel@tonic-gate
1127c478bd9Sstevel@tonic-gate void Fir::
resetState(void)1137c478bd9Sstevel@tonic-gate resetState(void) // reset state to all zero
1147c478bd9Sstevel@tonic-gate {
1157c478bd9Sstevel@tonic-gate for (int i = 0; i < order; i++)
1167c478bd9Sstevel@tonic-gate state[i] = 0.0;
1177c478bd9Sstevel@tonic-gate }
1187c478bd9Sstevel@tonic-gate
1197c478bd9Sstevel@tonic-gate Fir::
Fir(void)1207c478bd9Sstevel@tonic-gate Fir(void)
1217c478bd9Sstevel@tonic-gate {
1227c478bd9Sstevel@tonic-gate }
1237c478bd9Sstevel@tonic-gate
1247c478bd9Sstevel@tonic-gate Fir::
Fir(int order_in)1257c478bd9Sstevel@tonic-gate Fir(int order_in): order(order_in) // construct Fir object
1267c478bd9Sstevel@tonic-gate {
1277c478bd9Sstevel@tonic-gate state = new double[order];
1287c478bd9Sstevel@tonic-gate resetState();
1297c478bd9Sstevel@tonic-gate coef = new double[order + 1];
1307c478bd9Sstevel@tonic-gate delay = (order + 1) >> 1; // assuming symmetric FIR
1317c478bd9Sstevel@tonic-gate }
1327c478bd9Sstevel@tonic-gate
1337c478bd9Sstevel@tonic-gate Fir::
~Fir()1347c478bd9Sstevel@tonic-gate ~Fir() // destruct Fir object
1357c478bd9Sstevel@tonic-gate {
1367c478bd9Sstevel@tonic-gate delete coef;
1377c478bd9Sstevel@tonic-gate delete state;
1387c478bd9Sstevel@tonic-gate }
1397c478bd9Sstevel@tonic-gate
1407c478bd9Sstevel@tonic-gate int Fir::
getOrder(void)1417c478bd9Sstevel@tonic-gate getOrder(void) // returns filter order
1427c478bd9Sstevel@tonic-gate {
1437c478bd9Sstevel@tonic-gate return (order);
1447c478bd9Sstevel@tonic-gate }
1457c478bd9Sstevel@tonic-gate
1467c478bd9Sstevel@tonic-gate int Fir::
getNumCoefs(void)1477c478bd9Sstevel@tonic-gate getNumCoefs(void) // returns number of filter coefficients
1487c478bd9Sstevel@tonic-gate {
1497c478bd9Sstevel@tonic-gate return (order + 1);
1507c478bd9Sstevel@tonic-gate }
1517c478bd9Sstevel@tonic-gate
1527c478bd9Sstevel@tonic-gate void Fir::
putCoef(double * coef_in)1537c478bd9Sstevel@tonic-gate putCoef(double *coef_in) // copy coef_in in filter coefficients
1547c478bd9Sstevel@tonic-gate {
1557c478bd9Sstevel@tonic-gate memcpy(coef, coef_in, (order + 1) * sizeof (double));
1567c478bd9Sstevel@tonic-gate }
1577c478bd9Sstevel@tonic-gate
1587c478bd9Sstevel@tonic-gate void Fir::
getCoef(double * coef_out)1597c478bd9Sstevel@tonic-gate getCoef(double *coef_out) // returns filter coefs in coef_out
1607c478bd9Sstevel@tonic-gate {
1617c478bd9Sstevel@tonic-gate memcpy(coef_out, coef, (order + 1) * sizeof (double));
1627c478bd9Sstevel@tonic-gate }
1637c478bd9Sstevel@tonic-gate
1647c478bd9Sstevel@tonic-gate int Fir:: // filter in[size], and updates the state.
filter_noadjust(short * in,int size,short * out)1657c478bd9Sstevel@tonic-gate filter_noadjust(
1667c478bd9Sstevel@tonic-gate short *in,
1677c478bd9Sstevel@tonic-gate int size,
1687c478bd9Sstevel@tonic-gate short *out)
1697c478bd9Sstevel@tonic-gate {
1707c478bd9Sstevel@tonic-gate if (size <= 0)
1717c478bd9Sstevel@tonic-gate return (0);
1727c478bd9Sstevel@tonic-gate
1737c478bd9Sstevel@tonic-gate double *in_buf = new double[size];
1747c478bd9Sstevel@tonic-gate short2double(in_buf, in, size); // convert short input to double
1757c478bd9Sstevel@tonic-gate int i;
1767c478bd9Sstevel@tonic-gate int init_size = (size <= order)? size : order;
1777c478bd9Sstevel@tonic-gate int init_order = order;
1787c478bd9Sstevel@tonic-gate double *state_ptr = state;
1797c478bd9Sstevel@tonic-gate short *out_ptr = out;
1807c478bd9Sstevel@tonic-gate
1817c478bd9Sstevel@tonic-gate // the first "order" outputs need state in convolution
1827c478bd9Sstevel@tonic-gate for (i = 1; i <= init_size; i++)
1837c478bd9Sstevel@tonic-gate *out_ptr++ = double2short(convolve(coef, in_buf, i) +
1847c478bd9Sstevel@tonic-gate convolve(coef + i, state_ptr++, init_order--));
1857c478bd9Sstevel@tonic-gate
1867c478bd9Sstevel@tonic-gate // starting from "order + 1"th output, state is no longer needed
1877c478bd9Sstevel@tonic-gate state_ptr = in_buf;
1887c478bd9Sstevel@tonic-gate while (i++ <= size)
1897c478bd9Sstevel@tonic-gate *out_ptr++ =
1907c478bd9Sstevel@tonic-gate double2short(convolve(coef, state_ptr++, order + 1));
1917c478bd9Sstevel@tonic-gate updateState(in_buf, size);
192*7cad4b84SToomas Soome delete[] in_buf;
1937c478bd9Sstevel@tonic-gate return (out_ptr - out);
1947c478bd9Sstevel@tonic-gate }
1957c478bd9Sstevel@tonic-gate
1967c478bd9Sstevel@tonic-gate int Fir::
getFlushSize(void)1977c478bd9Sstevel@tonic-gate getFlushSize(void)
1987c478bd9Sstevel@tonic-gate {
1997c478bd9Sstevel@tonic-gate int group_delay = (order + 1) >> 1;
2007c478bd9Sstevel@tonic-gate return ((delay < group_delay)? group_delay - delay : 0);
2017c478bd9Sstevel@tonic-gate }
2027c478bd9Sstevel@tonic-gate
2037c478bd9Sstevel@tonic-gate int Fir::
flush(short * out)2047c478bd9Sstevel@tonic-gate flush(short *out) // zero input response of Fir
2057c478bd9Sstevel@tonic-gate {
2067c478bd9Sstevel@tonic-gate int num = getFlushSize();
2077c478bd9Sstevel@tonic-gate if (num > 0) {
2087c478bd9Sstevel@tonic-gate short *in = new short[num];
2097c478bd9Sstevel@tonic-gate memset(in, 0, num * sizeof (short));
2107c478bd9Sstevel@tonic-gate num = filter_noadjust(in, num, out);
211*7cad4b84SToomas Soome delete[] in;
2127c478bd9Sstevel@tonic-gate }
2137c478bd9Sstevel@tonic-gate return (num);
2147c478bd9Sstevel@tonic-gate }
2157c478bd9Sstevel@tonic-gate
2167c478bd9Sstevel@tonic-gate /*
2177c478bd9Sstevel@tonic-gate * filter() filters in[size] with filter delay adjusted to 0
2187c478bd9Sstevel@tonic-gate *
2197c478bd9Sstevel@tonic-gate * All FIR filters introduce a delay of "order" samples between input and
2207c478bd9Sstevel@tonic-gate * output sequences. Most FIR filters are symmetric filters to keep the
2217c478bd9Sstevel@tonic-gate * linear phase responses. For those FIR fitlers the group delay is
2227c478bd9Sstevel@tonic-gate * "(order + 1) / 2". So filter_nodelay adjusts the group delay in the
2237c478bd9Sstevel@tonic-gate * output sequence such that the output is aligned with the input and
2247c478bd9Sstevel@tonic-gate * direct comparison between them is possible.
2257c478bd9Sstevel@tonic-gate *
2267c478bd9Sstevel@tonic-gate * The first call of filter returns "size - group_delay" output samples.
2277c478bd9Sstevel@tonic-gate * After all the input samples have been filtered, filter() needs
2287c478bd9Sstevel@tonic-gate * to be called with size = 0 to get the residual output samples to make
2297c478bd9Sstevel@tonic-gate * the output sequence the same length as the input.
2307c478bd9Sstevel@tonic-gate *
2317c478bd9Sstevel@tonic-gate */
2327c478bd9Sstevel@tonic-gate
2337c478bd9Sstevel@tonic-gate int Fir::
filter(short * in,int size,short * out)2347c478bd9Sstevel@tonic-gate filter(
2357c478bd9Sstevel@tonic-gate short *in,
2367c478bd9Sstevel@tonic-gate int size,
2377c478bd9Sstevel@tonic-gate short *out)
2387c478bd9Sstevel@tonic-gate {
2397c478bd9Sstevel@tonic-gate if ((size <= 0) || (in == NULL))
2407c478bd9Sstevel@tonic-gate return (flush(out));
2417c478bd9Sstevel@tonic-gate else if (delay <= 0)
2427c478bd9Sstevel@tonic-gate return (filter_noadjust(in, size, out));
2437c478bd9Sstevel@tonic-gate else if (size <= delay) {
2447c478bd9Sstevel@tonic-gate update_short(in, size);
2457c478bd9Sstevel@tonic-gate delay -= size;
2467c478bd9Sstevel@tonic-gate return (0);
2477c478bd9Sstevel@tonic-gate } else {
2487c478bd9Sstevel@tonic-gate update_short(in, delay);
2497c478bd9Sstevel@tonic-gate in += delay;
2507c478bd9Sstevel@tonic-gate size -= delay;
2517c478bd9Sstevel@tonic-gate delay = 0;
2527c478bd9Sstevel@tonic-gate return (filter_noadjust(in, size, out));
2537c478bd9Sstevel@tonic-gate }
2547c478bd9Sstevel@tonic-gate }
255