GNU Radio Manual and C++ API Reference  3.7.6.1
The Free & Open Software Radio Ecosystem
volk_32f_cos_32f.h
Go to the documentation of this file.
1 /* -*- c++ -*- */
2 /*
3  * Copyright 2014 Free Software Foundation, Inc.
4  *
5  * This file is part of GNU Radio
6  *
7  * GNU Radio is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 3, or (at your option)
10  * any later version.
11  *
12  * GNU Radio is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with GNU Radio; see the file COPYING. If not, write to
19  * the Free Software Foundation, Inc., 51 Franklin Street,
20  * Boston, MA 02110-1301, USA.
21  */
22 
23 #include <stdio.h>
24 #include <math.h>
25 #include <inttypes.h>
26 
27 #ifndef INCLUDED_volk_32f_cos_32f_a_H
28 #define INCLUDED_volk_32f_cos_32f_a_H
29 
30 #ifdef LV_HAVE_SSE4_1
31 #include <smmintrin.h>
32 /*!
33  \brief Computes cosine of input vector and stores results in output vector
34  \param bVector The vector where results will be stored
35  \param aVector The input vector of floats
36  \param num_points Number of points for which cosine is to be computed
37 */
38 static inline void volk_32f_cos_32f_a_sse4_1(float* bVector, const float* aVector, unsigned int num_points){
39 
40  float* bPtr = bVector;
41  const float* aPtr = aVector;
42 
43  unsigned int number = 0;
44  unsigned int quarterPoints = num_points / 4;
45  unsigned int i = 0;
46 
47  __m128 aVal, s, m4pi, pio4A, pio4B, cp1, cp2, cp3, cp4, cp5, ffours, ftwos, fones, fzeroes;
48  __m128 sine, cosine, condition1, condition3;
49  __m128i q, r, ones, twos, fours;
50 
51  m4pi = _mm_set1_ps(1.273239545);
52  pio4A = _mm_set1_ps(0.78515625);
53  pio4B = _mm_set1_ps(0.241876e-3);
54  ffours = _mm_set1_ps(4.0);
55  ftwos = _mm_set1_ps(2.0);
56  fones = _mm_set1_ps(1.0);
57  fzeroes = _mm_setzero_ps();
58  ones = _mm_set1_epi32(1);
59  twos = _mm_set1_epi32(2);
60  fours = _mm_set1_epi32(4);
61 
62  cp1 = _mm_set1_ps(1.0);
63  cp2 = _mm_set1_ps(0.83333333e-1);
64  cp3 = _mm_set1_ps(0.2777778e-2);
65  cp4 = _mm_set1_ps(0.49603e-4);
66  cp5 = _mm_set1_ps(0.551e-6);
67 
68  for(;number < quarterPoints; number++){
69 
70  aVal = _mm_load_ps(aPtr);
71  s = _mm_sub_ps(aVal, _mm_and_ps(_mm_mul_ps(aVal, ftwos), _mm_cmplt_ps(aVal, fzeroes)));
72  q = _mm_cvtps_epi32(_mm_mul_ps(s, m4pi));
73  r = _mm_add_epi32(q, _mm_and_si128(q, ones));
74 
75  s = _mm_sub_ps(s, _mm_mul_ps(_mm_cvtepi32_ps(r), pio4A));
76  s = _mm_sub_ps(s, _mm_mul_ps(_mm_cvtepi32_ps(r), pio4B));
77 
78  s = _mm_div_ps(s, _mm_set1_ps(8.0)); // The constant is 2^N, for 3 times argument reduction
79  s = _mm_mul_ps(s, s);
80  // Evaluate Taylor series
81  s = _mm_mul_ps(_mm_add_ps(_mm_mul_ps(_mm_sub_ps(_mm_mul_ps(_mm_add_ps(_mm_mul_ps(_mm_sub_ps(_mm_mul_ps(s, cp5), cp4), s), cp3), s), cp2), s), cp1), s);
82 
83  for(i = 0; i < 3; i++) s = _mm_mul_ps(s, _mm_sub_ps(ffours, s));
84  s = _mm_div_ps(s, ftwos);
85 
86  sine = _mm_sqrt_ps(_mm_mul_ps(_mm_sub_ps(ftwos, s), s));
87  cosine = _mm_sub_ps(fones, s);
88 
89  condition1 = _mm_cmpneq_ps(_mm_cvtepi32_ps(_mm_and_si128(_mm_add_epi32(q, ones), twos)), fzeroes);
90 
91  condition3 = _mm_cmpneq_ps(_mm_cvtepi32_ps(_mm_and_si128(_mm_add_epi32(q, twos), fours)), fzeroes);
92 
93  cosine = _mm_add_ps(cosine, _mm_and_ps(_mm_sub_ps(sine, cosine), condition1));
94  cosine = _mm_sub_ps(cosine, _mm_and_ps(_mm_mul_ps(cosine, _mm_set1_ps(2.0f)), condition3));
95  _mm_store_ps(bPtr, cosine);
96  aPtr += 4;
97  bPtr += 4;
98  }
99 
100  number = quarterPoints * 4;
101  for(;number < num_points; number++){
102  *bPtr++ = cos(*aPtr++);
103  }
104 }
105 
106 #endif /* LV_HAVE_SSE4_1 for aligned */
107 
108 #endif /* INCLUDED_volk_32f_cos_32f_a_H */
109 
110 #ifndef INCLUDED_volk_32f_cos_32f_u_H
111 #define INCLUDED_volk_32f_cos_32f_u_H
112 
113 #ifdef LV_HAVE_SSE4_1
114 #include <smmintrin.h>
115 /*!
116  \brief Computes cosine of input vector and stores results in output vector
117  \param bVector The vector where results will be stored
118  \param aVector The input vector of floats
119  \param num_points Number of points for which cosine is to be computed
120 */
121 static inline void volk_32f_cos_32f_u_sse4_1(float* bVector, const float* aVector, unsigned int num_points){
122 
123  float* bPtr = bVector;
124  const float* aPtr = aVector;
125 
126  unsigned int number = 0;
127  unsigned int quarterPoints = num_points / 4;
128  unsigned int i = 0;
129 
130  __m128 aVal, s, m4pi, pio4A, pio4B, cp1, cp2, cp3, cp4, cp5, ffours, ftwos, fones, fzeroes;
131  __m128 sine, cosine, condition1, condition3;
132  __m128i q, r, ones, twos, fours;
133 
134  m4pi = _mm_set1_ps(1.273239545);
135  pio4A = _mm_set1_ps(0.78515625);
136  pio4B = _mm_set1_ps(0.241876e-3);
137  ffours = _mm_set1_ps(4.0);
138  ftwos = _mm_set1_ps(2.0);
139  fones = _mm_set1_ps(1.0);
140  fzeroes = _mm_setzero_ps();
141  ones = _mm_set1_epi32(1);
142  twos = _mm_set1_epi32(2);
143  fours = _mm_set1_epi32(4);
144 
145  cp1 = _mm_set1_ps(1.0);
146  cp2 = _mm_set1_ps(0.83333333e-1);
147  cp3 = _mm_set1_ps(0.2777778e-2);
148  cp4 = _mm_set1_ps(0.49603e-4);
149  cp5 = _mm_set1_ps(0.551e-6);
150 
151  for(;number < quarterPoints; number++){
152  aVal = _mm_loadu_ps(aPtr);
153  s = _mm_sub_ps(aVal, _mm_and_ps(_mm_mul_ps(aVal, ftwos), _mm_cmplt_ps(aVal, fzeroes)));
154  q = _mm_cvtps_epi32(_mm_mul_ps(s, m4pi));
155  r = _mm_add_epi32(q, _mm_and_si128(q, ones));
156 
157  s = _mm_sub_ps(s, _mm_mul_ps(_mm_cvtepi32_ps(r), pio4A));
158  s = _mm_sub_ps(s, _mm_mul_ps(_mm_cvtepi32_ps(r), pio4B));
159 
160  s = _mm_div_ps(s, _mm_set1_ps(8.0)); // The constant is 2^N, for 3 times argument reduction
161  s = _mm_mul_ps(s, s);
162  // Evaluate Taylor series
163  s = _mm_mul_ps(_mm_add_ps(_mm_mul_ps(_mm_sub_ps(_mm_mul_ps(_mm_add_ps(_mm_mul_ps(_mm_sub_ps(_mm_mul_ps(s, cp5), cp4), s), cp3), s), cp2), s), cp1), s);
164 
165  for(i = 0; i < 3; i++){
166  s = _mm_mul_ps(s, _mm_sub_ps(ffours, s));
167  }
168  s = _mm_div_ps(s, ftwos);
169 
170  sine = _mm_sqrt_ps(_mm_mul_ps(_mm_sub_ps(ftwos, s), s));
171  cosine = _mm_sub_ps(fones, s);
172 
173  condition1 = _mm_cmpneq_ps(_mm_cvtepi32_ps(_mm_and_si128(_mm_add_epi32(q, ones), twos)), fzeroes);
174 
175  condition3 = _mm_cmpneq_ps(_mm_cvtepi32_ps(_mm_and_si128(_mm_add_epi32(q, twos), fours)), fzeroes);
176 
177  cosine = _mm_add_ps(cosine, _mm_and_ps(_mm_sub_ps(sine, cosine), condition1));
178  cosine = _mm_sub_ps(cosine, _mm_and_ps(_mm_mul_ps(cosine, _mm_set1_ps(2.0f)), condition3));
179  _mm_storeu_ps(bPtr, cosine);
180  aPtr += 4;
181  bPtr += 4;
182  }
183 
184  number = quarterPoints * 4;
185  for(;number < num_points; number++){
186  *bPtr++ = cos(*aPtr++);
187  }
188 }
189 
190 #endif /* LV_HAVE_SSE4_1 for unaligned */
191 
192 #ifdef LV_HAVE_GENERIC
193 /*!
194  \brief Computes cosine of input vector and stores results in output vector
195  \param bVector The vector where results will be stored
196  \param aVector The input vector of floats
197  \param num_points Number of points for which cosine is to be computed
198 */
199 static inline void volk_32f_cos_32f_generic(float* bVector, const float* aVector, unsigned int num_points){
200  float* bPtr = bVector;
201  const float* aPtr = aVector;
202  unsigned int number = 0;
203 
204  for(; number < num_points; number++){
205  *bPtr++ = cos(*aPtr++);
206  }
207 
208 }
209 #endif /* LV_HAVE_GENERIC */
210 
211 #endif /* INCLUDED_volk_32f_cos_32f_u_H */