12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910 |
- /* ----------------------------------------------------------------------
- * Project: CMSIS DSP Library
- * Title: arm_cfft_radix4_q15.c
- * Description: This file has function definition of Radix-4 FFT & IFFT function and
- * In-place bit reversal using bit reversal table
- *
- * $Date: 27. January 2017
- * $Revision: V.1.5.1
- *
- * Target Processor: Cortex-M cores
- * -------------------------------------------------------------------- */
- /*
- * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
- *
- * SPDX-License-Identifier: Apache-2.0
- *
- * Licensed under the Apache License, Version 2.0 (the License); you may
- * not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an AS IS BASIS, WITHOUT
- * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
- #include "arm_math.h"
- void arm_radix4_butterfly_q15(
- q15_t * pSrc16,
- uint32_t fftLen,
- q15_t * pCoef16,
- uint32_t twidCoefModifier);
- void arm_radix4_butterfly_inverse_q15(
- q15_t * pSrc16,
- uint32_t fftLen,
- q15_t * pCoef16,
- uint32_t twidCoefModifier);
- void arm_bitreversal_q15(
- q15_t * pSrc,
- uint32_t fftLen,
- uint16_t bitRevFactor,
- uint16_t * pBitRevTab);
- /**
- * @ingroup groupTransforms
- */
- /**
- * @addtogroup ComplexFFT
- * @{
- */
- /**
- * @details
- * @brief Processing function for the Q15 CFFT/CIFFT.
- * @deprecated Do not use this function. It has been superseded by \ref arm_cfft_q15 and will be removed
- * @param[in] *S points to an instance of the Q15 CFFT/CIFFT structure.
- * @param[in, out] *pSrc points to the complex data buffer. Processing occurs in-place.
- * @return none.
- *
- * \par Input and output formats:
- * \par
- * Internally input is downscaled by 2 for every stage to avoid saturations inside CFFT/CIFFT process.
- * Hence the output format is different for different FFT sizes.
- * The input and output formats for different FFT sizes and number of bits to upscale are mentioned in the tables below for CFFT and CIFFT:
- * \par
- * \image html CFFTQ15.gif "Input and Output Formats for Q15 CFFT"
- * \image html CIFFTQ15.gif "Input and Output Formats for Q15 CIFFT"
- */
- void arm_cfft_radix4_q15(
- const arm_cfft_radix4_instance_q15 * S,
- q15_t * pSrc)
- {
- if (S->ifftFlag == 1U)
- {
- /* Complex IFFT radix-4 */
- arm_radix4_butterfly_inverse_q15(pSrc, S->fftLen, S->pTwiddle, S->twidCoefModifier);
- }
- else
- {
- /* Complex FFT radix-4 */
- arm_radix4_butterfly_q15(pSrc, S->fftLen, S->pTwiddle, S->twidCoefModifier);
- }
- if (S->bitReverseFlag == 1U)
- {
- /* Bit Reversal */
- arm_bitreversal_q15(pSrc, S->fftLen, S->bitRevFactor, S->pBitRevTable);
- }
- }
- /**
- * @} end of ComplexFFT group
- */
- /*
- * Radix-4 FFT algorithm used is :
- *
- * Input real and imaginary data:
- * x(n) = xa + j * ya
- * x(n+N/4 ) = xb + j * yb
- * x(n+N/2 ) = xc + j * yc
- * x(n+3N 4) = xd + j * yd
- *
- *
- * Output real and imaginary data:
- * x(4r) = xa'+ j * ya'
- * x(4r+1) = xb'+ j * yb'
- * x(4r+2) = xc'+ j * yc'
- * x(4r+3) = xd'+ j * yd'
- *
- *
- * Twiddle factors for radix-4 FFT:
- * Wn = co1 + j * (- si1)
- * W2n = co2 + j * (- si2)
- * W3n = co3 + j * (- si3)
- * The real and imaginary output values for the radix-4 butterfly are
- * xa' = xa + xb + xc + xd
- * ya' = ya + yb + yc + yd
- * xb' = (xa+yb-xc-yd)* co1 + (ya-xb-yc+xd)* (si1)
- * yb' = (ya-xb-yc+xd)* co1 - (xa+yb-xc-yd)* (si1)
- * xc' = (xa-xb+xc-xd)* co2 + (ya-yb+yc-yd)* (si2)
- * yc' = (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2)
- * xd' = (xa-yb-xc+yd)* co3 + (ya+xb-yc-xd)* (si3)
- * yd' = (ya+xb-yc-xd)* co3 - (xa-yb-xc+yd)* (si3)
- *
- */
- /**
- * @brief Core function for the Q15 CFFT butterfly process.
- * @param[in, out] *pSrc16 points to the in-place buffer of Q15 data type.
- * @param[in] fftLen length of the FFT.
- * @param[in] *pCoef16 points to twiddle coefficient buffer.
- * @param[in] twidCoefModifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
- * @return none.
- */
- void arm_radix4_butterfly_q15(
- q15_t * pSrc16,
- uint32_t fftLen,
- q15_t * pCoef16,
- uint32_t twidCoefModifier)
- {
- #if defined (ARM_MATH_DSP)
- /* Run the below code for Cortex-M4 and Cortex-M3 */
- q31_t R, S, T, U;
- q31_t C1, C2, C3, out1, out2;
- uint32_t n1, n2, ic, i0, j, k;
- q15_t *ptr1;
- q15_t *pSi0;
- q15_t *pSi1;
- q15_t *pSi2;
- q15_t *pSi3;
- q31_t xaya, xbyb, xcyc, xdyd;
- /* Total process is divided into three stages */
- /* process first stage, middle stages, & last stage */
- /* Initializations for the first stage */
- n2 = fftLen;
- n1 = n2;
- /* n2 = fftLen/4 */
- n2 >>= 2U;
- /* Index for twiddle coefficient */
- ic = 0U;
- /* Index for input read and output write */
- j = n2;
- pSi0 = pSrc16;
- pSi1 = pSi0 + 2 * n2;
- pSi2 = pSi1 + 2 * n2;
- pSi3 = pSi2 + 2 * n2;
- /* Input is in 1.15(q15) format */
- /* start of first stage process */
- do
- {
- /* Butterfly implementation */
- /* Reading i0, i0+fftLen/2 inputs */
- /* Read ya (real), xa(imag) input */
- T = _SIMD32_OFFSET(pSi0);
- T = __SHADD16(T, 0); // this is just a SIMD arithmetic shift right by 1
- T = __SHADD16(T, 0); // it turns out doing this twice is 2 cycles, the alternative takes 3 cycles
- //in = ((int16_t) (T & 0xFFFF)) >> 2; // alternative code that takes 3 cycles
- //T = ((T >> 2) & 0xFFFF0000) | (in & 0xFFFF);
- /* Read yc (real), xc(imag) input */
- S = _SIMD32_OFFSET(pSi2);
- S = __SHADD16(S, 0);
- S = __SHADD16(S, 0);
- /* R = packed((ya + yc), (xa + xc) ) */
- R = __QADD16(T, S);
- /* S = packed((ya - yc), (xa - xc) ) */
- S = __QSUB16(T, S);
- /* Reading i0+fftLen/4 , i0+3fftLen/4 inputs */
- /* Read yb (real), xb(imag) input */
- T = _SIMD32_OFFSET(pSi1);
- T = __SHADD16(T, 0);
- T = __SHADD16(T, 0);
- /* Read yd (real), xd(imag) input */
- U = _SIMD32_OFFSET(pSi3);
- U = __SHADD16(U, 0);
- U = __SHADD16(U, 0);
- /* T = packed((yb + yd), (xb + xd) ) */
- T = __QADD16(T, U);
- /* writing the butterfly processed i0 sample */
- /* xa' = xa + xb + xc + xd */
- /* ya' = ya + yb + yc + yd */
- _SIMD32_OFFSET(pSi0) = __SHADD16(R, T);
- pSi0 += 2;
- /* R = packed((ya + yc) - (yb + yd), (xa + xc)- (xb + xd)) */
- R = __QSUB16(R, T);
- /* co2 & si2 are read from SIMD Coefficient pointer */
- C2 = _SIMD32_OFFSET(pCoef16 + (4U * ic));
- #ifndef ARM_MATH_BIG_ENDIAN
- /* xc' = (xa-xb+xc-xd)* co2 + (ya-yb+yc-yd)* (si2) */
- out1 = __SMUAD(C2, R) >> 16U;
- /* yc' = (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) */
- out2 = __SMUSDX(C2, R);
- #else
- /* xc' = (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) */
- out1 = __SMUSDX(R, C2) >> 16U;
- /* yc' = (xa-xb+xc-xd)* co2 + (ya-yb+yc-yd)* (si2) */
- out2 = __SMUAD(C2, R);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- /* Reading i0+fftLen/4 */
- /* T = packed(yb, xb) */
- T = _SIMD32_OFFSET(pSi1);
- T = __SHADD16(T, 0);
- T = __SHADD16(T, 0);
- /* writing the butterfly processed i0 + fftLen/4 sample */
- /* writing output(xc', yc') in little endian format */
- _SIMD32_OFFSET(pSi1) =
- (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
- pSi1 += 2;
- /* Butterfly calculations */
- /* U = packed(yd, xd) */
- U = _SIMD32_OFFSET(pSi3);
- U = __SHADD16(U, 0);
- U = __SHADD16(U, 0);
- /* T = packed(yb-yd, xb-xd) */
- T = __QSUB16(T, U);
- #ifndef ARM_MATH_BIG_ENDIAN
- /* R = packed((ya-yc) + (xb- xd) , (xa-xc) - (yb-yd)) */
- R = __QASX(S, T);
- /* S = packed((ya-yc) - (xb- xd), (xa-xc) + (yb-yd)) */
- S = __QSAX(S, T);
- #else
- /* R = packed((ya-yc) + (xb- xd) , (xa-xc) - (yb-yd)) */
- R = __QSAX(S, T);
- /* S = packed((ya-yc) - (xb- xd), (xa-xc) + (yb-yd)) */
- S = __QASX(S, T);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- /* co1 & si1 are read from SIMD Coefficient pointer */
- C1 = _SIMD32_OFFSET(pCoef16 + (2U * ic));
- /* Butterfly process for the i0+fftLen/2 sample */
- #ifndef ARM_MATH_BIG_ENDIAN
- /* xb' = (xa+yb-xc-yd)* co1 + (ya-xb-yc+xd)* (si1) */
- out1 = __SMUAD(C1, S) >> 16U;
- /* yb' = (ya-xb-yc+xd)* co1 - (xa+yb-xc-yd)* (si1) */
- out2 = __SMUSDX(C1, S);
- #else
- /* xb' = (ya-xb-yc+xd)* co1 - (xa+yb-xc-yd)* (si1) */
- out1 = __SMUSDX(S, C1) >> 16U;
- /* yb' = (xa+yb-xc-yd)* co1 + (ya-xb-yc+xd)* (si1) */
- out2 = __SMUAD(C1, S);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- /* writing output(xb', yb') in little endian format */
- _SIMD32_OFFSET(pSi2) =
- ((out2) & 0xFFFF0000) | ((out1) & 0x0000FFFF);
- pSi2 += 2;
- /* co3 & si3 are read from SIMD Coefficient pointer */
- C3 = _SIMD32_OFFSET(pCoef16 + (6U * ic));
- /* Butterfly process for the i0+3fftLen/4 sample */
- #ifndef ARM_MATH_BIG_ENDIAN
- /* xd' = (xa-yb-xc+yd)* co3 + (ya+xb-yc-xd)* (si3) */
- out1 = __SMUAD(C3, R) >> 16U;
- /* yd' = (ya+xb-yc-xd)* co3 - (xa-yb-xc+yd)* (si3) */
- out2 = __SMUSDX(C3, R);
- #else
- /* xd' = (ya+xb-yc-xd)* co3 - (xa-yb-xc+yd)* (si3) */
- out1 = __SMUSDX(R, C3) >> 16U;
- /* yd' = (xa-yb-xc+yd)* co3 + (ya+xb-yc-xd)* (si3) */
- out2 = __SMUAD(C3, R);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- /* writing output(xd', yd') in little endian format */
- _SIMD32_OFFSET(pSi3) =
- ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
- pSi3 += 2;
- /* Twiddle coefficients index modifier */
- ic = ic + twidCoefModifier;
- } while (--j);
- /* data is in 4.11(q11) format */
- /* end of first stage process */
- /* start of middle stage process */
- /* Twiddle coefficients index modifier */
- twidCoefModifier <<= 2U;
- /* Calculation of Middle stage */
- for (k = fftLen / 4U; k > 4U; k >>= 2U)
- {
- /* Initializations for the middle stage */
- n1 = n2;
- n2 >>= 2U;
- ic = 0U;
- for (j = 0U; j <= (n2 - 1U); j++)
- {
- /* index calculation for the coefficients */
- C1 = _SIMD32_OFFSET(pCoef16 + (2U * ic));
- C2 = _SIMD32_OFFSET(pCoef16 + (4U * ic));
- C3 = _SIMD32_OFFSET(pCoef16 + (6U * ic));
- /* Twiddle coefficients index modifier */
- ic = ic + twidCoefModifier;
- pSi0 = pSrc16 + 2 * j;
- pSi1 = pSi0 + 2 * n2;
- pSi2 = pSi1 + 2 * n2;
- pSi3 = pSi2 + 2 * n2;
- /* Butterfly implementation */
- for (i0 = j; i0 < fftLen; i0 += n1)
- {
- /* Reading i0, i0+fftLen/2 inputs */
- /* Read ya (real), xa(imag) input */
- T = _SIMD32_OFFSET(pSi0);
- /* Read yc (real), xc(imag) input */
- S = _SIMD32_OFFSET(pSi2);
- /* R = packed( (ya + yc), (xa + xc)) */
- R = __QADD16(T, S);
- /* S = packed((ya - yc), (xa - xc)) */
- S = __QSUB16(T, S);
- /* Reading i0+fftLen/4 , i0+3fftLen/4 inputs */
- /* Read yb (real), xb(imag) input */
- T = _SIMD32_OFFSET(pSi1);
- /* Read yd (real), xd(imag) input */
- U = _SIMD32_OFFSET(pSi3);
- /* T = packed( (yb + yd), (xb + xd)) */
- T = __QADD16(T, U);
- /* writing the butterfly processed i0 sample */
- /* xa' = xa + xb + xc + xd */
- /* ya' = ya + yb + yc + yd */
- out1 = __SHADD16(R, T);
- out1 = __SHADD16(out1, 0);
- _SIMD32_OFFSET(pSi0) = out1;
- pSi0 += 2 * n1;
- /* R = packed( (ya + yc) - (yb + yd), (xa + xc) - (xb + xd)) */
- R = __SHSUB16(R, T);
- #ifndef ARM_MATH_BIG_ENDIAN
- /* (ya-yb+yc-yd)* (si2) + (xa-xb+xc-xd)* co2 */
- out1 = __SMUAD(C2, R) >> 16U;
- /* (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) */
- out2 = __SMUSDX(C2, R);
- #else
- /* (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) */
- out1 = __SMUSDX(R, C2) >> 16U;
- /* (ya-yb+yc-yd)* (si2) + (xa-xb+xc-xd)* co2 */
- out2 = __SMUAD(C2, R);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- /* Reading i0+3fftLen/4 */
- /* Read yb (real), xb(imag) input */
- T = _SIMD32_OFFSET(pSi1);
- /* writing the butterfly processed i0 + fftLen/4 sample */
- /* xc' = (xa-xb+xc-xd)* co2 + (ya-yb+yc-yd)* (si2) */
- /* yc' = (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) */
- _SIMD32_OFFSET(pSi1) =
- ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
- pSi1 += 2 * n1;
- /* Butterfly calculations */
- /* Read yd (real), xd(imag) input */
- U = _SIMD32_OFFSET(pSi3);
- /* T = packed(yb-yd, xb-xd) */
- T = __QSUB16(T, U);
- #ifndef ARM_MATH_BIG_ENDIAN
- /* R = packed((ya-yc) + (xb- xd) , (xa-xc) - (yb-yd)) */
- R = __SHASX(S, T);
- /* S = packed((ya-yc) - (xb- xd), (xa-xc) + (yb-yd)) */
- S = __SHSAX(S, T);
- /* Butterfly process for the i0+fftLen/2 sample */
- out1 = __SMUAD(C1, S) >> 16U;
- out2 = __SMUSDX(C1, S);
- #else
- /* R = packed((ya-yc) + (xb- xd) , (xa-xc) - (yb-yd)) */
- R = __SHSAX(S, T);
- /* S = packed((ya-yc) - (xb- xd), (xa-xc) + (yb-yd)) */
- S = __SHASX(S, T);
- /* Butterfly process for the i0+fftLen/2 sample */
- out1 = __SMUSDX(S, C1) >> 16U;
- out2 = __SMUAD(C1, S);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- /* xb' = (xa+yb-xc-yd)* co1 + (ya-xb-yc+xd)* (si1) */
- /* yb' = (ya-xb-yc+xd)* co1 - (xa+yb-xc-yd)* (si1) */
- _SIMD32_OFFSET(pSi2) =
- ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
- pSi2 += 2 * n1;
- /* Butterfly process for the i0+3fftLen/4 sample */
- #ifndef ARM_MATH_BIG_ENDIAN
- out1 = __SMUAD(C3, R) >> 16U;
- out2 = __SMUSDX(C3, R);
- #else
- out1 = __SMUSDX(R, C3) >> 16U;
- out2 = __SMUAD(C3, R);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- /* xd' = (xa-yb-xc+yd)* co3 + (ya+xb-yc-xd)* (si3) */
- /* yd' = (ya+xb-yc-xd)* co3 - (xa-yb-xc+yd)* (si3) */
- _SIMD32_OFFSET(pSi3) =
- ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
- pSi3 += 2 * n1;
- }
- }
- /* Twiddle coefficients index modifier */
- twidCoefModifier <<= 2U;
- }
- /* end of middle stage process */
- /* data is in 10.6(q6) format for the 1024 point */
- /* data is in 8.8(q8) format for the 256 point */
- /* data is in 6.10(q10) format for the 64 point */
- /* data is in 4.12(q12) format for the 16 point */
- /* Initializations for the last stage */
- j = fftLen >> 2;
- ptr1 = &pSrc16[0];
- /* start of last stage process */
- /* Butterfly implementation */
- do
- {
- /* Read xa (real), ya(imag) input */
- xaya = *__SIMD32(ptr1)++;
- /* Read xb (real), yb(imag) input */
- xbyb = *__SIMD32(ptr1)++;
- /* Read xc (real), yc(imag) input */
- xcyc = *__SIMD32(ptr1)++;
- /* Read xd (real), yd(imag) input */
- xdyd = *__SIMD32(ptr1)++;
- /* R = packed((ya + yc), (xa + xc)) */
- R = __QADD16(xaya, xcyc);
- /* T = packed((yb + yd), (xb + xd)) */
- T = __QADD16(xbyb, xdyd);
- /* pointer updation for writing */
- ptr1 = ptr1 - 8U;
- /* xa' = xa + xb + xc + xd */
- /* ya' = ya + yb + yc + yd */
- *__SIMD32(ptr1)++ = __SHADD16(R, T);
- /* T = packed((yb + yd), (xb + xd)) */
- T = __QADD16(xbyb, xdyd);
- /* xc' = (xa-xb+xc-xd) */
- /* yc' = (ya-yb+yc-yd) */
- *__SIMD32(ptr1)++ = __SHSUB16(R, T);
- /* S = packed((ya - yc), (xa - xc)) */
- S = __QSUB16(xaya, xcyc);
- /* Read yd (real), xd(imag) input */
- /* T = packed( (yb - yd), (xb - xd)) */
- U = __QSUB16(xbyb, xdyd);
- #ifndef ARM_MATH_BIG_ENDIAN
- /* xb' = (xa+yb-xc-yd) */
- /* yb' = (ya-xb-yc+xd) */
- *__SIMD32(ptr1)++ = __SHSAX(S, U);
- /* xd' = (xa-yb-xc+yd) */
- /* yd' = (ya+xb-yc-xd) */
- *__SIMD32(ptr1)++ = __SHASX(S, U);
- #else
- /* xb' = (xa+yb-xc-yd) */
- /* yb' = (ya-xb-yc+xd) */
- *__SIMD32(ptr1)++ = __SHASX(S, U);
- /* xd' = (xa-yb-xc+yd) */
- /* yd' = (ya+xb-yc-xd) */
- *__SIMD32(ptr1)++ = __SHSAX(S, U);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- } while (--j);
- /* end of last stage process */
- /* output is in 11.5(q5) format for the 1024 point */
- /* output is in 9.7(q7) format for the 256 point */
- /* output is in 7.9(q9) format for the 64 point */
- /* output is in 5.11(q11) format for the 16 point */
- #else
- /* Run the below code for Cortex-M0 */
- q15_t R0, R1, S0, S1, T0, T1, U0, U1;
- q15_t Co1, Si1, Co2, Si2, Co3, Si3, out1, out2;
- uint32_t n1, n2, ic, i0, i1, i2, i3, j, k;
- /* Total process is divided into three stages */
- /* process first stage, middle stages, & last stage */
- /* Initializations for the first stage */
- n2 = fftLen;
- n1 = n2;
- /* n2 = fftLen/4 */
- n2 >>= 2U;
- /* Index for twiddle coefficient */
- ic = 0U;
- /* Index for input read and output write */
- i0 = 0U;
- j = n2;
- /* Input is in 1.15(q15) format */
- /* start of first stage process */
- do
- {
- /* Butterfly implementation */
- /* index calculation for the input as, */
- /* pSrc16[i0 + 0], pSrc16[i0 + fftLen/4], pSrc16[i0 + fftLen/2], pSrc16[i0 + 3fftLen/4] */
- i1 = i0 + n2;
- i2 = i1 + n2;
- i3 = i2 + n2;
- /* Reading i0, i0+fftLen/2 inputs */
- /* input is down scale by 4 to avoid overflow */
- /* Read ya (real), xa(imag) input */
- T0 = pSrc16[i0 * 2U] >> 2U;
- T1 = pSrc16[(i0 * 2U) + 1U] >> 2U;
- /* input is down scale by 4 to avoid overflow */
- /* Read yc (real), xc(imag) input */
- S0 = pSrc16[i2 * 2U] >> 2U;
- S1 = pSrc16[(i2 * 2U) + 1U] >> 2U;
- /* R0 = (ya + yc) */
- R0 = __SSAT(T0 + S0, 16U);
- /* R1 = (xa + xc) */
- R1 = __SSAT(T1 + S1, 16U);
- /* S0 = (ya - yc) */
- S0 = __SSAT(T0 - S0, 16);
- /* S1 = (xa - xc) */
- S1 = __SSAT(T1 - S1, 16);
- /* Reading i0+fftLen/4 , i0+3fftLen/4 inputs */
- /* input is down scale by 4 to avoid overflow */
- /* Read yb (real), xb(imag) input */
- T0 = pSrc16[i1 * 2U] >> 2U;
- T1 = pSrc16[(i1 * 2U) + 1U] >> 2U;
- /* input is down scale by 4 to avoid overflow */
- /* Read yd (real), xd(imag) input */
- U0 = pSrc16[i3 * 2U] >> 2U;
- U1 = pSrc16[(i3 * 2U) + 1] >> 2U;
- /* T0 = (yb + yd) */
- T0 = __SSAT(T0 + U0, 16U);
- /* T1 = (xb + xd) */
- T1 = __SSAT(T1 + U1, 16U);
- /* writing the butterfly processed i0 sample */
- /* ya' = ya + yb + yc + yd */
- /* xa' = xa + xb + xc + xd */
- pSrc16[i0 * 2U] = (R0 >> 1U) + (T0 >> 1U);
- pSrc16[(i0 * 2U) + 1U] = (R1 >> 1U) + (T1 >> 1U);
- /* R0 = (ya + yc) - (yb + yd) */
- /* R1 = (xa + xc) - (xb + xd) */
- R0 = __SSAT(R0 - T0, 16U);
- R1 = __SSAT(R1 - T1, 16U);
- /* co2 & si2 are read from Coefficient pointer */
- Co2 = pCoef16[2U * ic * 2U];
- Si2 = pCoef16[(2U * ic * 2U) + 1];
- /* xc' = (xa-xb+xc-xd)* co2 + (ya-yb+yc-yd)* (si2) */
- out1 = (q15_t) ((Co2 * R0 + Si2 * R1) >> 16U);
- /* yc' = (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) */
- out2 = (q15_t) ((-Si2 * R0 + Co2 * R1) >> 16U);
- /* Reading i0+fftLen/4 */
- /* input is down scale by 4 to avoid overflow */
- /* T0 = yb, T1 = xb */
- T0 = pSrc16[i1 * 2U] >> 2;
- T1 = pSrc16[(i1 * 2U) + 1] >> 2;
- /* writing the butterfly processed i0 + fftLen/4 sample */
- /* writing output(xc', yc') in little endian format */
- pSrc16[i1 * 2U] = out1;
- pSrc16[(i1 * 2U) + 1] = out2;
- /* Butterfly calculations */
- /* input is down scale by 4 to avoid overflow */
- /* U0 = yd, U1 = xd */
- U0 = pSrc16[i3 * 2U] >> 2;
- U1 = pSrc16[(i3 * 2U) + 1] >> 2;
- /* T0 = yb-yd */
- T0 = __SSAT(T0 - U0, 16);
- /* T1 = xb-xd */
- T1 = __SSAT(T1 - U1, 16);
- /* R1 = (ya-yc) + (xb- xd), R0 = (xa-xc) - (yb-yd)) */
- R0 = (q15_t) __SSAT((q31_t) (S0 - T1), 16);
- R1 = (q15_t) __SSAT((q31_t) (S1 + T0), 16);
- /* S1 = (ya-yc) - (xb- xd), S0 = (xa-xc) + (yb-yd)) */
- S0 = (q15_t) __SSAT(((q31_t) S0 + T1), 16U);
- S1 = (q15_t) __SSAT(((q31_t) S1 - T0), 16U);
- /* co1 & si1 are read from Coefficient pointer */
- Co1 = pCoef16[ic * 2U];
- Si1 = pCoef16[(ic * 2U) + 1];
- /* Butterfly process for the i0+fftLen/2 sample */
- /* xb' = (xa+yb-xc-yd)* co1 + (ya-xb-yc+xd)* (si1) */
- out1 = (q15_t) ((Si1 * S1 + Co1 * S0) >> 16);
- /* yb' = (ya-xb-yc+xd)* co1 - (xa+yb-xc-yd)* (si1) */
- out2 = (q15_t) ((-Si1 * S0 + Co1 * S1) >> 16);
- /* writing output(xb', yb') in little endian format */
- pSrc16[i2 * 2U] = out1;
- pSrc16[(i2 * 2U) + 1] = out2;
- /* Co3 & si3 are read from Coefficient pointer */
- Co3 = pCoef16[3U * (ic * 2U)];
- Si3 = pCoef16[(3U * (ic * 2U)) + 1];
- /* Butterfly process for the i0+3fftLen/4 sample */
- /* xd' = (xa-yb-xc+yd)* Co3 + (ya+xb-yc-xd)* (si3) */
- out1 = (q15_t) ((Si3 * R1 + Co3 * R0) >> 16U);
- /* yd' = (ya+xb-yc-xd)* Co3 - (xa-yb-xc+yd)* (si3) */
- out2 = (q15_t) ((-Si3 * R0 + Co3 * R1) >> 16U);
- /* writing output(xd', yd') in little endian format */
- pSrc16[i3 * 2U] = out1;
- pSrc16[(i3 * 2U) + 1] = out2;
- /* Twiddle coefficients index modifier */
- ic = ic + twidCoefModifier;
- /* Updating input index */
- i0 = i0 + 1U;
- } while (--j);
- /* data is in 4.11(q11) format */
- /* end of first stage process */
- /* start of middle stage process */
- /* Twiddle coefficients index modifier */
- twidCoefModifier <<= 2U;
- /* Calculation of Middle stage */
- for (k = fftLen / 4U; k > 4U; k >>= 2U)
- {
- /* Initializations for the middle stage */
- n1 = n2;
- n2 >>= 2U;
- ic = 0U;
- for (j = 0U; j <= (n2 - 1U); j++)
- {
- /* index calculation for the coefficients */
- Co1 = pCoef16[ic * 2U];
- Si1 = pCoef16[(ic * 2U) + 1U];
- Co2 = pCoef16[2U * (ic * 2U)];
- Si2 = pCoef16[(2U * (ic * 2U)) + 1U];
- Co3 = pCoef16[3U * (ic * 2U)];
- Si3 = pCoef16[(3U * (ic * 2U)) + 1U];
- /* Twiddle coefficients index modifier */
- ic = ic + twidCoefModifier;
- /* Butterfly implementation */
- for (i0 = j; i0 < fftLen; i0 += n1)
- {
- /* index calculation for the input as, */
- /* pSrc16[i0 + 0], pSrc16[i0 + fftLen/4], pSrc16[i0 + fftLen/2], pSrc16[i0 + 3fftLen/4] */
- i1 = i0 + n2;
- i2 = i1 + n2;
- i3 = i2 + n2;
- /* Reading i0, i0+fftLen/2 inputs */
- /* Read ya (real), xa(imag) input */
- T0 = pSrc16[i0 * 2U];
- T1 = pSrc16[(i0 * 2U) + 1U];
- /* Read yc (real), xc(imag) input */
- S0 = pSrc16[i2 * 2U];
- S1 = pSrc16[(i2 * 2U) + 1U];
- /* R0 = (ya + yc), R1 = (xa + xc) */
- R0 = __SSAT(T0 + S0, 16);
- R1 = __SSAT(T1 + S1, 16);
- /* S0 = (ya - yc), S1 =(xa - xc) */
- S0 = __SSAT(T0 - S0, 16);
- S1 = __SSAT(T1 - S1, 16);
- /* Reading i0+fftLen/4 , i0+3fftLen/4 inputs */
- /* Read yb (real), xb(imag) input */
- T0 = pSrc16[i1 * 2U];
- T1 = pSrc16[(i1 * 2U) + 1U];
- /* Read yd (real), xd(imag) input */
- U0 = pSrc16[i3 * 2U];
- U1 = pSrc16[(i3 * 2U) + 1U];
- /* T0 = (yb + yd), T1 = (xb + xd) */
- T0 = __SSAT(T0 + U0, 16);
- T1 = __SSAT(T1 + U1, 16);
- /* writing the butterfly processed i0 sample */
- /* xa' = xa + xb + xc + xd */
- /* ya' = ya + yb + yc + yd */
- out1 = ((R0 >> 1U) + (T0 >> 1U)) >> 1U;
- out2 = ((R1 >> 1U) + (T1 >> 1U)) >> 1U;
- pSrc16[i0 * 2U] = out1;
- pSrc16[(2U * i0) + 1U] = out2;
- /* R0 = (ya + yc) - (yb + yd), R1 = (xa + xc) - (xb + xd) */
- R0 = (R0 >> 1U) - (T0 >> 1U);
- R1 = (R1 >> 1U) - (T1 >> 1U);
- /* (ya-yb+yc-yd)* (si2) + (xa-xb+xc-xd)* co2 */
- out1 = (q15_t) ((Co2 * R0 + Si2 * R1) >> 16U);
- /* (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) */
- out2 = (q15_t) ((-Si2 * R0 + Co2 * R1) >> 16U);
- /* Reading i0+3fftLen/4 */
- /* Read yb (real), xb(imag) input */
- T0 = pSrc16[i1 * 2U];
- T1 = pSrc16[(i1 * 2U) + 1U];
- /* writing the butterfly processed i0 + fftLen/4 sample */
- /* xc' = (xa-xb+xc-xd)* co2 + (ya-yb+yc-yd)* (si2) */
- /* yc' = (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) */
- pSrc16[i1 * 2U] = out1;
- pSrc16[(i1 * 2U) + 1U] = out2;
- /* Butterfly calculations */
- /* Read yd (real), xd(imag) input */
- U0 = pSrc16[i3 * 2U];
- U1 = pSrc16[(i3 * 2U) + 1U];
- /* T0 = yb-yd, T1 = xb-xd */
- T0 = __SSAT(T0 - U0, 16);
- T1 = __SSAT(T1 - U1, 16);
- /* R0 = (ya-yc) + (xb- xd), R1 = (xa-xc) - (yb-yd)) */
- R0 = (S0 >> 1U) - (T1 >> 1U);
- R1 = (S1 >> 1U) + (T0 >> 1U);
- /* S0 = (ya-yc) - (xb- xd), S1 = (xa-xc) + (yb-yd)) */
- S0 = (S0 >> 1U) + (T1 >> 1U);
- S1 = (S1 >> 1U) - (T0 >> 1U);
- /* Butterfly process for the i0+fftLen/2 sample */
- out1 = (q15_t) ((Co1 * S0 + Si1 * S1) >> 16U);
- out2 = (q15_t) ((-Si1 * S0 + Co1 * S1) >> 16U);
- /* xb' = (xa+yb-xc-yd)* co1 + (ya-xb-yc+xd)* (si1) */
- /* yb' = (ya-xb-yc+xd)* co1 - (xa+yb-xc-yd)* (si1) */
- pSrc16[i2 * 2U] = out1;
- pSrc16[(i2 * 2U) + 1U] = out2;
- /* Butterfly process for the i0+3fftLen/4 sample */
- out1 = (q15_t) ((Si3 * R1 + Co3 * R0) >> 16U);
- out2 = (q15_t) ((-Si3 * R0 + Co3 * R1) >> 16U);
- /* xd' = (xa-yb-xc+yd)* Co3 + (ya+xb-yc-xd)* (si3) */
- /* yd' = (ya+xb-yc-xd)* Co3 - (xa-yb-xc+yd)* (si3) */
- pSrc16[i3 * 2U] = out1;
- pSrc16[(i3 * 2U) + 1U] = out2;
- }
- }
- /* Twiddle coefficients index modifier */
- twidCoefModifier <<= 2U;
- }
- /* end of middle stage process */
- /* data is in 10.6(q6) format for the 1024 point */
- /* data is in 8.8(q8) format for the 256 point */
- /* data is in 6.10(q10) format for the 64 point */
- /* data is in 4.12(q12) format for the 16 point */
- /* Initializations for the last stage */
- n1 = n2;
- n2 >>= 2U;
- /* start of last stage process */
- /* Butterfly implementation */
- for (i0 = 0U; i0 <= (fftLen - n1); i0 += n1)
- {
- /* index calculation for the input as, */
- /* pSrc16[i0 + 0], pSrc16[i0 + fftLen/4], pSrc16[i0 + fftLen/2], pSrc16[i0 + 3fftLen/4] */
- i1 = i0 + n2;
- i2 = i1 + n2;
- i3 = i2 + n2;
- /* Reading i0, i0+fftLen/2 inputs */
- /* Read ya (real), xa(imag) input */
- T0 = pSrc16[i0 * 2U];
- T1 = pSrc16[(i0 * 2U) + 1U];
- /* Read yc (real), xc(imag) input */
- S0 = pSrc16[i2 * 2U];
- S1 = pSrc16[(i2 * 2U) + 1U];
- /* R0 = (ya + yc), R1 = (xa + xc) */
- R0 = __SSAT(T0 + S0, 16U);
- R1 = __SSAT(T1 + S1, 16U);
- /* S0 = (ya - yc), S1 = (xa - xc) */
- S0 = __SSAT(T0 - S0, 16U);
- S1 = __SSAT(T1 - S1, 16U);
- /* Reading i0+fftLen/4 , i0+3fftLen/4 inputs */
- /* Read yb (real), xb(imag) input */
- T0 = pSrc16[i1 * 2U];
- T1 = pSrc16[(i1 * 2U) + 1U];
- /* Read yd (real), xd(imag) input */
- U0 = pSrc16[i3 * 2U];
- U1 = pSrc16[(i3 * 2U) + 1U];
- /* T0 = (yb + yd), T1 = (xb + xd)) */
- T0 = __SSAT(T0 + U0, 16U);
- T1 = __SSAT(T1 + U1, 16U);
- /* writing the butterfly processed i0 sample */
- /* xa' = xa + xb + xc + xd */
- /* ya' = ya + yb + yc + yd */
- pSrc16[i0 * 2U] = (R0 >> 1U) + (T0 >> 1U);
- pSrc16[(i0 * 2U) + 1U] = (R1 >> 1U) + (T1 >> 1U);
- /* R0 = (ya + yc) - (yb + yd), R1 = (xa + xc) - (xb + xd) */
- R0 = (R0 >> 1U) - (T0 >> 1U);
- R1 = (R1 >> 1U) - (T1 >> 1U);
- /* Read yb (real), xb(imag) input */
- T0 = pSrc16[i1 * 2U];
- T1 = pSrc16[(i1 * 2U) + 1U];
- /* writing the butterfly processed i0 + fftLen/4 sample */
- /* xc' = (xa-xb+xc-xd) */
- /* yc' = (ya-yb+yc-yd) */
- pSrc16[i1 * 2U] = R0;
- pSrc16[(i1 * 2U) + 1U] = R1;
- /* Read yd (real), xd(imag) input */
- U0 = pSrc16[i3 * 2U];
- U1 = pSrc16[(i3 * 2U) + 1U];
- /* T0 = (yb - yd), T1 = (xb - xd) */
- T0 = __SSAT(T0 - U0, 16U);
- T1 = __SSAT(T1 - U1, 16U);
- /* writing the butterfly processed i0 + fftLen/2 sample */
- /* xb' = (xa+yb-xc-yd) */
- /* yb' = (ya-xb-yc+xd) */
- pSrc16[i2 * 2U] = (S0 >> 1U) + (T1 >> 1U);
- pSrc16[(i2 * 2U) + 1U] = (S1 >> 1U) - (T0 >> 1U);
- /* writing the butterfly processed i0 + 3fftLen/4 sample */
- /* xd' = (xa-yb-xc+yd) */
- /* yd' = (ya+xb-yc-xd) */
- pSrc16[i3 * 2U] = (S0 >> 1U) - (T1 >> 1U);
- pSrc16[(i3 * 2U) + 1U] = (S1 >> 1U) + (T0 >> 1U);
- }
- /* end of last stage process */
- /* output is in 11.5(q5) format for the 1024 point */
- /* output is in 9.7(q7) format for the 256 point */
- /* output is in 7.9(q9) format for the 64 point */
- /* output is in 5.11(q11) format for the 16 point */
- #endif /* #if defined (ARM_MATH_DSP) */
- }
- /**
- * @brief Core function for the Q15 CIFFT butterfly process.
- * @param[in, out] *pSrc16 points to the in-place buffer of Q15 data type.
- * @param[in] fftLen length of the FFT.
- * @param[in] *pCoef16 points to twiddle coefficient buffer.
- * @param[in] twidCoefModifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
- * @return none.
- */
- /*
- * Radix-4 IFFT algorithm used is :
- *
- * CIFFT uses same twiddle coefficients as CFFT function
- * x[k] = x[n] + (j)k * x[n + fftLen/4] + (-1)k * x[n+fftLen/2] + (-j)k * x[n+3*fftLen/4]
- *
- *
- * IFFT is implemented with following changes in equations from FFT
- *
- * Input real and imaginary data:
- * x(n) = xa + j * ya
- * x(n+N/4 ) = xb + j * yb
- * x(n+N/2 ) = xc + j * yc
- * x(n+3N 4) = xd + j * yd
- *
- *
- * Output real and imaginary data:
- * x(4r) = xa'+ j * ya'
- * x(4r+1) = xb'+ j * yb'
- * x(4r+2) = xc'+ j * yc'
- * x(4r+3) = xd'+ j * yd'
- *
- *
- * Twiddle factors for radix-4 IFFT:
- * Wn = co1 + j * (si1)
- * W2n = co2 + j * (si2)
- * W3n = co3 + j * (si3)
- * The real and imaginary output values for the radix-4 butterfly are
- * xa' = xa + xb + xc + xd
- * ya' = ya + yb + yc + yd
- * xb' = (xa-yb-xc+yd)* co1 - (ya+xb-yc-xd)* (si1)
- * yb' = (ya+xb-yc-xd)* co1 + (xa-yb-xc+yd)* (si1)
- * xc' = (xa-xb+xc-xd)* co2 - (ya-yb+yc-yd)* (si2)
- * yc' = (ya-yb+yc-yd)* co2 + (xa-xb+xc-xd)* (si2)
- * xd' = (xa+yb-xc-yd)* co3 - (ya-xb-yc+xd)* (si3)
- * yd' = (ya-xb-yc+xd)* co3 + (xa+yb-xc-yd)* (si3)
- *
- */
- void arm_radix4_butterfly_inverse_q15(
- q15_t * pSrc16,
- uint32_t fftLen,
- q15_t * pCoef16,
- uint32_t twidCoefModifier)
- {
- #if defined (ARM_MATH_DSP)
- /* Run the below code for Cortex-M4 and Cortex-M3 */
- q31_t R, S, T, U;
- q31_t C1, C2, C3, out1, out2;
- uint32_t n1, n2, ic, i0, j, k;
- q15_t *ptr1;
- q15_t *pSi0;
- q15_t *pSi1;
- q15_t *pSi2;
- q15_t *pSi3;
- q31_t xaya, xbyb, xcyc, xdyd;
- /* Total process is divided into three stages */
- /* process first stage, middle stages, & last stage */
- /* Initializations for the first stage */
- n2 = fftLen;
- n1 = n2;
- /* n2 = fftLen/4 */
- n2 >>= 2U;
- /* Index for twiddle coefficient */
- ic = 0U;
- /* Index for input read and output write */
- j = n2;
- pSi0 = pSrc16;
- pSi1 = pSi0 + 2 * n2;
- pSi2 = pSi1 + 2 * n2;
- pSi3 = pSi2 + 2 * n2;
- /* Input is in 1.15(q15) format */
- /* start of first stage process */
- do
- {
- /* Butterfly implementation */
- /* Reading i0, i0+fftLen/2 inputs */
- /* Read ya (real), xa(imag) input */
- T = _SIMD32_OFFSET(pSi0);
- T = __SHADD16(T, 0);
- T = __SHADD16(T, 0);
- /* Read yc (real), xc(imag) input */
- S = _SIMD32_OFFSET(pSi2);
- S = __SHADD16(S, 0);
- S = __SHADD16(S, 0);
- /* R = packed((ya + yc), (xa + xc) ) */
- R = __QADD16(T, S);
- /* S = packed((ya - yc), (xa - xc) ) */
- S = __QSUB16(T, S);
- /* Reading i0+fftLen/4 , i0+3fftLen/4 inputs */
- /* Read yb (real), xb(imag) input */
- T = _SIMD32_OFFSET(pSi1);
- T = __SHADD16(T, 0);
- T = __SHADD16(T, 0);
- /* Read yd (real), xd(imag) input */
- U = _SIMD32_OFFSET(pSi3);
- U = __SHADD16(U, 0);
- U = __SHADD16(U, 0);
- /* T = packed((yb + yd), (xb + xd) ) */
- T = __QADD16(T, U);
- /* writing the butterfly processed i0 sample */
- /* xa' = xa + xb + xc + xd */
- /* ya' = ya + yb + yc + yd */
- _SIMD32_OFFSET(pSi0) = __SHADD16(R, T);
- pSi0 += 2;
- /* R = packed((ya + yc) - (yb + yd), (xa + xc)- (xb + xd)) */
- R = __QSUB16(R, T);
- /* co2 & si2 are read from SIMD Coefficient pointer */
- C2 = _SIMD32_OFFSET(pCoef16 + (4U * ic));
- #ifndef ARM_MATH_BIG_ENDIAN
- /* xc' = (xa-xb+xc-xd)* co2 + (ya-yb+yc-yd)* (si2) */
- out1 = __SMUSD(C2, R) >> 16U;
- /* yc' = (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) */
- out2 = __SMUADX(C2, R);
- #else
- /* xc' = (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) */
- out1 = __SMUADX(C2, R) >> 16U;
- /* yc' = (xa-xb+xc-xd)* co2 + (ya-yb+yc-yd)* (si2) */
- out2 = __SMUSD(__QSUB16(0, C2), R);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- /* Reading i0+fftLen/4 */
- /* T = packed(yb, xb) */
- T = _SIMD32_OFFSET(pSi1);
- T = __SHADD16(T, 0);
- T = __SHADD16(T, 0);
- /* writing the butterfly processed i0 + fftLen/4 sample */
- /* writing output(xc', yc') in little endian format */
- _SIMD32_OFFSET(pSi1) =
- (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
- pSi1 += 2;
- /* Butterfly calculations */
- /* U = packed(yd, xd) */
- U = _SIMD32_OFFSET(pSi3);
- U = __SHADD16(U, 0);
- U = __SHADD16(U, 0);
- /* T = packed(yb-yd, xb-xd) */
- T = __QSUB16(T, U);
- #ifndef ARM_MATH_BIG_ENDIAN
- /* R = packed((ya-yc) + (xb- xd) , (xa-xc) - (yb-yd)) */
- R = __QSAX(S, T);
- /* S = packed((ya-yc) + (xb- xd), (xa-xc) - (yb-yd)) */
- S = __QASX(S, T);
- #else
- /* R = packed((ya-yc) + (xb- xd) , (xa-xc) - (yb-yd)) */
- R = __QASX(S, T);
- /* S = packed((ya-yc) - (xb- xd), (xa-xc) + (yb-yd)) */
- S = __QSAX(S, T);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- /* co1 & si1 are read from SIMD Coefficient pointer */
- C1 = _SIMD32_OFFSET(pCoef16 + (2U * ic));
- /* Butterfly process for the i0+fftLen/2 sample */
- #ifndef ARM_MATH_BIG_ENDIAN
- /* xb' = (xa+yb-xc-yd)* co1 + (ya-xb-yc+xd)* (si1) */
- out1 = __SMUSD(C1, S) >> 16U;
- /* yb' = (ya-xb-yc+xd)* co1 - (xa+yb-xc-yd)* (si1) */
- out2 = __SMUADX(C1, S);
- #else
- /* xb' = (ya-xb-yc+xd)* co1 - (xa+yb-xc-yd)* (si1) */
- out1 = __SMUADX(C1, S) >> 16U;
- /* yb' = (xa+yb-xc-yd)* co1 + (ya-xb-yc+xd)* (si1) */
- out2 = __SMUSD(__QSUB16(0, C1), S);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- /* writing output(xb', yb') in little endian format */
- _SIMD32_OFFSET(pSi2) =
- ((out2) & 0xFFFF0000) | ((out1) & 0x0000FFFF);
- pSi2 += 2;
- /* co3 & si3 are read from SIMD Coefficient pointer */
- C3 = _SIMD32_OFFSET(pCoef16 + (6U * ic));
- /* Butterfly process for the i0+3fftLen/4 sample */
- #ifndef ARM_MATH_BIG_ENDIAN
- /* xd' = (xa-yb-xc+yd)* co3 + (ya+xb-yc-xd)* (si3) */
- out1 = __SMUSD(C3, R) >> 16U;
- /* yd' = (ya+xb-yc-xd)* co3 - (xa-yb-xc+yd)* (si3) */
- out2 = __SMUADX(C3, R);
- #else
- /* xd' = (ya+xb-yc-xd)* co3 - (xa-yb-xc+yd)* (si3) */
- out1 = __SMUADX(C3, R) >> 16U;
- /* yd' = (xa-yb-xc+yd)* co3 + (ya+xb-yc-xd)* (si3) */
- out2 = __SMUSD(__QSUB16(0, C3), R);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- /* writing output(xd', yd') in little endian format */
- _SIMD32_OFFSET(pSi3) =
- ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
- pSi3 += 2;
- /* Twiddle coefficients index modifier */
- ic = ic + twidCoefModifier;
- } while (--j);
- /* data is in 4.11(q11) format */
- /* end of first stage process */
- /* start of middle stage process */
- /* Twiddle coefficients index modifier */
- twidCoefModifier <<= 2U;
- /* Calculation of Middle stage */
- for (k = fftLen / 4U; k > 4U; k >>= 2U)
- {
- /* Initializations for the middle stage */
- n1 = n2;
- n2 >>= 2U;
- ic = 0U;
- for (j = 0U; j <= (n2 - 1U); j++)
- {
- /* index calculation for the coefficients */
- C1 = _SIMD32_OFFSET(pCoef16 + (2U * ic));
- C2 = _SIMD32_OFFSET(pCoef16 + (4U * ic));
- C3 = _SIMD32_OFFSET(pCoef16 + (6U * ic));
- /* Twiddle coefficients index modifier */
- ic = ic + twidCoefModifier;
- pSi0 = pSrc16 + 2 * j;
- pSi1 = pSi0 + 2 * n2;
- pSi2 = pSi1 + 2 * n2;
- pSi3 = pSi2 + 2 * n2;
- /* Butterfly implementation */
- for (i0 = j; i0 < fftLen; i0 += n1)
- {
- /* Reading i0, i0+fftLen/2 inputs */
- /* Read ya (real), xa(imag) input */
- T = _SIMD32_OFFSET(pSi0);
- /* Read yc (real), xc(imag) input */
- S = _SIMD32_OFFSET(pSi2);
- /* R = packed( (ya + yc), (xa + xc)) */
- R = __QADD16(T, S);
- /* S = packed((ya - yc), (xa - xc)) */
- S = __QSUB16(T, S);
- /* Reading i0+fftLen/4 , i0+3fftLen/4 inputs */
- /* Read yb (real), xb(imag) input */
- T = _SIMD32_OFFSET(pSi1);
- /* Read yd (real), xd(imag) input */
- U = _SIMD32_OFFSET(pSi3);
- /* T = packed( (yb + yd), (xb + xd)) */
- T = __QADD16(T, U);
- /* writing the butterfly processed i0 sample */
- /* xa' = xa + xb + xc + xd */
- /* ya' = ya + yb + yc + yd */
- out1 = __SHADD16(R, T);
- out1 = __SHADD16(out1, 0);
- _SIMD32_OFFSET(pSi0) = out1;
- pSi0 += 2 * n1;
- /* R = packed( (ya + yc) - (yb + yd), (xa + xc) - (xb + xd)) */
- R = __SHSUB16(R, T);
- #ifndef ARM_MATH_BIG_ENDIAN
- /* (ya-yb+yc-yd)* (si2) + (xa-xb+xc-xd)* co2 */
- out1 = __SMUSD(C2, R) >> 16U;
- /* (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) */
- out2 = __SMUADX(C2, R);
- #else
- /* (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) */
- out1 = __SMUADX(R, C2) >> 16U;
- /* (ya-yb+yc-yd)* (si2) + (xa-xb+xc-xd)* co2 */
- out2 = __SMUSD(__QSUB16(0, C2), R);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- /* Reading i0+3fftLen/4 */
- /* Read yb (real), xb(imag) input */
- T = _SIMD32_OFFSET(pSi1);
- /* writing the butterfly processed i0 + fftLen/4 sample */
- /* xc' = (xa-xb+xc-xd)* co2 + (ya-yb+yc-yd)* (si2) */
- /* yc' = (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) */
- _SIMD32_OFFSET(pSi1) =
- ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
- pSi1 += 2 * n1;
- /* Butterfly calculations */
- /* Read yd (real), xd(imag) input */
- U = _SIMD32_OFFSET(pSi3);
- /* T = packed(yb-yd, xb-xd) */
- T = __QSUB16(T, U);
- #ifndef ARM_MATH_BIG_ENDIAN
- /* R = packed((ya-yc) + (xb- xd) , (xa-xc) - (yb-yd)) */
- R = __SHSAX(S, T);
- /* S = packed((ya-yc) - (xb- xd), (xa-xc) + (yb-yd)) */
- S = __SHASX(S, T);
- /* Butterfly process for the i0+fftLen/2 sample */
- out1 = __SMUSD(C1, S) >> 16U;
- out2 = __SMUADX(C1, S);
- #else
- /* R = packed((ya-yc) + (xb- xd) , (xa-xc) - (yb-yd)) */
- R = __SHASX(S, T);
- /* S = packed((ya-yc) - (xb- xd), (xa-xc) + (yb-yd)) */
- S = __SHSAX(S, T);
- /* Butterfly process for the i0+fftLen/2 sample */
- out1 = __SMUADX(S, C1) >> 16U;
- out2 = __SMUSD(__QSUB16(0, C1), S);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- /* xb' = (xa+yb-xc-yd)* co1 + (ya-xb-yc+xd)* (si1) */
- /* yb' = (ya-xb-yc+xd)* co1 - (xa+yb-xc-yd)* (si1) */
- _SIMD32_OFFSET(pSi2) =
- ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
- pSi2 += 2 * n1;
- /* Butterfly process for the i0+3fftLen/4 sample */
- #ifndef ARM_MATH_BIG_ENDIAN
- out1 = __SMUSD(C3, R) >> 16U;
- out2 = __SMUADX(C3, R);
- #else
- out1 = __SMUADX(C3, R) >> 16U;
- out2 = __SMUSD(__QSUB16(0, C3), R);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- /* xd' = (xa-yb-xc+yd)* co3 + (ya+xb-yc-xd)* (si3) */
- /* yd' = (ya+xb-yc-xd)* co3 - (xa-yb-xc+yd)* (si3) */
- _SIMD32_OFFSET(pSi3) =
- ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
- pSi3 += 2 * n1;
- }
- }
- /* Twiddle coefficients index modifier */
- twidCoefModifier <<= 2U;
- }
- /* end of middle stage process */
- /* data is in 10.6(q6) format for the 1024 point */
- /* data is in 8.8(q8) format for the 256 point */
- /* data is in 6.10(q10) format for the 64 point */
- /* data is in 4.12(q12) format for the 16 point */
- /* Initializations for the last stage */
- j = fftLen >> 2;
- ptr1 = &pSrc16[0];
- /* start of last stage process */
- /* Butterfly implementation */
- do
- {
- /* Read xa (real), ya(imag) input */
- xaya = *__SIMD32(ptr1)++;
- /* Read xb (real), yb(imag) input */
- xbyb = *__SIMD32(ptr1)++;
- /* Read xc (real), yc(imag) input */
- xcyc = *__SIMD32(ptr1)++;
- /* Read xd (real), yd(imag) input */
- xdyd = *__SIMD32(ptr1)++;
- /* R = packed((ya + yc), (xa + xc)) */
- R = __QADD16(xaya, xcyc);
- /* T = packed((yb + yd), (xb + xd)) */
- T = __QADD16(xbyb, xdyd);
- /* pointer updation for writing */
- ptr1 = ptr1 - 8U;
- /* xa' = xa + xb + xc + xd */
- /* ya' = ya + yb + yc + yd */
- *__SIMD32(ptr1)++ = __SHADD16(R, T);
- /* T = packed((yb + yd), (xb + xd)) */
- T = __QADD16(xbyb, xdyd);
- /* xc' = (xa-xb+xc-xd) */
- /* yc' = (ya-yb+yc-yd) */
- *__SIMD32(ptr1)++ = __SHSUB16(R, T);
- /* S = packed((ya - yc), (xa - xc)) */
- S = __QSUB16(xaya, xcyc);
- /* Read yd (real), xd(imag) input */
- /* T = packed( (yb - yd), (xb - xd)) */
- U = __QSUB16(xbyb, xdyd);
- #ifndef ARM_MATH_BIG_ENDIAN
- /* xb' = (xa+yb-xc-yd) */
- /* yb' = (ya-xb-yc+xd) */
- *__SIMD32(ptr1)++ = __SHASX(S, U);
- /* xd' = (xa-yb-xc+yd) */
- /* yd' = (ya+xb-yc-xd) */
- *__SIMD32(ptr1)++ = __SHSAX(S, U);
- #else
- /* xb' = (xa+yb-xc-yd) */
- /* yb' = (ya-xb-yc+xd) */
- *__SIMD32(ptr1)++ = __SHSAX(S, U);
- /* xd' = (xa-yb-xc+yd) */
- /* yd' = (ya+xb-yc-xd) */
- *__SIMD32(ptr1)++ = __SHASX(S, U);
- #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
- } while (--j);
- /* end of last stage process */
- /* output is in 11.5(q5) format for the 1024 point */
- /* output is in 9.7(q7) format for the 256 point */
- /* output is in 7.9(q9) format for the 64 point */
- /* output is in 5.11(q11) format for the 16 point */
- #else
- /* Run the below code for Cortex-M0 */
- q15_t R0, R1, S0, S1, T0, T1, U0, U1;
- q15_t Co1, Si1, Co2, Si2, Co3, Si3, out1, out2;
- uint32_t n1, n2, ic, i0, i1, i2, i3, j, k;
- /* Total process is divided into three stages */
- /* process first stage, middle stages, & last stage */
- /* Initializations for the first stage */
- n2 = fftLen;
- n1 = n2;
- /* n2 = fftLen/4 */
- n2 >>= 2U;
- /* Index for twiddle coefficient */
- ic = 0U;
- /* Index for input read and output write */
- i0 = 0U;
- j = n2;
- /* Input is in 1.15(q15) format */
- /* Start of first stage process */
- do
- {
- /* Butterfly implementation */
- /* index calculation for the input as, */
- /* pSrc16[i0 + 0], pSrc16[i0 + fftLen/4], pSrc16[i0 + fftLen/2], pSrc16[i0 + 3fftLen/4] */
- i1 = i0 + n2;
- i2 = i1 + n2;
- i3 = i2 + n2;
- /* Reading i0, i0+fftLen/2 inputs */
- /* input is down scale by 4 to avoid overflow */
- /* Read ya (real), xa(imag) input */
- T0 = pSrc16[i0 * 2U] >> 2U;
- T1 = pSrc16[(i0 * 2U) + 1U] >> 2U;
- /* input is down scale by 4 to avoid overflow */
- /* Read yc (real), xc(imag) input */
- S0 = pSrc16[i2 * 2U] >> 2U;
- S1 = pSrc16[(i2 * 2U) + 1U] >> 2U;
- /* R0 = (ya + yc), R1 = (xa + xc) */
- R0 = __SSAT(T0 + S0, 16U);
- R1 = __SSAT(T1 + S1, 16U);
- /* S0 = (ya - yc), S1 = (xa - xc) */
- S0 = __SSAT(T0 - S0, 16U);
- S1 = __SSAT(T1 - S1, 16U);
- /* Reading i0+fftLen/4 , i0+3fftLen/4 inputs */
- /* input is down scale by 4 to avoid overflow */
- /* Read yb (real), xb(imag) input */
- T0 = pSrc16[i1 * 2U] >> 2U;
- T1 = pSrc16[(i1 * 2U) + 1U] >> 2U;
- /* Read yd (real), xd(imag) input */
- /* input is down scale by 4 to avoid overflow */
- U0 = pSrc16[i3 * 2U] >> 2U;
- U1 = pSrc16[(i3 * 2U) + 1U] >> 2U;
- /* T0 = (yb + yd), T1 = (xb + xd) */
- T0 = __SSAT(T0 + U0, 16U);
- T1 = __SSAT(T1 + U1, 16U);
- /* writing the butterfly processed i0 sample */
- /* xa' = xa + xb + xc + xd */
- /* ya' = ya + yb + yc + yd */
- pSrc16[i0 * 2U] = (R0 >> 1U) + (T0 >> 1U);
- pSrc16[(i0 * 2U) + 1U] = (R1 >> 1U) + (T1 >> 1U);
- /* R0 = (ya + yc) - (yb + yd), R1 = (xa + xc)- (xb + xd) */
- R0 = __SSAT(R0 - T0, 16U);
- R1 = __SSAT(R1 - T1, 16U);
- /* co2 & si2 are read from Coefficient pointer */
- Co2 = pCoef16[2U * ic * 2U];
- Si2 = pCoef16[(2U * ic * 2U) + 1U];
- /* xc' = (xa-xb+xc-xd)* co2 - (ya-yb+yc-yd)* (si2) */
- out1 = (q15_t) ((Co2 * R0 - Si2 * R1) >> 16U);
- /* yc' = (ya-yb+yc-yd)* co2 + (xa-xb+xc-xd)* (si2) */
- out2 = (q15_t) ((Si2 * R0 + Co2 * R1) >> 16U);
- /* Reading i0+fftLen/4 */
- /* input is down scale by 4 to avoid overflow */
- /* T0 = yb, T1 = xb */
- T0 = pSrc16[i1 * 2U] >> 2U;
- T1 = pSrc16[(i1 * 2U) + 1U] >> 2U;
- /* writing the butterfly processed i0 + fftLen/4 sample */
- /* writing output(xc', yc') in little endian format */
- pSrc16[i1 * 2U] = out1;
- pSrc16[(i1 * 2U) + 1U] = out2;
- /* Butterfly calculations */
- /* input is down scale by 4 to avoid overflow */
- /* U0 = yd, U1 = xd) */
- U0 = pSrc16[i3 * 2U] >> 2U;
- U1 = pSrc16[(i3 * 2U) + 1U] >> 2U;
- /* T0 = yb-yd, T1 = xb-xd) */
- T0 = __SSAT(T0 - U0, 16U);
- T1 = __SSAT(T1 - U1, 16U);
- /* R0 = (ya-yc) - (xb- xd) , R1 = (xa-xc) + (yb-yd) */
- R0 = (q15_t) __SSAT((q31_t) (S0 + T1), 16);
- R1 = (q15_t) __SSAT((q31_t) (S1 - T0), 16);
- /* S = (ya-yc) + (xb- xd), S1 = (xa-xc) - (yb-yd) */
- S0 = (q15_t) __SSAT((q31_t) (S0 - T1), 16);
- S1 = (q15_t) __SSAT((q31_t) (S1 + T0), 16);
- /* co1 & si1 are read from Coefficient pointer */
- Co1 = pCoef16[ic * 2U];
- Si1 = pCoef16[(ic * 2U) + 1U];
- /* Butterfly process for the i0+fftLen/2 sample */
- /* xb' = (xa-yb-xc+yd)* co1 - (ya+xb-yc-xd)* (si1) */
- out1 = (q15_t) ((Co1 * S0 - Si1 * S1) >> 16U);
- /* yb' = (ya+xb-yc-xd)* co1 + (xa-yb-xc+yd)* (si1) */
- out2 = (q15_t) ((Si1 * S0 + Co1 * S1) >> 16U);
- /* writing output(xb', yb') in little endian format */
- pSrc16[i2 * 2U] = out1;
- pSrc16[(i2 * 2U) + 1U] = out2;
- /* Co3 & si3 are read from Coefficient pointer */
- Co3 = pCoef16[3U * ic * 2U];
- Si3 = pCoef16[(3U * ic * 2U) + 1U];
- /* Butterfly process for the i0+3fftLen/4 sample */
- /* xd' = (xa+yb-xc-yd)* Co3 - (ya-xb-yc+xd)* (si3) */
- out1 = (q15_t) ((Co3 * R0 - Si3 * R1) >> 16U);
- /* yd' = (ya-xb-yc+xd)* Co3 + (xa+yb-xc-yd)* (si3) */
- out2 = (q15_t) ((Si3 * R0 + Co3 * R1) >> 16U);
- /* writing output(xd', yd') in little endian format */
- pSrc16[i3 * 2U] = out1;
- pSrc16[(i3 * 2U) + 1U] = out2;
- /* Twiddle coefficients index modifier */
- ic = ic + twidCoefModifier;
- /* Updating input index */
- i0 = i0 + 1U;
- } while (--j);
- /* End of first stage process */
- /* data is in 4.11(q11) format */
- /* Start of Middle stage process */
- /* Twiddle coefficients index modifier */
- twidCoefModifier <<= 2U;
- /* Calculation of Middle stage */
- for (k = fftLen / 4U; k > 4U; k >>= 2U)
- {
- /* Initializations for the middle stage */
- n1 = n2;
- n2 >>= 2U;
- ic = 0U;
- for (j = 0U; j <= (n2 - 1U); j++)
- {
- /* index calculation for the coefficients */
- Co1 = pCoef16[ic * 2U];
- Si1 = pCoef16[(ic * 2U) + 1U];
- Co2 = pCoef16[2U * ic * 2U];
- Si2 = pCoef16[2U * ic * 2U + 1U];
- Co3 = pCoef16[3U * ic * 2U];
- Si3 = pCoef16[(3U * ic * 2U) + 1U];
- /* Twiddle coefficients index modifier */
- ic = ic + twidCoefModifier;
- /* Butterfly implementation */
- for (i0 = j; i0 < fftLen; i0 += n1)
- {
- /* index calculation for the input as, */
- /* pSrc16[i0 + 0], pSrc16[i0 + fftLen/4], pSrc16[i0 + fftLen/2], pSrc16[i0 + 3fftLen/4] */
- i1 = i0 + n2;
- i2 = i1 + n2;
- i3 = i2 + n2;
- /* Reading i0, i0+fftLen/2 inputs */
- /* Read ya (real), xa(imag) input */
- T0 = pSrc16[i0 * 2U];
- T1 = pSrc16[(i0 * 2U) + 1U];
- /* Read yc (real), xc(imag) input */
- S0 = pSrc16[i2 * 2U];
- S1 = pSrc16[(i2 * 2U) + 1U];
- /* R0 = (ya + yc), R1 = (xa + xc) */
- R0 = __SSAT(T0 + S0, 16U);
- R1 = __SSAT(T1 + S1, 16U);
- /* S0 = (ya - yc), S1 = (xa - xc) */
- S0 = __SSAT(T0 - S0, 16U);
- S1 = __SSAT(T1 - S1, 16U);
- /* Reading i0+fftLen/4 , i0+3fftLen/4 inputs */
- /* Read yb (real), xb(imag) input */
- T0 = pSrc16[i1 * 2U];
- T1 = pSrc16[(i1 * 2U) + 1U];
- /* Read yd (real), xd(imag) input */
- U0 = pSrc16[i3 * 2U];
- U1 = pSrc16[(i3 * 2U) + 1U];
- /* T0 = (yb + yd), T1 = (xb + xd) */
- T0 = __SSAT(T0 + U0, 16U);
- T1 = __SSAT(T1 + U1, 16U);
- /* writing the butterfly processed i0 sample */
- /* xa' = xa + xb + xc + xd */
- /* ya' = ya + yb + yc + yd */
- pSrc16[i0 * 2U] = ((R0 >> 1U) + (T0 >> 1U)) >> 1U;
- pSrc16[(i0 * 2U) + 1U] = ((R1 >> 1U) + (T1 >> 1U)) >> 1U;
- /* R0 = (ya + yc) - (yb + yd), R1 = (xa + xc) - (xb + xd) */
- R0 = (R0 >> 1U) - (T0 >> 1U);
- R1 = (R1 >> 1U) - (T1 >> 1U);
- /* (ya-yb+yc-yd)* (si2) - (xa-xb+xc-xd)* co2 */
- out1 = (q15_t) ((Co2 * R0 - Si2 * R1) >> 16);
- /* (ya-yb+yc-yd)* co2 + (xa-xb+xc-xd)* (si2) */
- out2 = (q15_t) ((Si2 * R0 + Co2 * R1) >> 16);
- /* Reading i0+3fftLen/4 */
- /* Read yb (real), xb(imag) input */
- T0 = pSrc16[i1 * 2U];
- T1 = pSrc16[(i1 * 2U) + 1U];
- /* writing the butterfly processed i0 + fftLen/4 sample */
- /* xc' = (xa-xb+xc-xd)* co2 - (ya-yb+yc-yd)* (si2) */
- /* yc' = (ya-yb+yc-yd)* co2 + (xa-xb+xc-xd)* (si2) */
- pSrc16[i1 * 2U] = out1;
- pSrc16[(i1 * 2U) + 1U] = out2;
- /* Butterfly calculations */
- /* Read yd (real), xd(imag) input */
- U0 = pSrc16[i3 * 2U];
- U1 = pSrc16[(i3 * 2U) + 1U];
- /* T0 = yb-yd, T1 = xb-xd) */
- T0 = __SSAT(T0 - U0, 16U);
- T1 = __SSAT(T1 - U1, 16U);
- /* R0 = (ya-yc) - (xb- xd) , R1 = (xa-xc) + (yb-yd) */
- R0 = (S0 >> 1U) + (T1 >> 1U);
- R1 = (S1 >> 1U) - (T0 >> 1U);
- /* S1 = (ya-yc) + (xb- xd), S1 = (xa-xc) - (yb-yd) */
- S0 = (S0 >> 1U) - (T1 >> 1U);
- S1 = (S1 >> 1U) + (T0 >> 1U);
- /* Butterfly process for the i0+fftLen/2 sample */
- out1 = (q15_t) ((Co1 * S0 - Si1 * S1) >> 16U);
- out2 = (q15_t) ((Si1 * S0 + Co1 * S1) >> 16U);
- /* xb' = (xa-yb-xc+yd)* co1 - (ya+xb-yc-xd)* (si1) */
- /* yb' = (ya+xb-yc-xd)* co1 + (xa-yb-xc+yd)* (si1) */
- pSrc16[i2 * 2U] = out1;
- pSrc16[(i2 * 2U) + 1U] = out2;
- /* Butterfly process for the i0+3fftLen/4 sample */
- out1 = (q15_t) ((Co3 * R0 - Si3 * R1) >> 16U);
- out2 = (q15_t) ((Si3 * R0 + Co3 * R1) >> 16U);
- /* xd' = (xa+yb-xc-yd)* Co3 - (ya-xb-yc+xd)* (si3) */
- /* yd' = (ya-xb-yc+xd)* Co3 + (xa+yb-xc-yd)* (si3) */
- pSrc16[i3 * 2U] = out1;
- pSrc16[(i3 * 2U) + 1U] = out2;
- }
- }
- /* Twiddle coefficients index modifier */
- twidCoefModifier <<= 2U;
- }
- /* End of Middle stages process */
- /* data is in 10.6(q6) format for the 1024 point */
- /* data is in 8.8(q8) format for the 256 point */
- /* data is in 6.10(q10) format for the 64 point */
- /* data is in 4.12(q12) format for the 16 point */
- /* start of last stage process */
- /* Initializations for the last stage */
- n1 = n2;
- n2 >>= 2U;
- /* Butterfly implementation */
- for (i0 = 0U; i0 <= (fftLen - n1); i0 += n1)
- {
- /* index calculation for the input as, */
- /* pSrc16[i0 + 0], pSrc16[i0 + fftLen/4], pSrc16[i0 + fftLen/2], pSrc16[i0 + 3fftLen/4] */
- i1 = i0 + n2;
- i2 = i1 + n2;
- i3 = i2 + n2;
- /* Reading i0, i0+fftLen/2 inputs */
- /* Read ya (real), xa(imag) input */
- T0 = pSrc16[i0 * 2U];
- T1 = pSrc16[(i0 * 2U) + 1U];
- /* Read yc (real), xc(imag) input */
- S0 = pSrc16[i2 * 2U];
- S1 = pSrc16[(i2 * 2U) + 1U];
- /* R0 = (ya + yc), R1 = (xa + xc) */
- R0 = __SSAT(T0 + S0, 16U);
- R1 = __SSAT(T1 + S1, 16U);
- /* S0 = (ya - yc), S1 = (xa - xc) */
- S0 = __SSAT(T0 - S0, 16U);
- S1 = __SSAT(T1 - S1, 16U);
- /* Reading i0+fftLen/4 , i0+3fftLen/4 inputs */
- /* Read yb (real), xb(imag) input */
- T0 = pSrc16[i1 * 2U];
- T1 = pSrc16[(i1 * 2U) + 1U];
- /* Read yd (real), xd(imag) input */
- U0 = pSrc16[i3 * 2U];
- U1 = pSrc16[(i3 * 2U) + 1U];
- /* T0 = (yb + yd), T1 = (xb + xd) */
- T0 = __SSAT(T0 + U0, 16U);
- T1 = __SSAT(T1 + U1, 16U);
- /* writing the butterfly processed i0 sample */
- /* xa' = xa + xb + xc + xd */
- /* ya' = ya + yb + yc + yd */
- pSrc16[i0 * 2U] = (R0 >> 1U) + (T0 >> 1U);
- pSrc16[(i0 * 2U) + 1U] = (R1 >> 1U) + (T1 >> 1U);
- /* R0 = (ya + yc) - (yb + yd), R1 = (xa + xc) - (xb + xd) */
- R0 = (R0 >> 1U) - (T0 >> 1U);
- R1 = (R1 >> 1U) - (T1 >> 1U);
- /* Read yb (real), xb(imag) input */
- T0 = pSrc16[i1 * 2U];
- T1 = pSrc16[(i1 * 2U) + 1U];
- /* writing the butterfly processed i0 + fftLen/4 sample */
- /* xc' = (xa-xb+xc-xd) */
- /* yc' = (ya-yb+yc-yd) */
- pSrc16[i1 * 2U] = R0;
- pSrc16[(i1 * 2U) + 1U] = R1;
- /* Read yd (real), xd(imag) input */
- U0 = pSrc16[i3 * 2U];
- U1 = pSrc16[(i3 * 2U) + 1U];
- /* T0 = (yb - yd), T1 = (xb - xd) */
- T0 = __SSAT(T0 - U0, 16U);
- T1 = __SSAT(T1 - U1, 16U);
- /* writing the butterfly processed i0 + fftLen/2 sample */
- /* xb' = (xa-yb-xc+yd) */
- /* yb' = (ya+xb-yc-xd) */
- pSrc16[i2 * 2U] = (S0 >> 1U) - (T1 >> 1U);
- pSrc16[(i2 * 2U) + 1U] = (S1 >> 1U) + (T0 >> 1U);
- /* writing the butterfly processed i0 + 3fftLen/4 sample */
- /* xd' = (xa+yb-xc-yd) */
- /* yd' = (ya-xb-yc+xd) */
- pSrc16[i3 * 2U] = (S0 >> 1U) + (T1 >> 1U);
- pSrc16[(i3 * 2U) + 1U] = (S1 >> 1U) - (T0 >> 1U);
- }
- /* end of last stage process */
- /* output is in 11.5(q5) format for the 1024 point */
- /* output is in 9.7(q7) format for the 256 point */
- /* output is in 7.9(q9) format for the 64 point */
- /* output is in 5.11(q11) format for the 16 point */
- #endif /* #if defined (ARM_MATH_DSP) */
- }
|