xref: /illumos-gate/usr/src/cmd/audio/utilities/Fir.cc (revision 7c478bd9)
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 
34 extern "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  */
50 double
convolve(double * coefs,double * in_buf,int length)51 convolve(
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 
67 void				// convert short to double
short2double(double * out,short * in,int size)68 short2double(
69 	double *out,
70 	short *in,
71 	int size)
72 {
73 	while (size-- > 0)
74 		*out++ = (double)*in++;
75 }
76 
77 short
double2short(double in)78 double2short(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 
88 void Fir::				// update state with data[size]
updateState(double * data,int size)89 updateState(
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 
103 void Fir::
update_short(short * in,int size)104 update_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 
114 void Fir::
resetState(void)115 resetState(void)			// reset state to all zero
116 {
117 	for (int i = 0; i < order; i++)
118 		state[i] = 0.0;
119 }
120 
121 Fir::
Fir(void)122 Fir(void)
123 {
124 }
125 
126 Fir::
Fir(int order_in)127 Fir(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 
135 Fir::
~Fir()136 ~Fir()					// destruct Fir object
137 {
138 	delete coef;
139 	delete state;
140 }
141 
142 int Fir::
getOrder(void)143 getOrder(void)				// returns filter order
144 {
145 	return (order);
146 }
147 
148 int Fir::
getNumCoefs(void)149 getNumCoefs(void)			// returns number of filter coefficients
150 {
151 	return (order + 1);
152 }
153 
154 void Fir::
putCoef(double * coef_in)155 putCoef(double *coef_in)		// copy coef_in in filter coefficients
156 {
157 	memcpy(coef, coef_in, (order + 1) * sizeof (double));
158 }
159 
160 void Fir::
getCoef(double * coef_out)161 getCoef(double *coef_out)		// returns filter coefs in coef_out
162 {
163 	memcpy(coef_out, coef, (order + 1) * sizeof (double));
164 }
165 
166 int Fir::		// filter in[size], and updates the state.
filter_noadjust(short * in,int size,short * out)167 filter_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 
198 int Fir::
getFlushSize(void)199 getFlushSize(void)
200 {
201 	int group_delay = (order + 1) >> 1;
202 	return ((delay < group_delay)? group_delay - delay : 0);
203 }
204 
205 int Fir::
flush(short * out)206 flush(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 
235 int Fir::
filter(short * in,int size,short * out)236 filter(
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