xref: /illumos-gate/usr/src/cmd/audio/utilities/Fir.cc (revision 7cad4b8445359a5ca8a244d16d2a2bc8d555c340)
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 #include <memory.h>
28 #include <stddef.h>
29 #include <sys/types.h>
30 #include <Fir.h>
31 
32 extern "C" {
33 	char *bcopy(char *, char *, int);
34 	char *memmove(char *, char *, int);
35 }
36 
37 #define	BCOPY(src, dest, num) memmove(dest, src, num)
38 
39 /*
40  * convolve()
41  * returns the convolution of coef[length] and in_buf[length]:
42  *
43  * convolution = coef[0] * in_buf[length - 1] +
44  *		 coef[1] * in_buf[length - 2] +
45  *		 ...
46  *		 coef[length - 1] * in_buf[0]
47  */
48 double
convolve(double * coefs,double * in_buf,int length)49 convolve(
50 	double	*coefs,
51 	double	*in_buf,
52 	int	length)
53 {
54 	if (length <= 0)
55 		return (0.0);
56 	else {
57 		in_buf += --length;
58 		double sum = *coefs * *in_buf;
59 		while (length--)
60 			sum += *++coefs * *--in_buf;
61 		return (sum);
62 	}
63 }
64 
65 void				// convert short to double
short2double(double * out,short * in,int size)66 short2double(
67 	double *out,
68 	short *in,
69 	int size)
70 {
71 	while (size-- > 0)
72 		*out++ = (double)*in++;
73 }
74 
75 short
double2short(double in)76 double2short(double in)			// limit double to short
77 {
78 	if (in <= -32768.0)
79 		return (-32768);
80 	else if (in >= 32767.0)
81 		return (32767);
82 	else
83 		return ((short)in);
84 }
85 
86 void Fir::				// update state with data[size]
updateState(double * data,int size)87 updateState(
88 	double	*data,
89 	int	size)
90 {
91 	if (size >= order)
92 		memcpy(state, data + size - order, order * sizeof (double));
93 	else {
94 		int old = order - size;
95 		BCOPY((char *)(state + size), (char *)state,
96 		    old * sizeof (double));
97 		memcpy(state + order - size, data, size * sizeof (double));
98 	}
99 }
100 
101 void Fir::
update_short(short * in,int size)102 update_short(
103 	short	*in,
104 	int	size)
105 {
106 	double *in_buf = new double[size];
107 	short2double(in_buf, in, size);
108 	updateState(in_buf, size);
109 	delete[] in_buf;
110 }
111 
112 void Fir::
resetState(void)113 resetState(void)			// reset state to all zero
114 {
115 	for (int i = 0; i < order; i++)
116 		state[i] = 0.0;
117 }
118 
119 Fir::
Fir(void)120 Fir(void)
121 {
122 }
123 
124 Fir::
Fir(int order_in)125 Fir(int order_in): order(order_in)	// construct Fir object
126 {
127 	state = new double[order];
128 	resetState();
129 	coef = new double[order + 1];
130 	delay = (order + 1) >> 1;	// assuming symmetric FIR
131 }
132 
133 Fir::
~Fir()134 ~Fir()					// destruct Fir object
135 {
136 	delete coef;
137 	delete state;
138 }
139 
140 int Fir::
getOrder(void)141 getOrder(void)				// returns filter order
142 {
143 	return (order);
144 }
145 
146 int Fir::
getNumCoefs(void)147 getNumCoefs(void)			// returns number of filter coefficients
148 {
149 	return (order + 1);
150 }
151 
152 void Fir::
putCoef(double * coef_in)153 putCoef(double *coef_in)		// copy coef_in in filter coefficients
154 {
155 	memcpy(coef, coef_in, (order + 1) * sizeof (double));
156 }
157 
158 void Fir::
getCoef(double * coef_out)159 getCoef(double *coef_out)		// returns filter coefs in coef_out
160 {
161 	memcpy(coef_out, coef, (order + 1) * sizeof (double));
162 }
163 
164 int Fir::		// filter in[size], and updates the state.
filter_noadjust(short * in,int size,short * out)165 filter_noadjust(
166 	short	*in,
167 	int	size,
168 	short	*out)
169 {
170 	if (size <= 0)
171 		return (0);
172 
173 	double *in_buf = new double[size];
174 	short2double(in_buf, in, size);		// convert short input to double
175 	int	i;
176 	int	init_size = (size <= order)? size : order;
177 	int	init_order = order;
178 	double	*state_ptr = state;
179 	short	*out_ptr = out;
180 
181 	// the first "order" outputs need state in convolution
182 	for (i = 1; i <= init_size; i++)
183 		*out_ptr++ = double2short(convolve(coef, in_buf, i) +
184 		    convolve(coef + i, state_ptr++, init_order--));
185 
186 	// starting from "order + 1"th output, state is no longer needed
187 	state_ptr = in_buf;
188 	while (i++ <= size)
189 		*out_ptr++ =
190 		    double2short(convolve(coef, state_ptr++, order + 1));
191 	updateState(in_buf, size);
192 	delete[] in_buf;
193 	return (out_ptr - out);
194 }
195 
196 int Fir::
getFlushSize(void)197 getFlushSize(void)
198 {
199 	int group_delay = (order + 1) >> 1;
200 	return ((delay < group_delay)? group_delay - delay : 0);
201 }
202 
203 int Fir::
flush(short * out)204 flush(short *out)		// zero input response of Fir
205 {
206 	int num = getFlushSize();
207 	if (num > 0) {
208 		short *in = new short[num];
209 		memset(in, 0, num * sizeof (short));
210 		num = filter_noadjust(in, num, out);
211 		delete[] in;
212 	}
213 	return (num);
214 }
215 
216 /*
217  * filter() filters in[size] with filter delay adjusted to 0
218  *
219  * All FIR filters introduce a delay of "order" samples between input and
220  * output sequences. Most FIR filters are symmetric filters to keep the
221  * linear phase responses. For those FIR fitlers the group delay is
222  * "(order + 1) / 2". So filter_nodelay adjusts the group delay in the
223  * output sequence such that the output is aligned with the input and
224  * direct comparison between them is possible.
225  *
226  * The first call of filter returns "size - group_delay" output samples.
227  * After all the input samples have been filtered, filter() needs
228  * to be called with size = 0 to get the residual output samples to make
229  * the output sequence the same length as the input.
230  *
231  */
232 
233 int Fir::
filter(short * in,int size,short * out)234 filter(
235 	short	*in,
236 	int	size,
237 	short	*out)
238 {
239 	if ((size <= 0) || (in == NULL))
240 		return (flush(out));
241 	else if (delay <= 0)
242 		return (filter_noadjust(in, size, out));
243 	else if (size <= delay) {
244 		update_short(in, size);
245 		delay -= size;
246 		return (0);
247 	} else {
248 		update_short(in, delay);
249 		in += delay;
250 		size -= delay;
251 		delay = 0;
252 		return (filter_noadjust(in, size, out));
253 	}
254 }
255