GNU Radio 3.6.3.1 C++ API
volk_32fc_x2_dot_prod_32fc_a.h
Go to the documentation of this file.
1 #ifndef INCLUDED_volk_32fc_x2_dot_prod_32fc_a_H
2 #define INCLUDED_volk_32fc_x2_dot_prod_32fc_a_H
3 
4 #include <volk/volk_common.h>
5 #include <volk/volk_complex.h>
6 #include <stdio.h>
7 #include <string.h>
8 
9 
10 #ifdef LV_HAVE_GENERIC
11 
12 
13 static inline void volk_32fc_x2_dot_prod_32fc_a_generic(lv_32fc_t* result, const lv_32fc_t* input, const lv_32fc_t* taps, unsigned int num_bytes) {
14 
15  float * res = (float*) result;
16  float * in = (float*) input;
17  float * tp = (float*) taps;
18  unsigned int n_2_ccomplex_blocks = num_bytes >> 4;
19  unsigned int isodd = (num_bytes >> 3) &1;
20 
21  float sum0[2] = {0,0};
22  float sum1[2] = {0,0};
23  unsigned int i = 0;
24 
25  for(i = 0; i < n_2_ccomplex_blocks; ++i) {
26  sum0[0] += in[0] * tp[0] - in[1] * tp[1];
27  sum0[1] += in[0] * tp[1] + in[1] * tp[0];
28  sum1[0] += in[2] * tp[2] - in[3] * tp[3];
29  sum1[1] += in[2] * tp[3] + in[3] * tp[2];
30 
31  in += 4;
32  tp += 4;
33  }
34 
35  res[0] = sum0[0] + sum1[0];
36  res[1] = sum0[1] + sum1[1];
37 
38  for(i = 0; i < isodd; ++i) {
39  *result += input[(num_bytes >> 3) - 1] * taps[(num_bytes >> 3) - 1];
40  }
41 }
42 
43 #endif /*LV_HAVE_GENERIC*/
44 
45 
46 #if LV_HAVE_SSE && LV_HAVE_64
47 
48 
49 static inline void volk_32fc_x2_dot_prod_32fc_a_sse_64(lv_32fc_t* result, const lv_32fc_t* input, const lv_32fc_t* taps, unsigned int num_bytes) {
50 
51 
52  asm
53  (
54  "# ccomplex_dotprod_generic (float* result, const float *input,\n\t"
55  "# const float *taps, unsigned num_bytes)\n\t"
56  "# float sum0 = 0;\n\t"
57  "# float sum1 = 0;\n\t"
58  "# float sum2 = 0;\n\t"
59  "# float sum3 = 0;\n\t"
60  "# do {\n\t"
61  "# sum0 += input[0] * taps[0] - input[1] * taps[1];\n\t"
62  "# sum1 += input[0] * taps[1] + input[1] * taps[0];\n\t"
63  "# sum2 += input[2] * taps[2] - input[3] * taps[3];\n\t"
64  "# sum3 += input[2] * taps[3] + input[3] * taps[2];\n\t"
65  "# input += 4;\n\t"
66  "# taps += 4; \n\t"
67  "# } while (--n_2_ccomplex_blocks != 0);\n\t"
68  "# result[0] = sum0 + sum2;\n\t"
69  "# result[1] = sum1 + sum3;\n\t"
70  "# TODO: prefetch and better scheduling\n\t"
71  " xor %%r9, %%r9\n\t"
72  " xor %%r10, %%r10\n\t"
73  " movq %%rcx, %%rax\n\t"
74  " movq %%rcx, %%r8\n\t"
75  " movq %[rsi], %%r9\n\t"
76  " movq %[rdx], %%r10\n\t"
77  " xorps %%xmm6, %%xmm6 # zero accumulators\n\t"
78  " movaps 0(%%r9), %%xmm0\n\t"
79  " xorps %%xmm7, %%xmm7 # zero accumulators\n\t"
80  " movaps 0(%%r10), %%xmm2\n\t"
81  " shr $5, %%rax # rax = n_2_ccomplex_blocks / 2\n\t"
82  " shr $4, %%r8\n\t"
83  " jmp .%=L1_test\n\t"
84  " # 4 taps / loop\n\t"
85  " # something like ?? cycles / loop\n\t"
86  ".%=Loop1: \n\t"
87  "# complex prod: C += A * B, w/ temp Z & Y (or B), xmmPN=$0x8000000080000000\n\t"
88  "# movaps (%%r9), %%xmmA\n\t"
89  "# movaps (%%r10), %%xmmB\n\t"
90  "# movaps %%xmmA, %%xmmZ\n\t"
91  "# shufps $0xb1, %%xmmZ, %%xmmZ # swap internals\n\t"
92  "# mulps %%xmmB, %%xmmA\n\t"
93  "# mulps %%xmmZ, %%xmmB\n\t"
94  "# # SSE replacement for: pfpnacc %%xmmB, %%xmmA\n\t"
95  "# xorps %%xmmPN, %%xmmA\n\t"
96  "# movaps %%xmmA, %%xmmZ\n\t"
97  "# unpcklps %%xmmB, %%xmmA\n\t"
98  "# unpckhps %%xmmB, %%xmmZ\n\t"
99  "# movaps %%xmmZ, %%xmmY\n\t"
100  "# shufps $0x44, %%xmmA, %%xmmZ # b01000100\n\t"
101  "# shufps $0xee, %%xmmY, %%xmmA # b11101110\n\t"
102  "# addps %%xmmZ, %%xmmA\n\t"
103  "# addps %%xmmA, %%xmmC\n\t"
104  "# A=xmm0, B=xmm2, Z=xmm4\n\t"
105  "# A'=xmm1, B'=xmm3, Z'=xmm5\n\t"
106  " movaps 16(%%r9), %%xmm1\n\t"
107  " movaps %%xmm0, %%xmm4\n\t"
108  " mulps %%xmm2, %%xmm0\n\t"
109  " shufps $0xb1, %%xmm4, %%xmm4 # swap internals\n\t"
110  " movaps 16(%%r10), %%xmm3\n\t"
111  " movaps %%xmm1, %%xmm5\n\t"
112  " addps %%xmm0, %%xmm6\n\t"
113  " mulps %%xmm3, %%xmm1\n\t"
114  " shufps $0xb1, %%xmm5, %%xmm5 # swap internals\n\t"
115  " addps %%xmm1, %%xmm6\n\t"
116  " mulps %%xmm4, %%xmm2\n\t"
117  " movaps 32(%%r9), %%xmm0\n\t"
118  " addps %%xmm2, %%xmm7\n\t"
119  " mulps %%xmm5, %%xmm3\n\t"
120  " add $32, %%r9\n\t"
121  " movaps 32(%%r10), %%xmm2\n\t"
122  " addps %%xmm3, %%xmm7\n\t"
123  " add $32, %%r10\n\t"
124  ".%=L1_test:\n\t"
125  " dec %%rax\n\t"
126  " jge .%=Loop1\n\t"
127  " # We've handled the bulk of multiplies up to here.\n\t"
128  " # Let's sse if original n_2_ccomplex_blocks was odd.\n\t"
129  " # If so, we've got 2 more taps to do.\n\t"
130  " and $1, %%r8\n\t"
131  " je .%=Leven\n\t"
132  " # The count was odd, do 2 more taps.\n\t"
133  " # Note that we've already got mm0/mm2 preloaded\n\t"
134  " # from the main loop.\n\t"
135  " movaps %%xmm0, %%xmm4\n\t"
136  " mulps %%xmm2, %%xmm0\n\t"
137  " shufps $0xb1, %%xmm4, %%xmm4 # swap internals\n\t"
138  " addps %%xmm0, %%xmm6\n\t"
139  " mulps %%xmm4, %%xmm2\n\t"
140  " addps %%xmm2, %%xmm7\n\t"
141  ".%=Leven:\n\t"
142  " # neg inversor\n\t"
143  " xorps %%xmm1, %%xmm1\n\t"
144  " mov $0x80000000, %%r9\n\t"
145  " movd %%r9, %%xmm1\n\t"
146  " shufps $0x11, %%xmm1, %%xmm1 # b00010001 # 0 -0 0 -0\n\t"
147  " # pfpnacc\n\t"
148  " xorps %%xmm1, %%xmm6\n\t"
149  " movaps %%xmm6, %%xmm2\n\t"
150  " unpcklps %%xmm7, %%xmm6\n\t"
151  " unpckhps %%xmm7, %%xmm2\n\t"
152  " movaps %%xmm2, %%xmm3\n\t"
153  " shufps $0x44, %%xmm6, %%xmm2 # b01000100\n\t"
154  " shufps $0xee, %%xmm3, %%xmm6 # b11101110\n\t"
155  " addps %%xmm2, %%xmm6\n\t"
156  " # xmm6 = r1 i2 r3 i4\n\t"
157  " movhlps %%xmm6, %%xmm4 # xmm4 = r3 i4 ?? ??\n\t"
158  " addps %%xmm4, %%xmm6 # xmm6 = r1+r3 i2+i4 ?? ??\n\t"
159  " movlps %%xmm6, (%[rdi]) # store low 2x32 bits (complex) to memory\n\t"
160  :
161  :[rsi] "r" (input), [rdx] "r" (taps), "c" (num_bytes), [rdi] "r" (result)
162  :"rax", "r8", "r9", "r10"
163  );
164 
165 
166  if(((num_bytes >> 3) & 1)) {
167  *result += (input[(num_bytes >> 3) - 1] * taps[(num_bytes >> 3) - 1]);
168  }
169 
170  return;
171 
172 }
173 
174 #endif
175 
176 #if LV_HAVE_SSE && LV_HAVE_32
177 
178 static inline void volk_32fc_x2_dot_prod_32fc_a_sse_32(lv_32fc_t* result, const lv_32fc_t* input, const lv_32fc_t* taps, unsigned int num_bytes) {
179 
180  volk_32fc_x2_dot_prod_32fc_a_generic(result, input, taps, num_bytes);
181 
182 #if 0
183  asm volatile
184  (
185  " #pushl %%ebp\n\t"
186  " #movl %%esp, %%ebp\n\t"
187  " movl 12(%%ebp), %%eax # input\n\t"
188  " movl 16(%%ebp), %%edx # taps\n\t"
189  " movl 20(%%ebp), %%ecx # n_bytes\n\t"
190  " xorps %%xmm6, %%xmm6 # zero accumulators\n\t"
191  " movaps 0(%%eax), %%xmm0\n\t"
192  " xorps %%xmm7, %%xmm7 # zero accumulators\n\t"
193  " movaps 0(%%edx), %%xmm2\n\t"
194  " shrl $5, %%ecx # ecx = n_2_ccomplex_blocks / 2\n\t"
195  " jmp .%=L1_test\n\t"
196  " # 4 taps / loop\n\t"
197  " # something like ?? cycles / loop\n\t"
198  ".%=Loop1: \n\t"
199  "# complex prod: C += A * B, w/ temp Z & Y (or B), xmmPN=$0x8000000080000000\n\t"
200  "# movaps (%%eax), %%xmmA\n\t"
201  "# movaps (%%edx), %%xmmB\n\t"
202  "# movaps %%xmmA, %%xmmZ\n\t"
203  "# shufps $0xb1, %%xmmZ, %%xmmZ # swap internals\n\t"
204  "# mulps %%xmmB, %%xmmA\n\t"
205  "# mulps %%xmmZ, %%xmmB\n\t"
206  "# # SSE replacement for: pfpnacc %%xmmB, %%xmmA\n\t"
207  "# xorps %%xmmPN, %%xmmA\n\t"
208  "# movaps %%xmmA, %%xmmZ\n\t"
209  "# unpcklps %%xmmB, %%xmmA\n\t"
210  "# unpckhps %%xmmB, %%xmmZ\n\t"
211  "# movaps %%xmmZ, %%xmmY\n\t"
212  "# shufps $0x44, %%xmmA, %%xmmZ # b01000100\n\t"
213  "# shufps $0xee, %%xmmY, %%xmmA # b11101110\n\t"
214  "# addps %%xmmZ, %%xmmA\n\t"
215  "# addps %%xmmA, %%xmmC\n\t"
216  "# A=xmm0, B=xmm2, Z=xmm4\n\t"
217  "# A'=xmm1, B'=xmm3, Z'=xmm5\n\t"
218  " movaps 16(%%eax), %%xmm1\n\t"
219  " movaps %%xmm0, %%xmm4\n\t"
220  " mulps %%xmm2, %%xmm0\n\t"
221  " shufps $0xb1, %%xmm4, %%xmm4 # swap internals\n\t"
222  " movaps 16(%%edx), %%xmm3\n\t"
223  " movaps %%xmm1, %%xmm5\n\t"
224  " addps %%xmm0, %%xmm6\n\t"
225  " mulps %%xmm3, %%xmm1\n\t"
226  " shufps $0xb1, %%xmm5, %%xmm5 # swap internals\n\t"
227  " addps %%xmm1, %%xmm6\n\t"
228  " mulps %%xmm4, %%xmm2\n\t"
229  " movaps 32(%%eax), %%xmm0\n\t"
230  " addps %%xmm2, %%xmm7\n\t"
231  " mulps %%xmm5, %%xmm3\n\t"
232  " addl $32, %%eax\n\t"
233  " movaps 32(%%edx), %%xmm2\n\t"
234  " addps %%xmm3, %%xmm7\n\t"
235  " addl $32, %%edx\n\t"
236  ".%=L1_test:\n\t"
237  " decl %%ecx\n\t"
238  " jge .%=Loop1\n\t"
239  " # We've handled the bulk of multiplies up to here.\n\t"
240  " # Let's sse if original n_2_ccomplex_blocks was odd.\n\t"
241  " # If so, we've got 2 more taps to do.\n\t"
242  " movl 20(%%ebp), %%ecx # n_2_ccomplex_blocks\n\t"
243  " shrl $4, %%ecx\n\t"
244  " andl $1, %%ecx\n\t"
245  " je .%=Leven\n\t"
246  " # The count was odd, do 2 more taps.\n\t"
247  " # Note that we've already got mm0/mm2 preloaded\n\t"
248  " # from the main loop.\n\t"
249  " movaps %%xmm0, %%xmm4\n\t"
250  " mulps %%xmm2, %%xmm0\n\t"
251  " shufps $0xb1, %%xmm4, %%xmm4 # swap internals\n\t"
252  " addps %%xmm0, %%xmm6\n\t"
253  " mulps %%xmm4, %%xmm2\n\t"
254  " addps %%xmm2, %%xmm7\n\t"
255  ".%=Leven:\n\t"
256  " # neg inversor\n\t"
257  " movl 8(%%ebp), %%eax \n\t"
258  " xorps %%xmm1, %%xmm1\n\t"
259  " movl $0x80000000, (%%eax)\n\t"
260  " movss (%%eax), %%xmm1\n\t"
261  " shufps $0x11, %%xmm1, %%xmm1 # b00010001 # 0 -0 0 -0\n\t"
262  " # pfpnacc\n\t"
263  " xorps %%xmm1, %%xmm6\n\t"
264  " movaps %%xmm6, %%xmm2\n\t"
265  " unpcklps %%xmm7, %%xmm6\n\t"
266  " unpckhps %%xmm7, %%xmm2\n\t"
267  " movaps %%xmm2, %%xmm3\n\t"
268  " shufps $0x44, %%xmm6, %%xmm2 # b01000100\n\t"
269  " shufps $0xee, %%xmm3, %%xmm6 # b11101110\n\t"
270  " addps %%xmm2, %%xmm6\n\t"
271  " # xmm6 = r1 i2 r3 i4\n\t"
272  " #movl 8(%%ebp), %%eax # @result\n\t"
273  " movhlps %%xmm6, %%xmm4 # xmm4 = r3 i4 ?? ??\n\t"
274  " addps %%xmm4, %%xmm6 # xmm6 = r1+r3 i2+i4 ?? ??\n\t"
275  " movlps %%xmm6, (%%eax) # store low 2x32 bits (complex) to memory\n\t"
276  " #popl %%ebp\n\t"
277  :
278  :
279  : "eax", "ecx", "edx"
280  );
281 
282 
283  int getem = num_bytes % 16;
284 
285  for(; getem > 0; getem -= 8) {
286 
287 
288  *result += (input[(num_bytes >> 3) - 1] * taps[(num_bytes >> 3) - 1]);
289 
290  }
291 
292  return;
293 #endif
294 }
295 
296 #endif /*LV_HAVE_SSE*/
297 
298 #ifdef LV_HAVE_SSE3
299 
300 #include <pmmintrin.h>
301 
302 static inline void volk_32fc_x2_dot_prod_32fc_a_sse3(lv_32fc_t* result, const lv_32fc_t* input, const lv_32fc_t* taps, unsigned int num_bytes) {
303 
304 
305  lv_32fc_t dotProduct;
306  memset(&dotProduct, 0x0, 2*sizeof(float));
307 
308  unsigned int number = 0;
309  const unsigned int halfPoints = num_bytes >> 4;
310 
311  __m128 x, y, yl, yh, z, tmp1, tmp2, dotProdVal;
312 
313  const lv_32fc_t* a = input;
314  const lv_32fc_t* b = taps;
315 
316  dotProdVal = _mm_setzero_ps();
317 
318  for(;number < halfPoints; number++){
319 
320  x = _mm_load_ps((float*)a); // Load the ar + ai, br + bi as ar,ai,br,bi
321  y = _mm_load_ps((float*)b); // Load the cr + ci, dr + di as cr,ci,dr,di
322 
323  yl = _mm_moveldup_ps(y); // Load yl with cr,cr,dr,dr
324  yh = _mm_movehdup_ps(y); // Load yh with ci,ci,di,di
325 
326  tmp1 = _mm_mul_ps(x,yl); // tmp1 = ar*cr,ai*cr,br*dr,bi*dr
327 
328  x = _mm_shuffle_ps(x,x,0xB1); // Re-arrange x to be ai,ar,bi,br
329 
330  tmp2 = _mm_mul_ps(x,yh); // tmp2 = ai*ci,ar*ci,bi*di,br*di
331 
332  z = _mm_addsub_ps(tmp1,tmp2); // ar*cr-ai*ci, ai*cr+ar*ci, br*dr-bi*di, bi*dr+br*di
333 
334  dotProdVal = _mm_add_ps(dotProdVal, z); // Add the complex multiplication results together
335 
336  a += 2;
337  b += 2;
338  }
339 
340  __VOLK_ATTR_ALIGNED(16) lv_32fc_t dotProductVector[2];
341 
342  _mm_store_ps((float*)dotProductVector,dotProdVal); // Store the results back into the dot product vector
343 
344  dotProduct += ( dotProductVector[0] + dotProductVector[1] );
345 
346  if(((num_bytes >> 3) & 1) != 0) {
347  dotProduct += (*a) * (*b);
348  }
349 
350  *result = dotProduct;
351 }
352 
353 #endif /*LV_HAVE_SSE3*/
354 
355 #ifdef LV_HAVE_SSE4_1
356 
357 #include <smmintrin.h>
358 
359 static inline void volk_32fc_x2_dot_prod_32fc_a_sse4_1(lv_32fc_t* result, const lv_32fc_t* input, const lv_32fc_t* taps, unsigned int num_bytes) {
360 
361  __m128 xmm0, xmm1, xmm2, xmm3, xmm4, xmm5, xmm6, xmm7, real0, real1, im0, im1;
362  float *p_input, *p_taps;
363  __m64 *p_result;
364 
365  p_result = (__m64*)result;
366  p_input = (float*)input;
367  p_taps = (float*)taps;
368 
369  static const __m128i neg = {0x000000000000000080000000};
370 
371  int i = 0;
372 
373  int bound = (num_bytes >> 5);
374  int leftovers = (num_bytes & 24) >> 3;
375 
376  real0 = _mm_sub_ps(real0, real0);
377  real1 = _mm_sub_ps(real1, real1);
378  im0 = _mm_sub_ps(im0, im0);
379  im1 = _mm_sub_ps(im1, im1);
380 
381  for(; i < bound; ++i) {
382 
383 
384  xmm0 = _mm_load_ps(p_input);
385  xmm1 = _mm_load_ps(p_taps);
386 
387  p_input += 4;
388  p_taps += 4;
389 
390  xmm2 = _mm_load_ps(p_input);
391  xmm3 = _mm_load_ps(p_taps);
392 
393  p_input += 4;
394  p_taps += 4;
395 
396  xmm4 = _mm_unpackhi_ps(xmm0, xmm2);
397  xmm5 = _mm_unpackhi_ps(xmm1, xmm3);
398  xmm0 = _mm_unpacklo_ps(xmm0, xmm2);
399  xmm2 = _mm_unpacklo_ps(xmm1, xmm3);
400 
401  //imaginary vector from input
402  xmm1 = _mm_unpackhi_ps(xmm0, xmm4);
403  //real vector from input
404  xmm3 = _mm_unpacklo_ps(xmm0, xmm4);
405  //imaginary vector from taps
406  xmm0 = _mm_unpackhi_ps(xmm2, xmm5);
407  //real vector from taps
408  xmm2 = _mm_unpacklo_ps(xmm2, xmm5);
409 
410  xmm4 = _mm_dp_ps(xmm3, xmm2, 0xf1);
411  xmm5 = _mm_dp_ps(xmm1, xmm0, 0xf1);
412 
413  xmm6 = _mm_dp_ps(xmm3, xmm0, 0xf2);
414  xmm7 = _mm_dp_ps(xmm1, xmm2, 0xf2);
415 
416  real0 = _mm_add_ps(xmm4, real0);
417  real1 = _mm_add_ps(xmm5, real1);
418  im0 = _mm_add_ps(xmm6, im0);
419  im1 = _mm_add_ps(xmm7, im1);
420 
421  }
422 
423  real1 = _mm_xor_ps(real1, bit128_p(&neg)->float_vec);
424 
425  im0 = _mm_add_ps(im0, im1);
426  real0 = _mm_add_ps(real0, real1);
427 
428  im0 = _mm_add_ps(im0, real0);
429 
430  _mm_storel_pi(p_result, im0);
431 
432  for(i = bound * 4; i < (bound * 4) + leftovers; ++i) {
433 
434  *result += input[i] * taps[i];
435  }
436 }
437 
438 #endif /*LV_HAVE_SSE4_1*/
439 
440 #endif /*INCLUDED_volk_32fc_x2_dot_prod_32fc_a_H*/