1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
|
#ifndef INCLUDED_VOLK_32fc_MAGNITUDE_16s_ALIGNED16_H
#define INCLUDED_VOLK_32fc_MAGNITUDE_16s_ALIGNED16_H
#include <inttypes.h>
#include <stdio.h>
#include <math.h>
#if LV_HAVE_SSE3
#include <pmmintrin.h>
/*!
\brief Calculates the magnitude of the complexVector, scales the resulting value and stores the results in the magnitudeVector
\param complexVector The vector containing the complex input values
\param scalar The scale value multiplied to the magnitude of each complex vector
\param magnitudeVector The vector containing the real output values
\param num_points The number of complex values in complexVector to be calculated and stored into cVector
*/
static inline void volk_32fc_magnitude_16s_aligned16_sse3(int16_t* magnitudeVector, const lv_32fc_t* complexVector, const float scalar, unsigned int num_points){
unsigned int number = 0;
const unsigned int quarterPoints = num_points / 4;
const float* complexVectorPtr = (const float*)complexVector;
int16_t* magnitudeVectorPtr = magnitudeVector;
__m128 vScalar = _mm_set_ps1(scalar);
__m128 cplxValue1, cplxValue2, result;
float floatBuffer[4] __attribute__((aligned(128)));
for(;number < quarterPoints; number++){
cplxValue1 = _mm_load_ps(complexVectorPtr);
complexVectorPtr += 4;
cplxValue2 = _mm_load_ps(complexVectorPtr);
complexVectorPtr += 4;
cplxValue1 = _mm_mul_ps(cplxValue1, cplxValue1); // Square the values
cplxValue2 = _mm_mul_ps(cplxValue2, cplxValue2); // Square the Values
result = _mm_hadd_ps(cplxValue1, cplxValue2); // Add the I2 and Q2 values
result = _mm_sqrt_ps(result);
result = _mm_mul_ps(result, vScalar);
_mm_store_ps(floatBuffer, result);
*magnitudeVectorPtr++ = (int16_t)(floatBuffer[0]);
*magnitudeVectorPtr++ = (int16_t)(floatBuffer[1]);
*magnitudeVectorPtr++ = (int16_t)(floatBuffer[2]);
*magnitudeVectorPtr++ = (int16_t)(floatBuffer[3]);
}
number = quarterPoints * 4;
magnitudeVectorPtr = &magnitudeVector[number];
for(; number < num_points; number++){
float val1Real = *complexVectorPtr++;
float val1Imag = *complexVectorPtr++;
*magnitudeVectorPtr++ = (int16_t)(sqrtf((val1Real * val1Real) + (val1Imag * val1Imag)) * scalar);
}
}
#endif /* LV_HAVE_SSE3 */
#if LV_HAVE_SSE
#include <xmmintrin.h>
/*!
\brief Calculates the magnitude of the complexVector, scales the resulting value and stores the results in the magnitudeVector
\param complexVector The vector containing the complex input values
\param scalar The scale value multiplied to the magnitude of each complex vector
\param magnitudeVector The vector containing the real output values
\param num_points The number of complex values in complexVector to be calculated and stored into cVector
*/
static inline void volk_32fc_magnitude_16s_aligned16_sse(int16_t* magnitudeVector, const lv_32fc_t* complexVector, const float scalar, unsigned int num_points){
unsigned int number = 0;
const unsigned int quarterPoints = num_points / 4;
const float* complexVectorPtr = (const float*)complexVector;
int16_t* magnitudeVectorPtr = magnitudeVector;
__m128 vScalar = _mm_set_ps1(scalar);
__m128 cplxValue1, cplxValue2, iValue, qValue, result;
float floatBuffer[4] __attribute__((aligned(128)));
for(;number < quarterPoints; number++){
cplxValue1 = _mm_load_ps(complexVectorPtr);
complexVectorPtr += 4;
cplxValue2 = _mm_load_ps(complexVectorPtr);
complexVectorPtr += 4;
// Arrange in i1i2i3i4 format
iValue = _mm_shuffle_ps(cplxValue1, cplxValue2, _MM_SHUFFLE(2,0,2,0));
// Arrange in q1q2q3q4 format
qValue = _mm_shuffle_ps(cplxValue1, cplxValue2, _MM_SHUFFLE(3,1,3,1));
iValue = _mm_mul_ps(iValue, iValue); // Square the I values
qValue = _mm_mul_ps(qValue, qValue); // Square the Q Values
result = _mm_add_ps(iValue, qValue); // Add the I2 and Q2 values
result = _mm_sqrt_ps(result);
result = _mm_mul_ps(result, vScalar);
_mm_store_ps(floatBuffer, result);
*magnitudeVectorPtr++ = (int16_t)(floatBuffer[0]);
*magnitudeVectorPtr++ = (int16_t)(floatBuffer[1]);
*magnitudeVectorPtr++ = (int16_t)(floatBuffer[2]);
*magnitudeVectorPtr++ = (int16_t)(floatBuffer[3]);
}
number = quarterPoints * 4;
magnitudeVectorPtr = &magnitudeVector[number];
for(; number < num_points; number++){
float val1Real = *complexVectorPtr++;
float val1Imag = *complexVectorPtr++;
*magnitudeVectorPtr++ = (int16_t)(sqrtf((val1Real * val1Real) + (val1Imag * val1Imag)) * scalar);
}
}
#endif /* LV_HAVE_SSE */
#if LV_HAVE_GENERIC
/*!
\brief Calculates the magnitude of the complexVector, scales the resulting value and stores the results in the magnitudeVector
\param complexVector The vector containing the complex input values
\param scalar The scale value multiplied to the magnitude of each complex vector
\param magnitudeVector The vector containing the real output values
\param num_points The number of complex values in complexVector to be calculated and stored into cVector
*/
static inline void volk_32fc_magnitude_16s_aligned16_generic(int16_t* magnitudeVector, const lv_32fc_t* complexVector, const float scalar, unsigned int num_points){
const float* complexVectorPtr = (float*)complexVector;
int16_t* magnitudeVectorPtr = magnitudeVector;
unsigned int number = 0;
for(number = 0; number < num_points; number++){
const float real = *complexVectorPtr++;
const float imag = *complexVectorPtr++;
*magnitudeVectorPtr++ = (int16_t)(sqrtf((real*real) + (imag*imag)) * scalar);
}
}
#endif /* LV_HAVE_GENERIC */
#endif /* INCLUDED_VOLK_32fc_MAGNITUDE_16s_ALIGNED16_H */
|
