usr/src/cmd/audio/utilities/Fir.cc

   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 }