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 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 68 short2double( 69 double *out, 70 short *in, 71 int size) 72 { 73 while (size-- > 0) 74 *out++ = (double)*in++; 75 } 76 77 short 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] 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:: 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:: 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:: 122 Fir(void) 123 { 124 } 125 126 Fir:: 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:: 136 ~Fir() // destruct Fir object 137 { 138 delete coef; 139 delete state; 140 } 141 142 int Fir:: 143 getOrder(void) // returns filter order 144 { 145 return (order); 146 } 147 148 int Fir:: 149 getNumCoefs(void) // returns number of filter coefficients 150 { 151 return (order + 1); 152 } 153 154 void Fir:: 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:: 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. 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:: 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:: 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:: 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