Initial code package commit.
[apps/c55x-digital-mic-decimation.git] / src / pick_bits_cic.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   ===============================================================================*/
33 #include "data_types.h"
34 #include "pick_bits_cic.h"
36 #define BITS_PER_16BW  ( 16 )
38 /* Unpacks "left" and "right" 32-bit packed DMA buffers containing output from digital mic. */
39 /* Performs CIC on unpacked data, CIC_DF = 16 & CIC_NS = 4. */
40 void pickBitsCic(
41     Uint32 *lData,          /* "left" channel 32-bit packed input data */
42     Uint32 *rData,          /* "right" channel 32-bit packed input data */
43     Uint16 inDataLen,       /* length of "left" or "right" input data in 32-bit words */
44     Int32 *cicState,        /* CIC state. First NS values are integrator state, next NS values are differentiator delay buffer */
45     Int32 *outSamps,        /* CIC output samples */
46     Uint16 *pNumOutSamps    /* CIC number of output samples */
47 )
48 {
49     Uint16 cur16bW;
50     Int16 input;
51     Int32 *acc;
52     Int32 *diffDly;
53     Int32 diff[CIC_NS];
54     Int32 *pOutSamp;
55     Uint16 i, j;
58     /* Compute number of output samples */
59     /* x32 for 32-bit word, x2 for 2 channels */
60     //*pNumOutSamps = inDataLen*32*2/DS;
61     *pNumOutSamps = inDataLen<<2;
63     acc = &cicState[0];
64     diffDly = &cicState[CIC_NS];
65     pOutSamp = &outSamps[0];
66     for (i = 0; i < inDataLen; i++)
67     {
68         /* Process "left" channel MS 16-bit word for current 32-bit word */
69         cur16bW = lData[i]>>BITS_PER_16BW;
70         for (j = 0; j < CIC_DF; j++)
71         {
72             /* Get current input */
73             /* 0->-1, 1->+1 */
74             input = ((cur16bW>>(BITS_PER_16BW-2))&0x2) - 1;
75             cur16bW <<= 1;
77             /* Perform integration for current input */
78             acc[0] += input;
79             acc[1] += acc[0];
80             acc[2] += acc[1];
81             acc[3] += acc[2];
82         }
84         /* Perform decimation & differentiator stages */
85         diff[0] = acc[3] - diffDly[0];
86         diffDly[0] = acc[3];
87         diff[1] = diff[0] - diffDly[1];
88         diffDly[1] = diff[0];
89         diff[2] = diff[1] - diffDly[2];
90         diffDly[2] = diff[1];
91         diff[3] = diff[2] - diffDly[3];
92         diffDly[3] = diff[2];
94         /* Write output sample */
95         *pOutSamp++ = diff[3];
97         /* Process "left" channel LS 16-bit word for current 32-bit word */
98         cur16bW = lData[i]&0xFFFF;
99         for (j = 0; j < CIC_DF; j++)
100         {
101             /* Get current input */
102             /* 0->-1, 1->+1 */
103             input = ((cur16bW>>(BITS_PER_16BW-2))&0x2) - 1;
104             cur16bW <<= 1;
106             /* Perform integration for current input */
107             acc[0] += input;
108             acc[1] += acc[0];
109             acc[2] += acc[1];
110             acc[3] += acc[2];
111         }
113         /* Perform decimation & differentiator stages */
114         diff[0] = acc[3] - diffDly[0];
115         diffDly[0] = acc[3];
116         diff[1] = diff[0] - diffDly[1];
117         diffDly[1] = diff[0];
118         diff[2] = diff[1] - diffDly[2];
119         diffDly[2] = diff[1];
120         diff[3] = diff[2] - diffDly[3];
121         diffDly[3] = diff[2];
123         /* Write output sample */
124         *pOutSamp++ = diff[3];
126         /* Process "right" channel MS 16-bit word for current 32-bit word */
127         cur16bW = rData[i]>>16;
128         for (j = 0; j < CIC_DF; j++)
129         {
130             /* Get current input */
131             /* 0->-1, 1->+1 */
132             input = ((cur16bW>>(BITS_PER_16BW-2))&0x2) - 1;
133             cur16bW <<= 1;
135             /* Perform integration for current input */
136             acc[0] += input;
137             acc[1] += acc[0];
138             acc[2] += acc[1];
139             acc[3] += acc[2];
140         }
142         /* Perform decimation & differentiator stages */
143         diff[0] = acc[3] - diffDly[0];
144         diffDly[0] = acc[3];
145         diff[1] = diff[0] - diffDly[1];
146         diffDly[1] = diff[0];
147         diff[2] = diff[1] - diffDly[2];
148         diffDly[2] = diff[1];
149         diff[3] = diff[2] - diffDly[3];
150         diffDly[3] = diff[2];
152         /* Write output sample */
153         *pOutSamp++ = diff[3];
155         /* Process "right" channel LS 16-bit word for current 32-bit word */
156         cur16bW = rData[i]&0xFFFF;
157         for (j = 0; j < CIC_DF; j++)
158         {
159             /* Get current input */
160             /* 0->-1, 1->+1 */
161             input = ((cur16bW>>(BITS_PER_16BW-2))&0x2) - 1;
162             cur16bW <<= 1;
164             /* Perform integration for current input */
165             acc[0] += input;
166             acc[1] += acc[0];
167             acc[2] += acc[1];
168             acc[3] += acc[2];
169         }
171         /* Perform decimation & differentiator stages */
172         diff[0] = acc[3] - diffDly[0];
173         diffDly[0] = acc[3];
174         diff[1] = diff[0] - diffDly[1];
175         diffDly[1] = diff[0];
176         diff[2] = diff[1] - diffDly[2];
177         diffDly[2] = diff[1];
178         diff[3] = diff[2] - diffDly[3];
179         diffDly[3] = diff[2];
181         /* Write output sample */
182         *pOutSamp++ = diff[3];
183     }