GNU Radio 3.4.0 C++ API
|
00001 #ifndef INCLUDED_volk_32fc_s32f_magnitude_16i_a16_H 00002 #define INCLUDED_volk_32fc_s32f_magnitude_16i_a16_H 00003 00004 #include <inttypes.h> 00005 #include <stdio.h> 00006 #include <math.h> 00007 00008 #if LV_HAVE_SSE3 00009 #include <pmmintrin.h> 00010 /*! 00011 \brief Calculates the magnitude of the complexVector, scales the resulting value and stores the results in the magnitudeVector 00012 \param complexVector The vector containing the complex input values 00013 \param scalar The scale value multiplied to the magnitude of each complex vector 00014 \param magnitudeVector The vector containing the real output values 00015 \param num_points The number of complex values in complexVector to be calculated and stored into cVector 00016 */ 00017 static inline void volk_32fc_s32f_magnitude_16i_a16_sse3(int16_t* magnitudeVector, const lv_32fc_t* complexVector, const float scalar, unsigned int num_points){ 00018 unsigned int number = 0; 00019 const unsigned int quarterPoints = num_points / 4; 00020 00021 const float* complexVectorPtr = (const float*)complexVector; 00022 int16_t* magnitudeVectorPtr = magnitudeVector; 00023 00024 __m128 vScalar = _mm_set_ps1(scalar); 00025 00026 __m128 cplxValue1, cplxValue2, result; 00027 00028 float floatBuffer[4] __attribute__((aligned(128))); 00029 00030 for(;number < quarterPoints; number++){ 00031 cplxValue1 = _mm_load_ps(complexVectorPtr); 00032 complexVectorPtr += 4; 00033 00034 cplxValue2 = _mm_load_ps(complexVectorPtr); 00035 complexVectorPtr += 4; 00036 00037 cplxValue1 = _mm_mul_ps(cplxValue1, cplxValue1); // Square the values 00038 cplxValue2 = _mm_mul_ps(cplxValue2, cplxValue2); // Square the Values 00039 00040 result = _mm_hadd_ps(cplxValue1, cplxValue2); // Add the I2 and Q2 values 00041 00042 result = _mm_sqrt_ps(result); 00043 00044 result = _mm_mul_ps(result, vScalar); 00045 00046 _mm_store_ps(floatBuffer, result); 00047 *magnitudeVectorPtr++ = (int16_t)(floatBuffer[0]); 00048 *magnitudeVectorPtr++ = (int16_t)(floatBuffer[1]); 00049 *magnitudeVectorPtr++ = (int16_t)(floatBuffer[2]); 00050 *magnitudeVectorPtr++ = (int16_t)(floatBuffer[3]); 00051 } 00052 00053 number = quarterPoints * 4; 00054 magnitudeVectorPtr = &magnitudeVector[number]; 00055 for(; number < num_points; number++){ 00056 float val1Real = *complexVectorPtr++; 00057 float val1Imag = *complexVectorPtr++; 00058 *magnitudeVectorPtr++ = (int16_t)(sqrtf((val1Real * val1Real) + (val1Imag * val1Imag)) * scalar); 00059 } 00060 } 00061 #endif /* LV_HAVE_SSE3 */ 00062 00063 #if LV_HAVE_SSE 00064 #include <xmmintrin.h> 00065 /*! 00066 \brief Calculates the magnitude of the complexVector, scales the resulting value and stores the results in the magnitudeVector 00067 \param complexVector The vector containing the complex input values 00068 \param scalar The scale value multiplied to the magnitude of each complex vector 00069 \param magnitudeVector The vector containing the real output values 00070 \param num_points The number of complex values in complexVector to be calculated and stored into cVector 00071 */ 00072 static inline void volk_32fc_s32f_magnitude_16i_a16_sse(int16_t* magnitudeVector, const lv_32fc_t* complexVector, const float scalar, unsigned int num_points){ 00073 unsigned int number = 0; 00074 const unsigned int quarterPoints = num_points / 4; 00075 00076 const float* complexVectorPtr = (const float*)complexVector; 00077 int16_t* magnitudeVectorPtr = magnitudeVector; 00078 00079 __m128 vScalar = _mm_set_ps1(scalar); 00080 00081 __m128 cplxValue1, cplxValue2, iValue, qValue, result; 00082 00083 float floatBuffer[4] __attribute__((aligned(128))); 00084 00085 for(;number < quarterPoints; number++){ 00086 cplxValue1 = _mm_load_ps(complexVectorPtr); 00087 complexVectorPtr += 4; 00088 00089 cplxValue2 = _mm_load_ps(complexVectorPtr); 00090 complexVectorPtr += 4; 00091 00092 // Arrange in i1i2i3i4 format 00093 iValue = _mm_shuffle_ps(cplxValue1, cplxValue2, _MM_SHUFFLE(2,0,2,0)); 00094 // Arrange in q1q2q3q4 format 00095 qValue = _mm_shuffle_ps(cplxValue1, cplxValue2, _MM_SHUFFLE(3,1,3,1)); 00096 00097 iValue = _mm_mul_ps(iValue, iValue); // Square the I values 00098 qValue = _mm_mul_ps(qValue, qValue); // Square the Q Values 00099 00100 result = _mm_add_ps(iValue, qValue); // Add the I2 and Q2 values 00101 00102 result = _mm_sqrt_ps(result); 00103 00104 result = _mm_mul_ps(result, vScalar); 00105 00106 _mm_store_ps(floatBuffer, result); 00107 *magnitudeVectorPtr++ = (int16_t)(floatBuffer[0]); 00108 *magnitudeVectorPtr++ = (int16_t)(floatBuffer[1]); 00109 *magnitudeVectorPtr++ = (int16_t)(floatBuffer[2]); 00110 *magnitudeVectorPtr++ = (int16_t)(floatBuffer[3]); 00111 } 00112 00113 number = quarterPoints * 4; 00114 magnitudeVectorPtr = &magnitudeVector[number]; 00115 for(; number < num_points; number++){ 00116 float val1Real = *complexVectorPtr++; 00117 float val1Imag = *complexVectorPtr++; 00118 *magnitudeVectorPtr++ = (int16_t)(sqrtf((val1Real * val1Real) + (val1Imag * val1Imag)) * scalar); 00119 } 00120 } 00121 #endif /* LV_HAVE_SSE */ 00122 00123 #if LV_HAVE_GENERIC 00124 /*! 00125 \brief Calculates the magnitude of the complexVector, scales the resulting value and stores the results in the magnitudeVector 00126 \param complexVector The vector containing the complex input values 00127 \param scalar The scale value multiplied to the magnitude of each complex vector 00128 \param magnitudeVector The vector containing the real output values 00129 \param num_points The number of complex values in complexVector to be calculated and stored into cVector 00130 */ 00131 static inline void volk_32fc_s32f_magnitude_16i_a16_generic(int16_t* magnitudeVector, const lv_32fc_t* complexVector, const float scalar, unsigned int num_points){ 00132 const float* complexVectorPtr = (float*)complexVector; 00133 int16_t* magnitudeVectorPtr = magnitudeVector; 00134 unsigned int number = 0; 00135 for(number = 0; number < num_points; number++){ 00136 const float real = *complexVectorPtr++; 00137 const float imag = *complexVectorPtr++; 00138 *magnitudeVectorPtr++ = (int16_t)(sqrtf((real*real) + (imag*imag)) * scalar); 00139 } 00140 } 00141 #endif /* LV_HAVE_GENERIC */ 00142 00143 #if LV_HAVE_ORC 00144 /*! 00145 \brief Calculates the magnitude of the complexVector, scales the resulting value and stores the results in the magnitudeVector 00146 \param complexVector The vector containing the complex input values 00147 \param scalar The scale value multiplied to the magnitude of each complex vector 00148 \param magnitudeVector The vector containing the real output values 00149 \param num_points The number of complex values in complexVector to be calculated and stored into cVector 00150 */ 00151 extern void volk_32fc_s32f_magnitude_16i_a16_orc_impl(int16_t* magnitudeVector, const lv_32fc_t* complexVector, const float scalar, unsigned int num_points); 00152 static inline void volk_32fc_s32f_magnitude_16i_a16_orc(int16_t* magnitudeVector, const lv_32fc_t* complexVector, const float scalar, unsigned int num_points){ 00153 volk_32fc_s32f_magnitude_16i_a16_orc_impl(magnitudeVector, complexVector, scalar, num_points); 00154 } 00155 #endif /* LV_HAVE_ORC */ 00156 00157 00158 #endif /* INCLUDED_volk_32fc_s32f_magnitude_16i_a16_H */