GNU Radio 3.6.5 C++ API
|
00001 #ifndef INCLUDED_volk_32fc_s32f_power_32fc_a_H 00002 #define INCLUDED_volk_32fc_s32f_power_32fc_a_H 00003 00004 #include <inttypes.h> 00005 #include <stdio.h> 00006 #include <math.h> 00007 00008 //! raise a complex float to a real float power 00009 static inline lv_32fc_t __volk_s32fc_s32f_power_s32fc_a(const lv_32fc_t exp, const float power){ 00010 const float arg = power*atan2f(lv_creal(exp), lv_cimag(exp)); 00011 const float mag = powf(lv_creal(exp)*lv_creal(exp) + lv_cimag(exp)*lv_cimag(exp), power/2); 00012 return mag*lv_cmake(cosf(arg), sinf(arg)); 00013 } 00014 00015 #ifdef LV_HAVE_SSE 00016 #include <xmmintrin.h> 00017 00018 #ifdef LV_HAVE_LIB_SIMDMATH 00019 #include <simdmath.h> 00020 #endif /* LV_HAVE_LIB_SIMDMATH */ 00021 00022 /*! 00023 \brief Takes each the input complex vector value to the specified power and stores the results in the return vector 00024 \param cVector The vector where the results will be stored 00025 \param aVector The complex vector of values to be taken to a power 00026 \param power The power value to be applied to each data point 00027 \param num_points The number of values in aVector to be taken to the specified power level and stored into cVector 00028 */ 00029 static inline void volk_32fc_s32f_power_32fc_a_sse(lv_32fc_t* cVector, const lv_32fc_t* aVector, const float power, unsigned int num_points){ 00030 unsigned int number = 0; 00031 00032 lv_32fc_t* cPtr = cVector; 00033 const lv_32fc_t* aPtr = aVector; 00034 00035 #ifdef LV_HAVE_LIB_SIMDMATH 00036 const unsigned int quarterPoints = num_points / 4; 00037 __m128 vPower = _mm_set_ps1(power); 00038 00039 __m128 cplxValue1, cplxValue2, magnitude, phase, iValue, qValue; 00040 for(;number < quarterPoints; number++){ 00041 00042 cplxValue1 = _mm_load_ps((float*)aPtr); 00043 aPtr += 2; 00044 00045 cplxValue2 = _mm_load_ps((float*)aPtr); 00046 aPtr += 2; 00047 00048 // Convert to polar coordinates 00049 00050 // Arrange in i1i2i3i4 format 00051 iValue = _mm_shuffle_ps(cplxValue1, cplxValue2, _MM_SHUFFLE(2,0,2,0)); 00052 // Arrange in q1q2q3q4 format 00053 qValue = _mm_shuffle_ps(cplxValue1, cplxValue2, _MM_SHUFFLE(3,1,3,1)); 00054 00055 phase = atan2f4(qValue, iValue); // Calculate the Phase 00056 00057 magnitude = _mm_sqrt_ps(_mm_add_ps(_mm_mul_ps(iValue, iValue), _mm_mul_ps(qValue, qValue))); // Calculate the magnitude by square rooting the added I2 and Q2 values 00058 00059 // Now calculate the power of the polar coordinate data 00060 magnitude = powf4(magnitude, vPower); // Take the magnitude to the specified power 00061 00062 phase = _mm_mul_ps(phase, vPower); // Multiply the phase by the specified power 00063 00064 // Convert back to cartesian coordinates 00065 iValue = _mm_mul_ps( cosf4(phase), magnitude); // Multiply the cos of the phase by the magnitude 00066 qValue = _mm_mul_ps( sinf4(phase), magnitude); // Multiply the sin of the phase by the magnitude 00067 00068 cplxValue1 = _mm_unpacklo_ps(iValue, qValue); // Interleave the lower two i & q values 00069 cplxValue2 = _mm_unpackhi_ps(iValue, qValue); // Interleave the upper two i & q values 00070 00071 _mm_store_ps((float*)cPtr,cplxValue1); // Store the results back into the C container 00072 00073 cPtr += 2; 00074 00075 _mm_store_ps((float*)cPtr,cplxValue2); // Store the results back into the C container 00076 00077 cPtr += 2; 00078 } 00079 00080 number = quarterPoints * 4; 00081 #endif /* LV_HAVE_LIB_SIMDMATH */ 00082 00083 for(;number < num_points; number++){ 00084 *cPtr++ = __volk_s32fc_s32f_power_s32fc_a((*aPtr++), power); 00085 } 00086 } 00087 #endif /* LV_HAVE_SSE */ 00088 00089 #ifdef LV_HAVE_GENERIC 00090 /*! 00091 \brief Takes each the input complex vector value to the specified power and stores the results in the return vector 00092 \param cVector The vector where the results will be stored 00093 \param aVector The complex vector of values to be taken to a power 00094 \param power The power value to be applied to each data point 00095 \param num_points The number of values in aVector to be taken to the specified power level and stored into cVector 00096 */ 00097 static inline void volk_32fc_s32f_power_32fc_a_generic(lv_32fc_t* cVector, const lv_32fc_t* aVector, const float power, unsigned int num_points){ 00098 lv_32fc_t* cPtr = cVector; 00099 const lv_32fc_t* aPtr = aVector; 00100 unsigned int number = 0; 00101 00102 for(number = 0; number < num_points; number++){ 00103 *cPtr++ = __volk_s32fc_s32f_power_s32fc_a((*aPtr++), power); 00104 } 00105 } 00106 #endif /* LV_HAVE_GENERIC */ 00107 00108 00109 00110 00111 #endif /* INCLUDED_volk_32fc_s32f_power_32fc_a_H */