Initial code package commit.
[apps/c55x-digital-mic-decimation.git] / src / BlkFirDecim.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 "BlkFirDecim.h"
37 #define DECIM_FACT          ( 2 )   /* decimation factor */
39 #define QUANT_TRUNC         ( 0 )   /* truncate */
40 #define QUANT_RND_INF       ( 1 )   /* round to infinite */
41 #define QUANT_MODE          ( QUANT_RND_INF )
43 /* Block decimating FIR, */
44 /* S18Q16 input and output data, */
45 /* S16Q15 coefficients. */
46 /* Decimation factor fixed at 2. */
47 /* Computes two outputs per inner loop (assumes even number of outputs). */
48 void blkFirDecim2(
49     Int32   *inSamps,       /* input samples (S18Q16) */
50     Int16   *coefs,         /* filter coefficients (S16Q15) */
51     Int32   *outSamps,      /* output samples (S18Q16) */
52     Int32   *dlyBuf,        /* delay buffer */
53     Uint16  numInSamps,     /* number of input samples*/
54     Uint16  numCoefs        /* number of coefficients */
55 )
56 {
57     Uint16 numOutSamps;
58     Uint16 numDlySamps;
59     Uint16 dataL_1, dataL_2;
60     Int16 dataH_1, dataH_2;
61     Int32 prdLL_1, prdLL_2;
62     Int32 prdLH_1, prdLH_2;
63     Int64 acc0_40b, acc1_40b, acc2_40b, acc3_40b;
64     Uint16 dlyBufIdx_1; // leading index
65     Uint16 dlyBufIdx_2; // lagging index
66     Uint16 inSampIdx, outSampIdx;
67     Uint16 outSampCnt;
68     Uint16 i;
70     /* Compute number of output samples */
71     numOutSamps = numInSamps>>1;
73     /* Compute number of delay buffer samples */
74     numDlySamps = numCoefs+2;
76     /* Read delay index of oldest sample */
77     dlyBufIdx_2 = dlyBuf[0]; // index of oldest sample stored in 0th location of delay buffer
79     /* Initialize lagging delay index */
80     dlyBufIdx_2++; // adjust index to 1->numDlySamps
81     if (dlyBufIdx_2 > numDlySamps)
82     {
83         dlyBufIdx_2 = 1;
84     }
86     /* Initialize leading delay index */
87     dlyBufIdx_1 = dlyBufIdx_2;
88     dlyBufIdx_1--;
89     if (dlyBufIdx_1 < 1)
90     {
91         dlyBufIdx_1 = numDlySamps;
92     }
94     inSampIdx = 0;
95     outSampIdx = 0;
96     for (outSampCnt = 0; outSampCnt < numOutSamps/2; outSampCnt++) 
97     {
98         /* Place 1 sample in delay line at lagging index */
99         dlyBuf[dlyBufIdx_2] = inSamps[inSampIdx++];
101         /* Place D=2 samples in delay line at leading index */
102         dlyBuf[dlyBufIdx_1] = inSamps[inSampIdx++];
103         dlyBufIdx_1--;
104         if (dlyBufIdx_1 < 1)
105         {
106             dlyBufIdx_1 = numDlySamps;
107         }
108         dlyBuf[dlyBufIdx_1] = inSamps[inSampIdx++];
110         /* Compute lagging output */
111         /* S18Q16 */
112         dataL_2 = (Uint16)dlyBuf[dlyBufIdx_2];
113         dataH_2 = (Int16)(dlyBuf[dlyBufIdx_2++] >> 16);
114         if (dlyBufIdx_2 > numDlySamps)
115         {
116             dlyBufIdx_2 = 1;
117         }
118         /* S18Q16 * S16Q15 = S34Q31 */
119         prdLL_2 = (Uint32)dataL_2 * (Int32)coefs[0];
120         prdLH_2 = (Int32)dataH_2 * coefs[0]; 
121         /* S40Q31 + S34Q31 = S40Q31 */
122         acc2_40b = (Int64)prdLL_2;
123         acc3_40b = (Int64)prdLH_2;
125         /* Compute leading output */
126         /* S18Q16 */
127         dataL_1 = (Uint16)dlyBuf[dlyBufIdx_1];
128         dataH_1 = (Int16)(dlyBuf[dlyBufIdx_1++] >> 16);
129         if (dlyBufIdx_1 > numDlySamps)
130         {
131             dlyBufIdx_1 = 1;
132         }
133         /* S18Q16 * S16Q15 = S34Q31 */
134         prdLL_1 = (Uint32)dataL_1 * (Int32)coefs[0];
135         prdLH_1 = (Int32)dataH_1 * coefs[0]; 
136         /* S40Q31 + S34Q31 = S40Q31 */
137         acc0_40b = (Int64)prdLL_1;
138         acc1_40b = (Int64)prdLH_1;
140         for (i = 1; i < numCoefs-1; i++)
141         {
142             /* Compute lagging output */
143             /* S18Q16 */
144             dataL_2 = (Uint16)dlyBuf[dlyBufIdx_2];
145             dataH_2 = (Int16)(dlyBuf[dlyBufIdx_2++] >> 16);
146             if (dlyBufIdx_2 > numDlySamps)
147             {
148                 dlyBufIdx_2 = 1;
149             }
150             /* S18Q16 * S16Q15 = S34Q31 */
151             prdLL_2 = (Uint32)dataL_2 * (Int32)coefs[i];
152             prdLH_2 = (Int32)dataH_2 * coefs[i]; 
153             /* S40Q31 + S34Q31 = S40Q31 */
154             acc2_40b += (Int64)prdLL_2;
155             acc3_40b += (Int64)prdLH_2;
157             /* Compute leading output */
158             /* S18Q16 */
159             dataL_1 = (Uint16)dlyBuf[dlyBufIdx_1];
160             dataH_1 = (Int16)(dlyBuf[dlyBufIdx_1++] >> 16);
161             if (dlyBufIdx_1 > numDlySamps)
162             {
163                 dlyBufIdx_1 = 1;
164             }
165             /* S18Q16 * S16Q15 = S34Q31 */
166             prdLL_1 = (Uint32)dataL_1 * (Int32)coefs[i];
167             prdLH_1 = (Int32)dataH_1 * coefs[i]; 
168             /* S40Q31 + S34Q31 = S40Q31 */
169             acc0_40b += (Int64)prdLL_1;
170             acc1_40b += (Int64)prdLH_1;
171         }
173         /* Compute lagging output */
174         /* S18Q16 */
175         dataL_2 = (Uint16)dlyBuf[dlyBufIdx_2];
176         dataH_2 = (Int16)(dlyBuf[dlyBufIdx_2] >> 16);
177         if (dlyBufIdx_2 > numDlySamps)
178         {
179             dlyBufIdx_2 = 1;
180         }
181         /* S18Q16 * S16Q15 = S34Q31 */
182         prdLL_2 = (Uint32)dataL_2 * (Int32)coefs[numCoefs-1];
183         prdLH_2 = (Int32)dataH_2 * coefs[numCoefs-1]; 
184         /* S40Q31 + S34Q31 = S40Q31 */
185         acc2_40b += (Int64)prdLL_2;
186         acc3_40b += (Int64)prdLH_2;
188         /* Compute leading output */
189         /* S18Q16 */
190         dataL_1 = (Uint16)dlyBuf[dlyBufIdx_1];
191         dataH_1 = (Int16)(dlyBuf[dlyBufIdx_1] >> 16);
192         if (dlyBufIdx_1 > numDlySamps)
193         {
194             dlyBufIdx_1 = 1;
195         }
196         /* S18Q16 * S16Q15 = S34Q31 */
197         prdLL_1 = (Uint32)dataL_1 * (Int32)coefs[numCoefs-1];
198         prdLH_1 = (Int32)dataH_1 * coefs[numCoefs-1]; 
199         /* S40Q31 + S34Q31 = S40Q31 */
200         acc0_40b += (Int64)prdLL_1;
201         acc1_40b += (Int64)prdLH_1;
203         acc2_40b += acc3_40b << 16;
204         acc0_40b += acc1_40b << 16;
206 #if (QUANT_MODE == QUANT_RND_INF)
207         acc2_40b += (Uint16)1<<14; /* round to infinite */
208         acc0_40b += (Uint16)1<<14; /* round to infinite */
209 #endif
210         acc2_40b >>= 15; /* truncate */
211         outSamps[outSampIdx++] = (Int32)acc2_40b;
212         acc0_40b >>= 15; /* truncate */
213         outSamps[outSampIdx++] = (Int32)acc0_40b;
215         /* Place D-1=1 samples in delay line at lagging index */
216         dlyBuf[dlyBufIdx_2] = inSamps[inSampIdx++];
217         dlyBufIdx_2--;
218         if (dlyBufIdx_2 < 1)
219         {
220             dlyBufIdx_2 = numDlySamps;
221         }
222     }
224     /* Write delay index of oldest sample */
225     dlyBuf[0] = dlyBufIdx_2-1; // adjust index to 0->numDlySamps-1