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