Initial code package commit.
[apps/c55x-digital-mic-decimation.git] / src / BlkIir.c
1 /* ============================================================================
2  * Copyright (c) 2016 Texas Instruments Incorporated.
3  *
4  *  Redistribution and use in source and binary forms, with or without
5  *  modification, are permitted provided that the following conditions
6  *  are met:
7  *
8  *    Redistributions of source code must retain the above copyright
9  *    notice, this list of conditions and the following disclaimer.
10  *
11  *    Redistributions in binary form must reproduce the above copyright
12  *    notice, this list of conditions and the following disclaimer in the
13  *    documentation and/or other materials provided with the
14  *    distribution.
15  *
16  *    Neither the name of Texas Instruments Incorporated nor the names of
17  *    its contributors may be used to endorse or promote products derived
18  *    from this software without specific prior written permission.
19  *
20  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23  *  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
24  *  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
25  *  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
26  *  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
27  *  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
28  *  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
29  *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30  *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  *
32   ===============================================================================*/
34 #include "data_types.h"
35 #include "BlkIir.h"
37 #define QUANT_TRUNC         ( 0 )   /* truncate */
38 #define QUANT_RND_INF       ( 1 )   /* round to infinite */
39 #define QUANT_MODE          ( QUANT_RND_INF )
41 /* Block IIR, Direct Form II */
42 /* S18Q16 input and output data */
43 /* Fixed-point format of coefficients determined by coefficient integer wordlength parameter */
44 /* Input gain applied to input signal */
45 void blkIirDf2(
46     Int32   *inSamps,       /* input samples (S18Q16) */
47     Int16   *coefs,         /* filter coefficients (S16Q15) */
48     Int32   *outSamps,      /* output samples (S18Q16) */
49     Int32   *dlyBuf,        /* delay buffer */
50     Uint16  inGain,         /* input gain (U16Q16) */
51     Uint16  numInSamps,     /* number of input samples */
52     Uint16  numBiquads,     /* number of biquads */
53     Uint16  coefIWL         /* coefficient integer wordlength */
54 )
55 {
56     Uint16 numDlySamps;
57     Uint16 dataL;
58     Int16 dataH;
59     Uint32 uPrdLL;
60     Int32 prdLL, prdLH;
61     Int64 acc_40b;
62     Uint16 dlyBufIdx;
63     Uint16 coefIdx;
64     Uint16 i, j;
67     /* Compute number of delay buffer samples */
68     numDlySamps = 2*numBiquads;
70     /* Read current delay index */
71     dlyBufIdx = dlyBuf[0];
73     /* Initialize delay index */
74     dlyBufIdx++; // adjust index to 1->numDlySamps
75     if (dlyBufIdx > numDlySamps)
76     {
77         dlyBufIdx -= numDlySamps;
78     }
80     for (i = 0; i < numInSamps; i++)
81     {
82         coefIdx = 0;
84         //acc_40b = inSamps[i]<<15; // acc = x(n) (S18Q16->S33Q31)
86         // acc = G*x(n)
87         dataL = (Uint16)inSamps[i];
88         dataH = (Int16)(inSamps[i] >> 16);
89         /* S18Q16 * U16Q16 = S34Q32 */
90         uPrdLL = (Uint32)dataL * inGain;
91         prdLH = (Int32)dataH * (Uint32)inGain;
92         acc_40b = (Uint64)uPrdLL;
93         acc_40b += ((Int64)prdLH << 16);
94         
95         for (j = 0; j < numBiquads; j++)
96         {
97             /* Compute current biquad output */
99             /* Compute d(n) */
100             // acc = x(n) - a1*d(n-1)
101             dataL = (Uint16)dlyBuf[dlyBufIdx]; // d(n-1)
102             dataH = (Int16)(dlyBuf[dlyBufIdx] >> 16);
103             dlyBufIdx += numBiquads;
104             if (dlyBufIdx > numDlySamps)
105             {
106                 dlyBufIdx -= numDlySamps;
107             }
108             /* S18Q16 * S16Q(16-1-IWL) = S34Q(32-1-IWL) */
109             prdLL = (Uint32)dataL * (Int32)coefs[coefIdx]; // a1
110             prdLH = (Int32)dataH * coefs[coefIdx++];
111             /* S40Q31 + S34Q(32-1-IWL) = S40Q31 */
112             acc_40b -= ((Int64)prdLL << (1+coefIWL));
113             acc_40b -= ((Int64)prdLH << (16+1+coefIWL));
115             // acc = x(n) - a1*d(n-1) - a2*d(n-2)
116             dataL = (Uint16)dlyBuf[dlyBufIdx]; // d(n-2)
117             dataH = (Int16)(dlyBuf[dlyBufIdx] >> 16);
118             /* S18Q16 * S16Q(16-1-IWL) = S34Q(32-1-IWL) */
119             prdLL = (Uint32)dataL * (Int32)coefs[coefIdx]; //a2
120             prdLH = (Int32)dataH * coefs[coefIdx++];
121             /* S40Q31 + S34Q(32-1-IWL) = S40Q31 */
122             acc_40b -= ((Int64)prdLL << (1+coefIWL));
123             acc_40b -= ((Int64)prdLH << (16+1+coefIWL));
125             /* Update d(n) */
126 #if (QUANT_MODE == QUANT_RND_INF)
127             acc_40b += (Uint16)1<<(14+1); /* round to infinite */
128 #endif
129             dlyBuf[dlyBufIdx] = (Int32)(acc_40b>>(15+1));
131             /* Compute y(n) */
132             // acc = b2*d(n-2)
133             /* S18Q16 * S16Q(16-1-IWL) = S34Q(32-1-IWL) */
134             prdLL = (Uint32)dataL * (Int32)coefs[coefIdx]; // b2
135             prdLH = (Int32)dataH * coefs[coefIdx++];
136             /* S40Q31 + S34Q(32-1-IWL) = S40Q31 */
137             acc_40b = ((Int64)prdLL << (1+coefIWL));
138             acc_40b += ((Int64)prdLH << (16+1+coefIWL));
140             // acc = b0*d(n) + b2*d(n-2)
141             dataL = (Uint16)dlyBuf[dlyBufIdx]; // d(n)
142             dataH = (Int16)(dlyBuf[dlyBufIdx] >> 16);
143             dlyBufIdx += numBiquads;
144             if (dlyBufIdx > numDlySamps)
145             {
146                 dlyBufIdx -= numDlySamps;
147             }
148             /* S18Q16 * S16Q(16-1-IWL) = S34Q(32-1-IWL) */
149             prdLL = (Uint32)dataL * (Int32)coefs[coefIdx]; // b0
150             prdLH = (Int32)dataH * coefs[coefIdx++];
151             /* S40Q31 + S34Q(32-1-IWL) = S40Q31 */
152             acc_40b += ((Int64)prdLL << (1+coefIWL));
153             acc_40b += ((Int64)prdLH << (16+1+coefIWL));
155             // acc = b0*d(n) + b1*d(n-1) + b2*d(n-2)
156             dataL = (Uint16)dlyBuf[dlyBufIdx]; // d(n-1)
157             dataH = (Int16)(dlyBuf[dlyBufIdx] >> 16);
158             dlyBufIdx++;
159             if (dlyBufIdx > numDlySamps)
160             {
161                 dlyBufIdx -= numDlySamps;
162             }
163             /* S18Q16 * S16Q(16-1-IWL) = S34Q(32-1-IWL) */
164             prdLL = (Uint32)dataL * (Int32)coefs[coefIdx]; // b1
165             prdLH = (Int32)dataH * coefs[coefIdx++];
166             /* S40Q31 + S34Q(32-1-IWL) = S40Q31 */
167             acc_40b += ((Int64)prdLL << (1+coefIWL));
168             acc_40b += ((Int64)prdLH << (16+1+coefIWL));
169         }
171 #if (QUANT_MODE == QUANT_RND_INF)
172         acc_40b += 1<<14;
173 #endif
175         outSamps[i] = (Int32)acc_40b>>(15+1);
176     }
178     /* Update current delay index */
179     dlyBuf[0] = dlyBufIdx-1; // adjust index to 0->numDlySamps-1
182 /* Block IIR, Direct Form I */
183 /* S18Q16 input and output data */
184 /* Fixed-point format of coefficients determined by coefficient integer wordlength parameter */
185 /* Input gain applied to input signal */
186 void blkIirDf1(
187     Int32   *inSamps,       /* input samples (S18Q16) */
188     Int16   *coefs,         /* filter coefficients (S16Q15) */
189     Int32   *outSamps,      /* output samples (S18Q16) */
190     Int32   *dlyBuf,        /* delay buffer */
191     Uint16  inGain,         /* input gain (U16Q16) */
192     Uint16  numInSamps,     /* number of input samples */
193     Uint16  numBiquads,     /* number of biquads */
194     Uint16  coefIWL         /* coefficient integer wordlength */
197     Uint16 numDlySamps;
198     Uint16 dataL;
199     Int16 dataH;
200     Uint32 uPrdLL;
201     Int32 prdLL, prdLH;
202     Int64 acc_40b;
203     Int32 tIn;
204     Int32 *ffDlyBuf, *fbDlyBuf;
205     Uint16 ffDlyBufIdx; // feed forward delay index
206     Uint16 fbDlyBufIdx; // feed back delay index
207     Uint16 coefIdx;
208     Uint16 i, j;
211     /* Compute number of delay buffer samples */
212     numDlySamps = 2*numBiquads;
214     /* Initialize feed forward delay buffer pointer */
215     ffDlyBuf = &dlyBuf[1];
216     ffDlyBufIdx = dlyBuf[0];
217     /* Initialize feed back delay buffer pointer & index */
218     fbDlyBuf = &dlyBuf[numDlySamps+1];
219     fbDlyBufIdx = ffDlyBufIdx;
221     for (i = 0; i < numInSamps; i++)
222     {
223         coefIdx = 0;
225         // acc = x(n) (S18Q16->S33Q31)
226         //acc_40b = inSamps[i]<<15;
228         /* Compute G*x(n) */
229         // acc = G*x(n)
230         dataL = (Uint16)inSamps[i];
231         dataH = (Int16)(inSamps[i] >> 16);
232         /* S18Q16 * U16Q16 = S34Q32 */
233         uPrdLL = (Uint32)dataL * inGain;
234         prdLH = (Int32)dataH * (Uint32)inGain;
235         acc_40b = (Uint64)uPrdLL;
236         acc_40b += ((Int64)prdLH << 16);
238         /* Store G*x(n) */
239 #if (QUANT_MODE == QUANT_RND_INF)
240         acc_40b += (Uint16)1<<15; /* round to infinite */
241 #endif
242         tIn = (Int32)(acc_40b >> 16); // S18Q16
243         
244         for (j = 0; j < numBiquads; j++)
245         {
246             /* Compute current biquad output */
248             // acc = b0*x(n)
249             dataL = (Uint16)tIn; // x(n)
250             dataH = (Int16)(tIn >> 16);
251             /* S18Q16 * S16Q(16-1-IWL) = S34Q(32-1-IWL) */
252             prdLL = (Uint32)dataL * (Int32)coefs[coefIdx]; // b0
253             prdLH = (Int32)dataH * coefs[coefIdx++];
254             /* S40Q31 + S34Q(32-1-IWL) = S40Q31 */
255             acc_40b = ((Int64)prdLL << coefIWL);
256             acc_40b += ((Int64)prdLH << (16+coefIWL));
258             // acc = b0*x(n) + b1*x(n-1)
259             dataL = (Uint16)ffDlyBuf[ffDlyBufIdx]; // x(n-1)
260             dataH = (Int16)(ffDlyBuf[ffDlyBufIdx] >> 16);
261             /* S18Q16 * S16Q(16-1-IWL) = S34Q(32-1-IWL) */
262             prdLL = (Uint32)dataL * (Int32)coefs[coefIdx]; // b1
263             prdLH = (Int32)dataH * coefs[coefIdx++];
264             /* S40Q31 + S34Q(32-1-IWL) = S40Q31 */
265             acc_40b += ((Int64)prdLL << coefIWL);
266             acc_40b += ((Int64)prdLH << (16+coefIWL));
268             ffDlyBufIdx += numBiquads; // point to x(n-2D)
269             if (ffDlyBufIdx > numDlySamps-1)
270             {
271                 ffDlyBufIdx -= numDlySamps;
272             }
274             // acc = b0*x(n) + b1*x(n-1) + b2*x(n-2)
275             dataL = (Uint16)ffDlyBuf[ffDlyBufIdx]; // x(n-2)
276             dataH = (Int16)(ffDlyBuf[ffDlyBufIdx] >> 16);
277             /* S18Q16 * S16Q(16-1-IWL) = S34Q(32-1-IWL) */
278             prdLL = (Uint32)dataL * (Int32)coefs[coefIdx]; // b2
279             prdLH = (Int32)dataH * coefs[coefIdx++];
280             /* S40Q31 + S34Q(32-1-IWL) = S40Q31 */
281             acc_40b += ((Int64)prdLL << coefIWL);
282             acc_40b += ((Int64)prdLH << (16+coefIWL));
284             // update x(n-2)
285             ffDlyBuf[ffDlyBufIdx] = tIn;
287             ffDlyBufIdx += numBiquads+1; // point to x(n-1) for next biquad
288             if (ffDlyBufIdx > numDlySamps-1)
289             {
290                 ffDlyBufIdx -= numDlySamps;
291             }
293             // acc = b0*x(n) + b1*x(n-1) + b2*x(n-2) - a1*y(n-1)
294             dataL = (Uint16)fbDlyBuf[fbDlyBufIdx]; // y(n-1)
295             dataH = (Int16)(fbDlyBuf[fbDlyBufIdx] >> 16);
296             /* S18Q16 * S16Q(16-1-IWL) = S34Q(32-1-IWL) */
297             prdLL = (Uint32)dataL * (Int32)coefs[coefIdx]; // a1
298             prdLH = (Int32)dataH * coefs[coefIdx++];
299             /* S40Q31 + S34Q(32-1-IWL) = S40Q31 */
300             acc_40b -= ((Int64)prdLL << coefIWL);
301             acc_40b -= ((Int64)prdLH << (16+coefIWL));
303             fbDlyBufIdx += numBiquads; // point to y(n-2)
304             if (fbDlyBufIdx > numDlySamps-1)
305             {
306                 fbDlyBufIdx -= numDlySamps;
307             }
309             // acc = b0*x(n) + b1*x(n-1) + b2*x(n-2) - a1*y(n-1) - a2*y(n-2)
310             dataL = (Uint16)fbDlyBuf[fbDlyBufIdx]; // y(n-2)
311             dataH = (Int16)(fbDlyBuf[fbDlyBufIdx] >> 16);
312             /* S18Q16 * S16Q(16-1-IWL) = S34Q(32-1-IWL) */
313             prdLL = (Uint32)dataL * (Int32)coefs[coefIdx]; // a2
314             prdLH = (Int32)dataH * coefs[coefIdx++];
315             /* S40Q31 + S34Q(32-1-IWL) = S40Q31 */
316             acc_40b -= ((Int64)prdLL << coefIWL);
317             acc_40b -= ((Int64)prdLH << (16+coefIWL));
319 #if (QUANT_MODE == QUANT_RND_INF)
320             acc_40b += (Uint16)1<<14; /* round to infinite */
321 #endif
322             tIn = (Int32)(acc_40b >> 15); // S18Q16
324             // update y(n-2)
325             fbDlyBuf[fbDlyBufIdx] = tIn;
327             fbDlyBufIdx += numBiquads+1; // point to y(n-1) for next biquad
328             if (fbDlyBufIdx > numDlySamps-1)
329             {
330                 fbDlyBufIdx -= numDlySamps;
331             }
332         }
334         outSamps[i] = tIn;
335     }
337     /* Update current delay index */
338     dlyBuf[0] = ffDlyBufIdx;