GNU Radio 3.7.2 C++ API
volk_32f_s32f_32f_fm_detect_32f.h
Go to the documentation of this file.
1 #ifndef INCLUDED_volk_32f_s32f_32f_fm_detect_32f_a_H
2 #define INCLUDED_volk_32f_s32f_32f_fm_detect_32f_a_H
3 
4 #include <inttypes.h>
5 #include <stdio.h>
6 
7 #ifdef LV_HAVE_SSE
8 #include <xmmintrin.h>
9 /*!
10  \brief performs the FM-detect differentiation on the input vector and stores the results in the output vector.
11  \param outputVector The byte-aligned vector where the results will be stored.
12  \param inputVector The byte-aligned input vector containing phase data (must be on the interval (-bound,bound] )
13  \param bound The interval that the input phase data is in, which is used to modulo the differentiation
14  \param saveValue A pointer to a float which contains the phase value of the sample before the first input sample.
15  \param num_noints The number of real values in the input vector.
16 */
17 static inline void volk_32f_s32f_32f_fm_detect_32f_a_sse(float* outputVector, const float* inputVector, const float bound, float* saveValue, unsigned int num_points){
18  if (num_points < 1) {
19  return;
20  }
21  unsigned int number = 1;
22  unsigned int j = 0;
23  // num_points-1 keeps Fedora 7's gcc from crashing...
24  // num_points won't work. :(
25  const unsigned int quarterPoints = (num_points-1) / 4;
26 
27  float* outPtr = outputVector;
28  const float* inPtr = inputVector;
29  __m128 upperBound = _mm_set_ps1(bound);
30  __m128 lowerBound = _mm_set_ps1(-bound);
31  __m128 next3old1;
32  __m128 next4;
33  __m128 boundAdjust;
34  __m128 posBoundAdjust = _mm_set_ps1(-2*bound); // Subtract when we're above.
35  __m128 negBoundAdjust = _mm_set_ps1(2*bound); // Add when we're below.
36  // Do the first 4 by hand since we're going in from the saveValue:
37  *outPtr = *inPtr - *saveValue;
38  if (*outPtr > bound) *outPtr -= 2*bound;
39  if (*outPtr < -bound) *outPtr += 2*bound;
40  inPtr++;
41  outPtr++;
42  for (j = 1; j < ( (4 < num_points) ? 4 : num_points); j++) {
43  *outPtr = *(inPtr) - *(inPtr-1);
44  if (*outPtr > bound) *outPtr -= 2*bound;
45  if (*outPtr < -bound) *outPtr += 2*bound;
46  inPtr++;
47  outPtr++;
48  }
49 
50  for (; number < quarterPoints; number++) {
51  // Load data
52  next3old1 = _mm_loadu_ps((float*) (inPtr-1));
53  next4 = _mm_load_ps(inPtr);
54  inPtr += 4;
55  // Subtract and store:
56  next3old1 = _mm_sub_ps(next4, next3old1);
57  // Bound:
58  boundAdjust = _mm_cmpgt_ps(next3old1, upperBound);
59  boundAdjust = _mm_and_ps(boundAdjust, posBoundAdjust);
60  next4 = _mm_cmplt_ps(next3old1, lowerBound);
61  next4 = _mm_and_ps(next4, negBoundAdjust);
62  boundAdjust = _mm_or_ps(next4, boundAdjust);
63  // Make sure we're in the bounding interval:
64  next3old1 = _mm_add_ps(next3old1, boundAdjust);
65  _mm_store_ps(outPtr,next3old1); // Store the results back into the output
66  outPtr += 4;
67  }
68 
69  for (number = (4 > (quarterPoints*4) ? 4 : (4 * quarterPoints)); number < num_points; number++) {
70  *outPtr = *(inPtr) - *(inPtr-1);
71  if (*outPtr > bound) *outPtr -= 2*bound;
72  if (*outPtr < -bound) *outPtr += 2*bound;
73  inPtr++;
74  outPtr++;
75  }
76 
77  *saveValue = inputVector[num_points-1];
78 }
79 #endif /* LV_HAVE_SSE */
80 
81 #ifdef LV_HAVE_GENERIC
82 /*!
83  \brief performs the FM-detect differentiation on the input vector and stores the results in the output vector.
84  \param outputVector The byte-aligned vector where the results will be stored.
85  \param inputVector The byte-aligned input vector containing phase data (must be on the interval (-bound,bound] )
86  \param bound The interval that the input phase data is in, which is used to modulo the differentiation
87  \param saveValue A pointer to a float which contains the phase value of the sample before the first input sample.
88  \param num_points The number of real values in the input vector.
89 */
90 static inline void volk_32f_s32f_32f_fm_detect_32f_generic(float* outputVector, const float* inputVector, const float bound, float* saveValue, unsigned int num_points){
91  if (num_points < 1) {
92  return;
93  }
94  unsigned int number = 0;
95  float* outPtr = outputVector;
96  const float* inPtr = inputVector;
97 
98  // Do the first 1 by hand since we're going in from the saveValue:
99  *outPtr = *inPtr - *saveValue;
100  if (*outPtr > bound) *outPtr -= 2*bound;
101  if (*outPtr < -bound) *outPtr += 2*bound;
102  inPtr++;
103  outPtr++;
104 
105  for (number = 1; number < num_points; number++) {
106  *outPtr = *(inPtr) - *(inPtr-1);
107  if (*outPtr > bound) *outPtr -= 2*bound;
108  if (*outPtr < -bound) *outPtr += 2*bound;
109  inPtr++;
110  outPtr++;
111  }
112 
113  *saveValue = inputVector[num_points-1];
114 }
115 #endif /* LV_HAVE_GENERIC */
116 
117 
118 
119 
120 #endif /* INCLUDED_volk_32f_s32f_32f_fm_detect_32f_a_H */