查看 | 历史 | 提交 | 提交对比
QuakeGod
2023-10-08 483170e190a0dd4666b2a63e5d31466052ba0c6a
提交 | 用户 | age
483170 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_rfft_q15.c    
9 *    
10 * Description:    RFFT & RIFFT Q15 process 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 /*--------------------------------------------------------------------    
45 *        Internal functions prototypes    
46 --------------------------------------------------------------------*/
47
48 void arm_split_rfft_q15(
49     q15_t * pSrc,
50     uint32_t fftLen,
51     q15_t * pATable,
52     q15_t * pBTable,
53     q15_t * pDst,
54     uint32_t modifier);
55
56 void arm_split_rifft_q15(
57     q15_t * pSrc,
58     uint32_t fftLen,
59     q15_t * pATable,
60     q15_t * pBTable,
61     q15_t * pDst,
62     uint32_t modifier);
63
64 /**    
65 * @addtogroup RealFFT    
66 * @{    
67 */
68
69 /**    
70 * @brief Processing function for the Q15 RFFT/RIFFT.   
71 * @param[in]  *S    points to an instance of the Q15 RFFT/RIFFT structure.   
72 * @param[in]  *pSrc points to the input buffer.   
73 * @param[out] *pDst points to the output buffer.   
74 * @return none.   
75 *    
76 * \par Input an output formats:   
77 * \par    
78 * Internally input is downscaled by 2 for every stage to avoid saturations inside CFFT/CIFFT process.    
79 * Hence the output format is different for different RFFT sizes.    
80 * The input and output formats for different RFFT sizes and number of bits to upscale are mentioned in the tables below for RFFT and RIFFT:   
81 * \par    
82 * \image html RFFTQ15.gif "Input and Output Formats for Q15 RFFT"    
83 * \par    
84 * \image html RIFFTQ15.gif "Input and Output Formats for Q15 RIFFT"    
85 */
86
87 void arm_rfft_q15(
88     const arm_rfft_instance_q15 * S,
89     q15_t * pSrc,
90     q15_t * pDst)
91 {
92     const arm_cfft_instance_q15 *S_CFFT = S->pCfft;
93     uint32_t i;
94     uint32_t L2 = S->fftLenReal >> 1;
95
96     /* Calculation of RIFFT of input */
97     if(S->ifftFlagR == 1u)
98     {
99         /*  Real IFFT core process */
100         arm_split_rifft_q15(pSrc, L2, S->pTwiddleAReal,
101                             S->pTwiddleBReal, pDst, S->twidCoefRModifier);
102         
103         /* Complex IFFT process */
104         arm_cfft_q15(S_CFFT, pDst, S->ifftFlagR, S->bitReverseFlagR);
105         
106         for(i=0;i<S->fftLenReal;i++)
107         {
108             pDst[i] = pDst[i] << 1;
109         }
110     }
111     else
112     {
113         /* Calculation of RFFT of input */
114         
115         /* Complex FFT process */
116         arm_cfft_q15(S_CFFT, pSrc, S->ifftFlagR, S->bitReverseFlagR);
117
118         /*  Real FFT core process */
119         arm_split_rfft_q15(pSrc, L2, S->pTwiddleAReal,
120                             S->pTwiddleBReal, pDst, S->twidCoefRModifier);
121     }
122 }
123
124 /**    
125 * @} end of RealFFT group    
126 */
127
128 /**    
129 * @brief  Core Real FFT process    
130 * @param  *pSrc                 points to the input buffer.   
131 * @param  fftLen                  length of FFT.   
132 * @param  *pATable             points to the A twiddle Coef buffer.    
133 * @param  *pBTable             points to the B twiddle Coef buffer.   
134 * @param  *pDst                 points to the output buffer.   
135 * @param  modifier             twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.   
136 * @return none.    
137 * The function implements a Real FFT    
138 */
139
140 void arm_split_rfft_q15(
141     q15_t * pSrc,
142     uint32_t fftLen,
143     q15_t * pATable,
144     q15_t * pBTable,
145     q15_t * pDst,
146     uint32_t modifier)
147 {
148     uint32_t i;                                    /* Loop Counter */
149     q31_t outR, outI;                              /* Temporary variables for output */
150     q15_t *pCoefA, *pCoefB;                        /* Temporary pointers for twiddle factors */
151     q15_t *pSrc1, *pSrc2;
152 #ifndef ARM_MATH_CM0_FAMILY
153     q15_t *pD1, *pD2;
154 #endif
155
156     //  pSrc[2u * fftLen] = pSrc[0]; 
157     //  pSrc[(2u * fftLen) + 1u] = pSrc[1]; 
158
159     pCoefA = &pATable[modifier * 2u];
160     pCoefB = &pBTable[modifier * 2u];
161
162     pSrc1 = &pSrc[2];
163     pSrc2 = &pSrc[(2u * fftLen) - 2u];
164
165 #ifndef ARM_MATH_CM0_FAMILY
166
167     /* Run the below code for Cortex-M4 and Cortex-M3 */
168     i = 1u;
169     pD1 = pDst + 2;
170     pD2 = pDst + (4u * fftLen) - 2;
171
172     for(i = fftLen - 1; i > 0; i--)
173     {
174         /*    
175         outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]    
176         + pSrc[2 * n - 2 * i] * pBTable[2 * i] +    
177         pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);    
178         */
179
180         /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +    
181         pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -    
182         pIn[2 * n - 2 * i + 1] * pBTable[2 * i]); */
183
184
185 #ifndef ARM_MATH_BIG_ENDIAN
186
187         /* pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1] */
188         outR = __SMUSD(*__SIMD32(pSrc1), *__SIMD32(pCoefA));
189
190 #else
191
192         /* -(pSrc[2 * i + 1] * pATable[2 * i + 1] - pSrc[2 * i] * pATable[2 * i]) */
193         outR = -(__SMUSD(*__SIMD32(pSrc1), *__SIMD32(pCoefA)));
194
195 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
196
197         /* pSrc[2 * n - 2 * i] * pBTable[2 * i] +    
198         pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]) */
199         outR = __SMLAD(*__SIMD32(pSrc2), *__SIMD32(pCoefB), outR) >> 16u;
200
201         /* pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -    
202         pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
203
204 #ifndef ARM_MATH_BIG_ENDIAN
205
206         outI = __SMUSDX(*__SIMD32(pSrc2)--, *__SIMD32(pCoefB));
207
208 #else
209
210         outI = __SMUSDX(*__SIMD32(pCoefB), *__SIMD32(pSrc2)--);
211
212 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
213
214         /* (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] */
215         outI = __SMLADX(*__SIMD32(pSrc1)++, *__SIMD32(pCoefA), outI);
216
217         /* write output */
218         *pD1++ = (q15_t) outR;
219         *pD1++ = outI >> 16u;
220
221         /* write complex conjugate output */
222         pD2[0] = (q15_t) outR;
223         pD2[1] = -(outI >> 16u);
224         pD2 -= 2;
225
226         /* update coefficient pointer */
227         pCoefB = pCoefB + (2u * modifier);
228         pCoefA = pCoefA + (2u * modifier);
229     }
230
231     pDst[2u * fftLen] = (pSrc[0] - pSrc[1]) >> 1;
232     pDst[(2u * fftLen) + 1u] = 0;
233
234     pDst[0] = (pSrc[0] + pSrc[1]) >> 1;
235     pDst[1] = 0;
236
237 #else
238
239     /* Run the below code for Cortex-M0 */
240     i = 1u;
241
242     while(i < fftLen)
243     {
244         /*    
245         outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]    
246         + pSrc[2 * n - 2 * i] * pBTable[2 * i] +    
247         pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);    
248         */
249
250         outR = *pSrc1 * *pCoefA;
251         outR = outR - (*(pSrc1 + 1) * *(pCoefA + 1));
252         outR = outR + (*pSrc2 * *pCoefB);
253         outR = (outR + (*(pSrc2 + 1) * *(pCoefB + 1))) >> 16;
254
255
256         /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +    
257         pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -    
258         pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);   
259         */
260
261         outI = *pSrc2 * *(pCoefB + 1);
262         outI = outI - (*(pSrc2 + 1) * *pCoefB);
263         outI = outI + (*(pSrc1 + 1) * *pCoefA);
264         outI = outI + (*pSrc1 * *(pCoefA + 1));
265
266         /* update input pointers */
267         pSrc1 += 2u;
268         pSrc2 -= 2u;
269
270         /* write output */
271         pDst[2u * i] = (q15_t) outR;
272         pDst[(2u * i) + 1u] = outI >> 16u;
273
274         /* write complex conjugate output */
275         pDst[(4u * fftLen) - (2u * i)] = (q15_t) outR;
276         pDst[((4u * fftLen) - (2u * i)) + 1u] = -(outI >> 16u);
277
278         /* update coefficient pointer */
279         pCoefB = pCoefB + (2u * modifier);
280         pCoefA = pCoefA + (2u * modifier);
281
282         i++;
283     }
284
285     pDst[2u * fftLen] = (pSrc[0] - pSrc[1]) >> 1;
286     pDst[(2u * fftLen) + 1u] = 0;
287
288     pDst[0] = (pSrc[0] + pSrc[1]) >> 1;
289     pDst[1] = 0;
290
291 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
292 }
293
294
295 /**    
296 * @brief  Core Real IFFT process    
297 * @param[in]   *pSrc                 points to the input buffer.    
298 * @param[in]   fftLen              length of FFT.   
299 * @param[in]   *pATable             points to the twiddle Coef A buffer.   
300 * @param[in]   *pBTable             points to the twiddle Coef B buffer.    
301 * @param[out]  *pDst                 points to the output buffer.   
302 * @param[in]   modifier             twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.   
303 * @return none.    
304 * The function implements a Real IFFT    
305 */
306 void arm_split_rifft_q15(
307     q15_t * pSrc,
308     uint32_t fftLen,
309     q15_t * pATable,
310     q15_t * pBTable,
311     q15_t * pDst,
312     uint32_t modifier)
313 {
314     uint32_t i;                                    /* Loop Counter */
315     q31_t outR, outI;                              /* Temporary variables for output */
316     q15_t *pCoefA, *pCoefB;                        /* Temporary pointers for twiddle factors */
317     q15_t *pSrc1, *pSrc2;
318     q15_t *pDst1 = &pDst[0];
319
320     pCoefA = &pATable[0];
321     pCoefB = &pBTable[0];
322
323     pSrc1 = &pSrc[0];
324     pSrc2 = &pSrc[2u * fftLen];
325
326 #ifndef ARM_MATH_CM0_FAMILY
327
328     /* Run the below code for Cortex-M4 and Cortex-M3 */
329     i = fftLen;
330
331     while(i > 0u)
332     {
333         /*    
334         outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +    
335         pIn[2 * n - 2 * i] * pBTable[2 * i] -    
336         pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);    
337
338         outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -    
339         pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -    
340         pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);    
341         */
342
343
344 #ifndef ARM_MATH_BIG_ENDIAN
345
346         /* pIn[2 * n - 2 * i] * pBTable[2 * i] -    
347         pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]) */
348         outR = __SMUSD(*__SIMD32(pSrc2), *__SIMD32(pCoefB));
349
350 #else
351
352         /* -(-pIn[2 * n - 2 * i] * pBTable[2 * i] +  
353         pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1])) */
354         outR = -(__SMUSD(*__SIMD32(pSrc2), *__SIMD32(pCoefB)));
355
356 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
357
358         /* pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +    
359         pIn[2 * n - 2 * i] * pBTable[2 * i] */
360         outR = __SMLAD(*__SIMD32(pSrc1), *__SIMD32(pCoefA), outR) >> 16u;
361
362         /*    
363         -pIn[2 * n - 2 * i] * pBTable[2 * i + 1] +    
364         pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
365         outI = __SMUADX(*__SIMD32(pSrc2)--, *__SIMD32(pCoefB));
366
367         /* pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] */
368
369 #ifndef ARM_MATH_BIG_ENDIAN
370
371         outI = __SMLSDX(*__SIMD32(pCoefA), *__SIMD32(pSrc1)++, -outI);
372
373 #else
374
375         outI = __SMLSDX(*__SIMD32(pSrc1)++, *__SIMD32(pCoefA), -outI);
376
377 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
378         /* write output */
379
380 #ifndef ARM_MATH_BIG_ENDIAN
381
382         *__SIMD32(pDst1)++ = __PKHBT(outR, (outI >> 16u), 16);
383
384 #else
385
386         *__SIMD32(pDst1)++ = __PKHBT((outI >> 16u), outR, 16);
387
388 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
389
390         /* update coefficient pointer */
391         pCoefB = pCoefB + (2u * modifier);
392         pCoefA = pCoefA + (2u * modifier);
393
394         i--;
395     }
396 #else
397     /* Run the below code for Cortex-M0 */
398     i = fftLen;
399
400     while(i > 0u)
401     {
402         /*    
403         outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +    
404         pIn[2 * n - 2 * i] * pBTable[2 * i] -    
405         pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);    
406         */
407
408         outR = *pSrc2 * *pCoefB;
409         outR = outR - (*(pSrc2 + 1) * *(pCoefB + 1));
410         outR = outR + (*pSrc1 * *pCoefA);
411         outR = (outR + (*(pSrc1 + 1) * *(pCoefA + 1))) >> 16;
412
413         /*   
414         outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -   
415         pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -   
416         pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);   
417         */
418
419         outI = *(pSrc1 + 1) * *pCoefA;
420         outI = outI - (*pSrc1 * *(pCoefA + 1));
421         outI = outI - (*pSrc2 * *(pCoefB + 1));
422         outI = outI - (*(pSrc2 + 1) * *(pCoefB));
423
424         /* update input pointers */
425         pSrc1 += 2u;
426         pSrc2 -= 2u;
427
428         /* write output */
429         *pDst1++ = (q15_t) outR;
430         *pDst1++ = (q15_t) (outI >> 16);
431
432         /* update coefficient pointer */
433         pCoefB = pCoefB + (2u * modifier);
434         pCoefA = pCoefA + (2u * modifier);
435
436         i--;
437     }
438 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
439 }