1/*
2 * CDDL HEADER START
3 *
4 * The contents of this file are subject to the terms of the
5 * Common Development and Distribution License, Version 1.0 only
6 * (the "License").  You may not use this file except in compliance
7 * with the License.
8 *
9 * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
10 * or http://www.opensolaris.org/os/licensing.
11 * See the License for the specific language governing permissions
12 * and limitations under the License.
13 *
14 * When distributing Covered Code, include this CDDL HEADER in each
15 * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
16 * If applicable, add the following below this CDDL HEADER, with the
17 * fields enclosed by brackets "[]" replaced with your own identifying
18 * information: Portions Copyright [yyyy] [name of copyright owner]
19 *
20 * CDDL HEADER END
21 */
22/*
23 * Copyright (c) 1992-2001 by Sun Microsystems, Inc.
24 * All rights reserved.
25 */
26
27#pragma ident	"%Z%%M%	%I%	%E% SMI"
28
29#include <memory.h>
30#include <stddef.h>
31#include <sys/types.h>
32#include <Fir.h>
33
34extern "C" {
35	char *bcopy(char *, char *, int);
36	char *memmove(char *, char *, int);
37}
38
39#define	BCOPY(src, dest, num) memmove(dest, src, num)
40
41/*
42 * convolve()
43 * returns the convolution of coef[length] and in_buf[length]:
44 *
45 * convolution = coef[0] * in_buf[length - 1] +
46 *		 coef[1] * in_buf[length - 2] +
47 *		 ...
48 *		 coef[length - 1] * in_buf[0]
49 */
50double
51convolve(
52	double	*coefs,
53	double	*in_buf,
54	int	length)
55{
56	if (length <= 0)
57		return (0.0);
58	else {
59		in_buf += --length;
60		double sum = *coefs * *in_buf;
61		while (length--)
62			sum += *++coefs * *--in_buf;
63		return (sum);
64	}
65}
66
67void				// convert short to double
68short2double(
69	double *out,
70	short *in,
71	int size)
72{
73	while (size-- > 0)
74		*out++ = (double)*in++;
75}
76
77short
78double2short(double in)			// limit double to short
79{
80	if (in <= -32768.0)
81		return (-32768);
82	else if (in >= 32767.0)
83		return (32767);
84	else
85		return ((short)in);
86}
87
88void Fir::				// update state with data[size]
89updateState(
90	double	*data,
91	int	size)
92{
93	if (size >= order)
94		memcpy(state, data + size - order, order * sizeof (double));
95	else {
96		int old = order - size;
97		BCOPY((char *)(state + size), (char *)state,
98		    old * sizeof (double));
99		memcpy(state + order - size, data, size * sizeof (double));
100	}
101}
102
103void Fir::
104update_short(
105	short	*in,
106	int	size)
107{
108	double *in_buf = new double[size];
109	short2double(in_buf, in, size);
110	updateState(in_buf, size);
111	delete in_buf;
112}
113
114void Fir::
115resetState(void)			// reset state to all zero
116{
117	for (int i = 0; i < order; i++)
118		state[i] = 0.0;
119}
120
121Fir::
122Fir(void)
123{
124}
125
126Fir::
127Fir(int order_in): order(order_in)	// construct Fir object
128{
129	state = new double[order];
130	resetState();
131	coef = new double[order + 1];
132	delay = (order + 1) >> 1;	// assuming symmetric FIR
133}
134
135Fir::
136~Fir()					// destruct Fir object
137{
138	delete coef;
139	delete state;
140}
141
142int Fir::
143getOrder(void)				// returns filter order
144{
145	return (order);
146}
147
148int Fir::
149getNumCoefs(void)			// returns number of filter coefficients
150{
151	return (order + 1);
152}
153
154void Fir::
155putCoef(double *coef_in)		// copy coef_in in filter coefficients
156{
157	memcpy(coef, coef_in, (order + 1) * sizeof (double));
158}
159
160void Fir::
161getCoef(double *coef_out)		// returns filter coefs in coef_out
162{
163	memcpy(coef_out, coef, (order + 1) * sizeof (double));
164}
165
166int Fir::		// filter in[size], and updates the state.
167filter_noadjust(
168	short	*in,
169	int	size,
170	short	*out)
171{
172	if (size <= 0)
173		return (0);
174
175	double *in_buf = new double[size];
176	short2double(in_buf, in, size);		// convert short input to double
177	int	i;
178	int	init_size = (size <= order)? size : order;
179	int	init_order = order;
180	double	*state_ptr = state;
181	short	*out_ptr = out;
182
183	// the first "order" outputs need state in convolution
184	for (i = 1; i <= init_size; i++)
185		*out_ptr++ = double2short(convolve(coef, in_buf, i) +
186		    convolve(coef + i, state_ptr++, init_order--));
187
188	// starting from "order + 1"th output, state is no longer needed
189	state_ptr = in_buf;
190	while (i++ <= size)
191		*out_ptr++ =
192		    double2short(convolve(coef, state_ptr++, order + 1));
193	updateState(in_buf, size);
194	delete in_buf;
195	return (out_ptr - out);
196}
197
198int Fir::
199getFlushSize(void)
200{
201	int group_delay = (order + 1) >> 1;
202	return ((delay < group_delay)? group_delay - delay : 0);
203}
204
205int Fir::
206flush(short *out)		// zero input response of Fir
207{
208	int num = getFlushSize();
209	if (num > 0) {
210		short *in = new short[num];
211		memset(in, 0, num * sizeof (short));
212		num = filter_noadjust(in, num, out);
213		delete in;
214	}
215	return (num);
216}
217
218/*
219 * filter() filters in[size] with filter delay adjusted to 0
220 *
221 * All FIR filters introduce a delay of "order" samples between input and
222 * output sequences. Most FIR filters are symmetric filters to keep the
223 * linear phase responses. For those FIR fitlers the group delay is
224 * "(order + 1) / 2". So filter_nodelay adjusts the group delay in the
225 * output sequence such that the output is aligned with the input and
226 * direct comparison between them is possible.
227 *
228 * The first call of filter returns "size - group_delay" output samples.
229 * After all the input samples have been filtered, filter() needs
230 * to be called with size = 0 to get the residual output samples to make
231 * the output sequence the same length as the input.
232 *
233 */
234
235int Fir::
236filter(
237	short	*in,
238	int	size,
239	short	*out)
240{
241	if ((size <= 0) || (in == NULL))
242		return (flush(out));
243	else if (delay <= 0)
244		return (filter_noadjust(in, size, out));
245	else if (size <= delay) {
246		update_short(in, size);
247		delay -= size;
248		return (0);
249	} else {
250		update_short(in, delay);
251		in += delay;
252		size -= delay;
253		delay = 0;
254		return (filter_noadjust(in, size, out));
255	}
256}
257