76 #ifndef INCLUDED_volk_32f_cos_32f_a_H
77 #define INCLUDED_volk_32f_cos_32f_a_H
80 #include <smmintrin.h>
83 volk_32f_cos_32f_a_sse4_1(
float* bVector,
const float* aVector,
unsigned int num_points)
85 float* bPtr = bVector;
86 const float* aPtr = aVector;
88 unsigned int number = 0;
89 unsigned int quarterPoints = num_points / 4;
92 __m128 aVal, s, m4pi, pio4A, pio4B, cp1, cp2, cp3, cp4, cp5, ffours, ftwos, fones, fzeroes;
93 __m128 sine, cosine, condition1, condition3;
94 __m128i q, r, ones, twos, fours;
96 m4pi = _mm_set1_ps(1.273239545);
97 pio4A = _mm_set1_ps(0.78515625);
98 pio4B = _mm_set1_ps(0.241876e-3);
99 ffours = _mm_set1_ps(4.0);
100 ftwos = _mm_set1_ps(2.0);
101 fones = _mm_set1_ps(1.0);
102 fzeroes = _mm_setzero_ps();
103 ones = _mm_set1_epi32(1);
104 twos = _mm_set1_epi32(2);
105 fours = _mm_set1_epi32(4);
107 cp1 = _mm_set1_ps(1.0);
108 cp2 = _mm_set1_ps(0.83333333e-1);
109 cp3 = _mm_set1_ps(0.2777778e-2);
110 cp4 = _mm_set1_ps(0.49603e-4);
111 cp5 = _mm_set1_ps(0.551e-6);
113 for(;number < quarterPoints; number++){
115 aVal = _mm_load_ps(aPtr);
116 s = _mm_sub_ps(aVal, _mm_and_ps(_mm_mul_ps(aVal, ftwos), _mm_cmplt_ps(aVal, fzeroes)));
117 q = _mm_cvtps_epi32(_mm_mul_ps(s, m4pi));
118 r = _mm_add_epi32(q, _mm_and_si128(q, ones));
120 s = _mm_sub_ps(s, _mm_mul_ps(_mm_cvtepi32_ps(r), pio4A));
121 s = _mm_sub_ps(s, _mm_mul_ps(_mm_cvtepi32_ps(r), pio4B));
123 s = _mm_div_ps(s, _mm_set1_ps(8.0));
124 s = _mm_mul_ps(s, s);
126 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);
128 for(i = 0; i < 3; i++)
129 s = _mm_mul_ps(s, _mm_sub_ps(ffours, s));
130 s = _mm_div_ps(s, ftwos);
132 sine = _mm_sqrt_ps(_mm_mul_ps(_mm_sub_ps(ftwos, s), s));
133 cosine = _mm_sub_ps(fones, s);
135 condition1 = _mm_cmpneq_ps(_mm_cvtepi32_ps(_mm_and_si128(_mm_add_epi32(q, ones), twos)), fzeroes);
137 condition3 = _mm_cmpneq_ps(_mm_cvtepi32_ps(_mm_and_si128(_mm_add_epi32(q, twos), fours)), fzeroes);
139 cosine = _mm_add_ps(cosine, _mm_and_ps(_mm_sub_ps(sine, cosine), condition1));
140 cosine = _mm_sub_ps(cosine, _mm_and_ps(_mm_mul_ps(cosine, _mm_set1_ps(2.0f)), condition3));
141 _mm_store_ps(bPtr, cosine);
146 number = quarterPoints * 4;
147 for(;number < num_points; number++){
148 *bPtr++ = cos(*aPtr++);
158 #ifndef INCLUDED_volk_32f_cos_32f_u_H
159 #define INCLUDED_volk_32f_cos_32f_u_H
161 #ifdef LV_HAVE_SSE4_1
162 #include <smmintrin.h>
165 volk_32f_cos_32f_u_sse4_1(
float* bVector,
const float* aVector,
unsigned int num_points)
167 float* bPtr = bVector;
168 const float* aPtr = aVector;
170 unsigned int number = 0;
171 unsigned int quarterPoints = num_points / 4;
174 __m128 aVal, s, m4pi, pio4A, pio4B, cp1, cp2, cp3, cp4, cp5, ffours, ftwos, fones, fzeroes;
175 __m128 sine, cosine, condition1, condition3;
176 __m128i q, r, ones, twos, fours;
178 m4pi = _mm_set1_ps(1.273239545);
179 pio4A = _mm_set1_ps(0.78515625);
180 pio4B = _mm_set1_ps(0.241876e-3);
181 ffours = _mm_set1_ps(4.0);
182 ftwos = _mm_set1_ps(2.0);
183 fones = _mm_set1_ps(1.0);
184 fzeroes = _mm_setzero_ps();
185 ones = _mm_set1_epi32(1);
186 twos = _mm_set1_epi32(2);
187 fours = _mm_set1_epi32(4);
189 cp1 = _mm_set1_ps(1.0);
190 cp2 = _mm_set1_ps(0.83333333e-1);
191 cp3 = _mm_set1_ps(0.2777778e-2);
192 cp4 = _mm_set1_ps(0.49603e-4);
193 cp5 = _mm_set1_ps(0.551e-6);
195 for(;number < quarterPoints; number++){
196 aVal = _mm_loadu_ps(aPtr);
197 s = _mm_sub_ps(aVal, _mm_and_ps(_mm_mul_ps(aVal, ftwos), _mm_cmplt_ps(aVal, fzeroes)));
198 q = _mm_cvtps_epi32(_mm_mul_ps(s, m4pi));
199 r = _mm_add_epi32(q, _mm_and_si128(q, ones));
201 s = _mm_sub_ps(s, _mm_mul_ps(_mm_cvtepi32_ps(r), pio4A));
202 s = _mm_sub_ps(s, _mm_mul_ps(_mm_cvtepi32_ps(r), pio4B));
204 s = _mm_div_ps(s, _mm_set1_ps(8.0));
205 s = _mm_mul_ps(s, s);
207 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);
209 for(i = 0; i < 3; i++){
210 s = _mm_mul_ps(s, _mm_sub_ps(ffours, s));
212 s = _mm_div_ps(s, ftwos);
214 sine = _mm_sqrt_ps(_mm_mul_ps(_mm_sub_ps(ftwos, s), s));
215 cosine = _mm_sub_ps(fones, s);
217 condition1 = _mm_cmpneq_ps(_mm_cvtepi32_ps(_mm_and_si128(_mm_add_epi32(q, ones), twos)), fzeroes);
219 condition3 = _mm_cmpneq_ps(_mm_cvtepi32_ps(_mm_and_si128(_mm_add_epi32(q, twos), fours)), fzeroes);
221 cosine = _mm_add_ps(cosine, _mm_and_ps(_mm_sub_ps(sine, cosine), condition1));
222 cosine = _mm_sub_ps(cosine, _mm_and_ps(_mm_mul_ps(cosine, _mm_set1_ps(2.0f)), condition3));
223 _mm_storeu_ps(bPtr, cosine);
228 number = quarterPoints * 4;
229 for(;number < num_points; number++){
230 *bPtr++ = cos(*aPtr++);
237 #ifdef LV_HAVE_GENERIC
240 volk_32f_cos_32f_generic(
float* bVector,
const float* aVector,
unsigned int num_points)
242 float* bPtr = bVector;
243 const float* aPtr = aVector;
244 unsigned int number = 0;
246 for(; number < num_points; number++){
247 *bPtr++ = cos(*aPtr++);