查看 | 历史 | 提交 | 提交对比
QuakeGod
2022-10-17 6a62617e99d79314d1e10995fbc29fbb00419a6e
提交 | 用户 | age
bfc108 1 /* ----------------------------------------------------------------------   
Q 2 * Copyright (C) 2010-2014 ARM Limited. All rights reserved.   
3 *   
4 * $Date:        19. March 2015 
5 * $Revision:     V.1.4.5  
6 *   
7 * Project:         CMSIS DSP Library   
8 * Title:        arm_cfft_radix2_q31.c   
9 *   
10 * Description:    Radix-2 Decimation in Frequency CFFT & CIFFT Fixed point processing function   
11 *   
12 *   
13 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
14 *  
15 * Redistribution and use in source and binary forms, with or without 
16 * modification, are permitted provided that the following conditions
17 * are met:
18 *   - Redistributions of source code must retain the above copyright
19 *     notice, this list of conditions and the following disclaimer.
20 *   - Redistributions in binary form must reproduce the above copyright
21 *     notice, this list of conditions and the following disclaimer in
22 *     the documentation and/or other materials provided with the 
23 *     distribution.
24 *   - Neither the name of ARM LIMITED nor the names of its contributors
25 *     may be used to endorse or promote products derived from this
26 *     software without specific prior written permission.
27 *
28 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
29 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
30 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
31 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
32 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
33 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
34 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
35 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
36 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
37 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
38 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
39 * POSSIBILITY OF SUCH DAMAGE.    
40 * -------------------------------------------------------------------- */
41
42 #include "arm_math.h"
43
44 void arm_radix2_butterfly_q31(
45   q31_t * pSrc,
46   uint32_t fftLen,
47   q31_t * pCoef,
48   uint16_t twidCoefModifier);
49
50 void arm_radix2_butterfly_inverse_q31(
51   q31_t * pSrc,
52   uint32_t fftLen,
53   q31_t * pCoef,
54   uint16_t twidCoefModifier);
55
56 void arm_bitreversal_q31(
57   q31_t * pSrc,
58   uint32_t fftLen,
59   uint16_t bitRevFactor,
60   uint16_t * pBitRevTab);
61
62 /**   
63 * @ingroup groupTransforms   
64 */
65
66 /**   
67 * @addtogroup ComplexFFT   
68 * @{   
69 */
70
71 /**   
72 * @details   
73 * @brief Processing function for the fixed-point CFFT/CIFFT.  
74 * @deprecated Do not use this function.  It has been superseded by \ref arm_cfft_q31 and will be removed
75 * @param[in]      *S    points to an instance of the fixed-point CFFT/CIFFT structure.  
76 * @param[in, out] *pSrc points to the complex data buffer of size <code>2*fftLen</code>. Processing occurs in-place.  
77 * @return none.  
78 */
79
80 void arm_cfft_radix2_q31(
81 const arm_cfft_radix2_instance_q31 * S,
82 q31_t * pSrc)
83 {
84
85    if(S->ifftFlag == 1u)
86    {
87       arm_radix2_butterfly_inverse_q31(pSrc, S->fftLen,
88       S->pTwiddle, S->twidCoefModifier);
89    }
90    else
91    {
92       arm_radix2_butterfly_q31(pSrc, S->fftLen,
93       S->pTwiddle, S->twidCoefModifier);
94    }
95
96    arm_bitreversal_q31(pSrc, S->fftLen, S->bitRevFactor, S->pBitRevTable);
97 }
98
99 /**   
100 * @} end of ComplexFFT group   
101 */
102
103 void arm_radix2_butterfly_q31(
104 q31_t * pSrc,
105 uint32_t fftLen,
106 q31_t * pCoef,
107 uint16_t twidCoefModifier)
108 {
109
110    unsigned i, j, k, l, m;
111    unsigned n1, n2, ia;
112    q31_t xt, yt, cosVal, sinVal;
113    q31_t p0, p1;
114
115    //N = fftLen; 
116    n2 = fftLen;
117
118    n1 = n2;
119    n2 = n2 >> 1;
120    ia = 0;
121
122    // loop for groups 
123    for (i = 0; i < n2; i++)
124    {
125       cosVal = pCoef[ia * 2];
126       sinVal = pCoef[(ia * 2) + 1];
127       ia = ia + twidCoefModifier;
128
129       l = i + n2;
130       xt = (pSrc[2 * i] >> 1u) - (pSrc[2 * l] >> 1u);
131       pSrc[2 * i] = ((pSrc[2 * i] >> 1u) + (pSrc[2 * l] >> 1u)) >> 1u;
132       
133       yt = (pSrc[2 * i + 1] >> 1u) - (pSrc[2 * l + 1] >> 1u);
134       pSrc[2 * i + 1] =
135         ((pSrc[2 * l + 1] >> 1u) + (pSrc[2 * i + 1] >> 1u)) >> 1u;
136
137       mult_32x32_keep32_R(p0, xt, cosVal);
138       mult_32x32_keep32_R(p1, yt, cosVal);
139       multAcc_32x32_keep32_R(p0, yt, sinVal); 
140       multSub_32x32_keep32_R(p1, xt, sinVal);
141       
142       pSrc[2u * l] = p0;
143       pSrc[2u * l + 1u] = p1;
144
145    }                             // groups loop end 
146
147    twidCoefModifier <<= 1u;
148
149    // loop for stage 
150    for (k = fftLen / 2; k > 2; k = k >> 1)
151    {
152       n1 = n2;
153       n2 = n2 >> 1;
154       ia = 0;
155
156       // loop for groups 
157       for (j = 0; j < n2; j++)
158       {
159          cosVal = pCoef[ia * 2];
160          sinVal = pCoef[(ia * 2) + 1];
161          ia = ia + twidCoefModifier;
162
163          // loop for butterfly 
164          i = j;
165          m = fftLen / n1;
166          do
167          {
168             l = i + n2;
169             xt = pSrc[2 * i] - pSrc[2 * l];
170             pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]) >> 1u;
171             
172             yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
173             pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]) >> 1u;
174
175             mult_32x32_keep32_R(p0, xt, cosVal);
176             mult_32x32_keep32_R(p1, yt, cosVal);
177             multAcc_32x32_keep32_R(p0, yt, sinVal);
178             multSub_32x32_keep32_R(p1, xt, sinVal);
179             
180             pSrc[2u * l] = p0;
181             pSrc[2u * l + 1u] = p1;
182             i += n1;
183             m--;
184          } while( m > 0);                   // butterfly loop end 
185
186       }                           // groups loop end 
187
188       twidCoefModifier <<= 1u;
189    }                             // stages loop end 
190
191    n1 = n2;
192    n2 = n2 >> 1;
193    ia = 0;
194
195    cosVal = pCoef[ia * 2];
196    sinVal = pCoef[(ia * 2) + 1];
197    ia = ia + twidCoefModifier;
198
199    // loop for butterfly 
200    for (i = 0; i < fftLen; i += n1)
201    {
202       l = i + n2;
203       xt = pSrc[2 * i] - pSrc[2 * l];
204       pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
205
206       yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
207       pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
208
209       pSrc[2u * l] = xt;
210
211       pSrc[2u * l + 1u] = yt;
212
213       i += n1;
214       l = i + n2;
215
216       xt = pSrc[2 * i] - pSrc[2 * l];
217       pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
218
219       yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
220       pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
221
222       pSrc[2u * l] = xt;
223
224       pSrc[2u * l + 1u] = yt;
225
226    }                             // butterfly loop end 
227
228 }
229
230
231 void arm_radix2_butterfly_inverse_q31(
232 q31_t * pSrc,
233 uint32_t fftLen,
234 q31_t * pCoef,
235 uint16_t twidCoefModifier)
236 {
237
238    unsigned i, j, k, l;
239    unsigned n1, n2, ia;
240    q31_t xt, yt, cosVal, sinVal;
241    q31_t p0, p1;
242
243    //N = fftLen; 
244    n2 = fftLen;
245
246    n1 = n2;
247    n2 = n2 >> 1;
248    ia = 0;
249
250    // loop for groups 
251    for (i = 0; i < n2; i++)
252    {
253       cosVal = pCoef[ia * 2];
254       sinVal = pCoef[(ia * 2) + 1];
255       ia = ia + twidCoefModifier;
256
257       l = i + n2;
258       xt = (pSrc[2 * i] >> 1u) - (pSrc[2 * l] >> 1u);
259       pSrc[2 * i] = ((pSrc[2 * i] >> 1u) + (pSrc[2 * l] >> 1u)) >> 1u;
260       
261       yt = (pSrc[2 * i + 1] >> 1u) - (pSrc[2 * l + 1] >> 1u);
262       pSrc[2 * i + 1] =
263         ((pSrc[2 * l + 1] >> 1u) + (pSrc[2 * i + 1] >> 1u)) >> 1u;
264
265       mult_32x32_keep32_R(p0, xt, cosVal);
266       mult_32x32_keep32_R(p1, yt, cosVal);
267       multSub_32x32_keep32_R(p0, yt, sinVal);
268       multAcc_32x32_keep32_R(p1, xt, sinVal);
269       
270       pSrc[2u * l] = p0;
271       pSrc[2u * l + 1u] = p1;
272    }                             // groups loop end 
273
274    twidCoefModifier = twidCoefModifier << 1u;
275
276    // loop for stage 
277    for (k = fftLen / 2; k > 2; k = k >> 1)
278    {
279       n1 = n2;
280       n2 = n2 >> 1;
281       ia = 0;
282
283       // loop for groups 
284       for (j = 0; j < n2; j++)
285       {
286          cosVal = pCoef[ia * 2];
287          sinVal = pCoef[(ia * 2) + 1];
288          ia = ia + twidCoefModifier;
289
290          // loop for butterfly 
291          for (i = j; i < fftLen; i += n1)
292          {
293             l = i + n2;
294             xt = pSrc[2 * i] - pSrc[2 * l];
295             pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]) >> 1u;
296       
297             yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
298             pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]) >> 1u;
299
300             mult_32x32_keep32_R(p0, xt, cosVal);
301             mult_32x32_keep32_R(p1, yt, cosVal);
302             multSub_32x32_keep32_R(p0, yt, sinVal);
303             multAcc_32x32_keep32_R(p1, xt, sinVal);
304             
305             pSrc[2u * l] = p0;
306             pSrc[2u * l + 1u] = p1;
307          }                         // butterfly loop end 
308
309       }                           // groups loop end 
310
311       twidCoefModifier = twidCoefModifier << 1u;
312    }                             // stages loop end 
313
314    n1 = n2;
315    n2 = n2 >> 1;
316    ia = 0;
317
318    cosVal = pCoef[ia * 2];
319    sinVal = pCoef[(ia * 2) + 1];
320    ia = ia + twidCoefModifier;
321
322    // loop for butterfly 
323    for (i = 0; i < fftLen; i += n1)
324    {
325       l = i + n2;
326       xt = pSrc[2 * i] - pSrc[2 * l];
327       pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
328
329       yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
330       pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
331
332       pSrc[2u * l] = xt;
333
334       pSrc[2u * l + 1u] = yt;
335
336       i += n1;
337       l = i + n2;
338
339       xt = pSrc[2 * i] - pSrc[2 * l];
340       pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
341
342       yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
343       pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
344
345       pSrc[2u * l] = xt;
346
347       pSrc[2u * l + 1u] = yt;
348
349    }                             // butterfly loop end 
350
351 }