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