arm_cmplx_mag_squared_f32.c

00001 /* ----------------------------------------------------------------------    
00002 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.    
00003 *    
00004 * $Date:        17. January 2013
00005 * $Revision:    V1.4.1
00006 *    
00007 * Project:      CMSIS DSP Library    
00008 * Title:        arm_cmplx_mag_squared_f32.c    
00009 *    
00010 * Description:  Floating-point complex magnitude squared.    
00011 *    
00012 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
00013 *  
00014 * Redistribution and use in source and binary forms, with or without 
00015 * modification, are permitted provided that the following conditions
00016 * are met:
00017 *   - Redistributions of source code must retain the above copyright
00018 *     notice, this list of conditions and the following disclaimer.
00019 *   - Redistributions in binary form must reproduce the above copyright
00020 *     notice, this list of conditions and the following disclaimer in
00021 *     the documentation and/or other materials provided with the 
00022 *     distribution.
00023 *   - Neither the name of ARM LIMITED nor the names of its contributors
00024 *     may be used to endorse or promote products derived from this
00025 *     software without specific prior written permission.
00026 *
00027 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00028 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00029 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00030 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
00031 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00032 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00033 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00034 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00035 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00036 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00037 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00038 * POSSIBILITY OF SUCH DAMAGE.  
00039 * ---------------------------------------------------------------------------- */
00040 #include "arm_math.h"
00041 
00042 /**        
00043  * @ingroup groupCmplxMath        
00044  */
00045 
00046 /**        
00047  * @defgroup cmplx_mag_squared Complex Magnitude Squared        
00048  *        
00049  * Computes the magnitude squared of the elements of a complex data vector.        
00050  *       
00051  * The <code>pSrc</code> points to the source data and        
00052  * <code>pDst</code> points to the where the result should be written.        
00053  * <code>numSamples</code> specifies the number of complex samples        
00054  * in the input array and the data is stored in an interleaved fashion        
00055  * (real, imag, real, imag, ...).        
00056  * The input array has a total of <code>2*numSamples</code> values;        
00057  * the output array has a total of <code>numSamples</code> values.        
00058  *        
00059  * The underlying algorithm is used:        
00060  *        
00061  * <pre>        
00062  * for(n=0; n<numSamples; n++) {        
00063  *     pDst[n] = pSrc[(2*n)+0]^2 + pSrc[(2*n)+1]^2;        
00064  * }        
00065  * </pre>        
00066  *        
00067  * There are separate functions for floating-point, Q15, and Q31 data types.        
00068  */
00069 
00070 /**        
00071  * @addtogroup cmplx_mag_squared        
00072  * @{        
00073  */
00074 
00075 
00076 /**        
00077  * @brief  Floating-point complex magnitude squared        
00078  * @param[in]  *pSrc points to the complex input vector        
00079  * @param[out]  *pDst points to the real output vector        
00080  * @param[in]  numSamples number of complex samples in the input vector        
00081  * @return none.        
00082  */
00083 
00084 void arm_cmplx_mag_squared_f32(
00085   float32_t * pSrc,
00086   float32_t * pDst,
00087   uint32_t numSamples)
00088 {
00089   float32_t real, imag;                          /* Temporary variables to store real and imaginary values */
00090   uint32_t blkCnt;                               /* loop counter */
00091 
00092 #ifndef ARM_MATH_CM0_FAMILY
00093   float32_t real1, real2, real3, real4;          /* Temporary variables to hold real values */
00094   float32_t imag1, imag2, imag3, imag4;          /* Temporary variables to hold imaginary values */
00095   float32_t mul1, mul2, mul3, mul4;              /* Temporary variables */
00096   float32_t mul5, mul6, mul7, mul8;              /* Temporary variables */
00097   float32_t out1, out2, out3, out4;              /* Temporary variables to hold output values */
00098 
00099   /*loop Unrolling */
00100   blkCnt = numSamples >> 2u;
00101 
00102   /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.        
00103    ** a second loop below computes the remaining 1 to 3 samples. */
00104   while(blkCnt > 0u)
00105   {
00106     /* C[0] = (A[0] * A[0] + A[1] * A[1]) */
00107     /* read real input sample from source buffer */
00108     real1 = pSrc[0];
00109     /* read imaginary input sample from source buffer */
00110     imag1 = pSrc[1];
00111 
00112     /* calculate power of real value */
00113     mul1 = real1 * real1;
00114 
00115     /* read real input sample from source buffer */
00116     real2 = pSrc[2];
00117 
00118     /* calculate power of imaginary value */
00119     mul2 = imag1 * imag1;
00120 
00121     /* read imaginary input sample from source buffer */
00122     imag2 = pSrc[3];
00123 
00124     /* calculate power of real value */
00125     mul3 = real2 * real2;
00126 
00127     /* read real input sample from source buffer */
00128     real3 = pSrc[4];
00129 
00130     /* calculate power of imaginary value */
00131     mul4 = imag2 * imag2;
00132 
00133     /* read imaginary input sample from source buffer */
00134     imag3 = pSrc[5];
00135 
00136     /* calculate power of real value */
00137     mul5 = real3 * real3;
00138     /* calculate power of imaginary value */
00139     mul6 = imag3 * imag3;
00140 
00141     /* read real input sample from source buffer */
00142     real4 = pSrc[6];
00143 
00144     /* accumulate real and imaginary powers */
00145     out1 = mul1 + mul2;
00146 
00147     /* read imaginary input sample from source buffer */
00148     imag4 = pSrc[7];
00149 
00150     /* accumulate real and imaginary powers */
00151     out2 = mul3 + mul4;
00152 
00153     /* calculate power of real value */
00154     mul7 = real4 * real4;
00155     /* calculate power of imaginary value */
00156     mul8 = imag4 * imag4;
00157 
00158     /* store output to destination */
00159     pDst[0] = out1;
00160 
00161     /* accumulate real and imaginary powers */
00162     out3 = mul5 + mul6;
00163 
00164     /* store output to destination */
00165     pDst[1] = out2;
00166 
00167     /* accumulate real and imaginary powers */
00168     out4 = mul7 + mul8;
00169 
00170     /* store output to destination */
00171     pDst[2] = out3;
00172 
00173     /* increment destination pointer by 8 to process next samples */
00174     pSrc += 8u;
00175 
00176     /* store output to destination */
00177     pDst[3] = out4;
00178 
00179     /* increment destination pointer by 4 to process next samples */
00180     pDst += 4u;
00181 
00182     /* Decrement the loop counter */
00183     blkCnt--;
00184   }
00185 
00186   /* If the numSamples is not a multiple of 4, compute any remaining output samples here.        
00187    ** No loop unrolling is used. */
00188   blkCnt = numSamples % 0x4u;
00189 
00190 #else
00191 
00192   /* Run the below code for Cortex-M0 */
00193 
00194   blkCnt = numSamples;
00195 
00196 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
00197 
00198   while(blkCnt > 0u)
00199   {
00200     /* C[0] = (A[0] * A[0] + A[1] * A[1]) */
00201     real = *pSrc++;
00202     imag = *pSrc++;
00203 
00204     /* out = (real * real) + (imag * imag) */
00205     /* store the result in the destination buffer. */
00206     *pDst++ = (real * real) + (imag * imag);
00207 
00208     /* Decrement the loop counter */
00209     blkCnt--;
00210   }
00211 }
00212 
00213 /**        
00214  * @} end of cmplx_mag_squared group        
00215  */