GNU Radio 3.4.0 C++ API
volk_32fc_x2_dot_prod_32fc_a16.h
Go to the documentation of this file.
00001 #ifndef INCLUDED_volk_32fc_x2_dot_prod_32fc_a16_H
00002 #define INCLUDED_volk_32fc_x2_dot_prod_32fc_a16_H
00003 
00004 #include <volk/volk_complex.h>
00005 #include <stdio.h>
00006 #include <string.h>
00007 
00008 
00009 #if LV_HAVE_GENERIC 
00010 
00011 
00012 static inline void volk_32fc_x2_dot_prod_32fc_a16_generic(lv_32fc_t* result, const lv_32fc_t* input, const lv_32fc_t* taps, unsigned int num_bytes) {
00013   
00014   float * res = (float*) result;
00015   float * in = (float*) input;
00016   float * tp = (float*) taps;
00017   unsigned int n_2_ccomplex_blocks = num_bytes >> 4;
00018   unsigned int isodd = (num_bytes >> 3) &1;
00019   
00020   
00021   
00022   float sum0[2] = {0,0};
00023   float sum1[2] = {0,0};
00024   int i = 0;
00025 
00026   
00027   for(i = 0; i < n_2_ccomplex_blocks; ++i) {
00028     
00029 
00030     sum0[0] += in[0] * tp[0] - in[1] * tp[1];
00031     sum0[1] += in[0] * tp[1] + in[1] * tp[0];
00032     sum1[0] += in[2] * tp[2] - in[3] * tp[3];
00033     sum1[1] += in[2] * tp[3] + in[3] * tp[2];
00034     
00035     
00036     in += 4;
00037     tp += 4;
00038 
00039   }
00040 
00041   
00042   res[0] = sum0[0] + sum1[0];
00043   res[1] = sum0[1] + sum1[1];
00044   
00045   
00046   
00047   for(i = 0; i < isodd; ++i) {
00048 
00049 
00050     *result += input[(num_bytes >> 3) - 1] * taps[(num_bytes >> 3) - 1];
00051 
00052   }
00053 
00054 }
00055 
00056 #endif /*LV_HAVE_GENERIC*/
00057 
00058 
00059 #if LV_HAVE_SSE && LV_HAVE_64
00060 
00061 
00062 static inline void volk_32fc_x2_dot_prod_32fc_a16_sse_64(lv_32fc_t* result, const lv_32fc_t* input, const lv_32fc_t* taps, unsigned int num_bytes) {
00063   
00064 
00065   asm 
00066     (
00067      "#  ccomplex_dotprod_generic (float* result, const float *input,\n\t"
00068      "#                         const float *taps, unsigned num_bytes)\n\t"
00069      "#    float sum0 = 0;\n\t"
00070      "#    float sum1 = 0;\n\t"
00071      "#    float sum2 = 0;\n\t"
00072      "#    float sum3 = 0;\n\t"
00073      "#    do {\n\t"
00074      "#      sum0 += input[0] * taps[0] - input[1] * taps[1];\n\t"
00075      "#      sum1 += input[0] * taps[1] + input[1] * taps[0];\n\t"
00076      "#      sum2 += input[2] * taps[2] - input[3] * taps[3];\n\t"
00077      "#      sum3 += input[2] * taps[3] + input[3] * taps[2];\n\t"
00078      "#      input += 4;\n\t"
00079      "#      taps += 4;  \n\t"
00080      "#    } while (--n_2_ccomplex_blocks != 0);\n\t"
00081      "#    result[0] = sum0 + sum2;\n\t"
00082      "#    result[1] = sum1 + sum3;\n\t"
00083      "# TODO: prefetch and better scheduling\n\t"
00084      "  xor    %%r9,  %%r9\n\t"
00085      "  xor    %%r10, %%r10\n\t"
00086      "  movq   %%rcx, %%rax\n\t"
00087      "  movq   %%rcx, %%r8\n\t"
00088      "  movq   %[rsi],  %%r9\n\t"
00089      "  movq   %[rdx], %%r10\n\t"
00090      "  xorps   %%xmm6, %%xmm6          # zero accumulators\n\t"
00091      "  movaps  0(%%r9), %%xmm0\n\t"
00092      "  xorps   %%xmm7, %%xmm7          # zero accumulators\n\t"
00093      "  movaps  0(%%r10), %%xmm2\n\t"
00094      "  shr     $5, %%rax               # rax = n_2_ccomplex_blocks / 2\n\t"
00095      "  shr     $4, %%r8\n\t"
00096      "  jmp     .%=L1_test\n\t"
00097      "  # 4 taps / loop\n\t"
00098      "  # something like ?? cycles / loop\n\t"
00099      ".%=Loop1: \n\t"
00100      "# complex prod: C += A * B,  w/ temp Z & Y (or B), xmmPN=$0x8000000080000000\n\t"
00101      "# movaps  (%%r9), %%xmmA\n\t"
00102      "# movaps  (%%r10), %%xmmB\n\t"
00103      "# movaps  %%xmmA, %%xmmZ\n\t"
00104      "# shufps  $0xb1, %%xmmZ, %%xmmZ   # swap internals\n\t"
00105      "# mulps   %%xmmB, %%xmmA\n\t"
00106      "# mulps   %%xmmZ, %%xmmB\n\t"
00107      "# # SSE replacement for: pfpnacc %%xmmB, %%xmmA\n\t"
00108      "# xorps   %%xmmPN, %%xmmA\n\t"
00109      "# movaps  %%xmmA, %%xmmZ\n\t"
00110      "# unpcklps %%xmmB, %%xmmA\n\t"
00111      "# unpckhps %%xmmB, %%xmmZ\n\t"
00112      "# movaps  %%xmmZ, %%xmmY\n\t"
00113      "# shufps  $0x44, %%xmmA, %%xmmZ   # b01000100\n\t"
00114      "# shufps  $0xee, %%xmmY, %%xmmA   # b11101110\n\t"
00115      "# addps   %%xmmZ, %%xmmA\n\t"
00116      "# addps   %%xmmA, %%xmmC\n\t"
00117      "# A=xmm0, B=xmm2, Z=xmm4\n\t"
00118      "# A'=xmm1, B'=xmm3, Z'=xmm5\n\t"
00119      "  movaps  16(%%r9), %%xmm1\n\t"
00120      "  movaps  %%xmm0, %%xmm4\n\t"
00121      "  mulps   %%xmm2, %%xmm0\n\t"
00122      "  shufps  $0xb1, %%xmm4, %%xmm4   # swap internals\n\t"
00123      "  movaps  16(%%r10), %%xmm3\n\t"
00124      "  movaps  %%xmm1, %%xmm5\n\t"
00125      "  addps   %%xmm0, %%xmm6\n\t"
00126      "  mulps   %%xmm3, %%xmm1\n\t"
00127      "  shufps  $0xb1, %%xmm5, %%xmm5   # swap internals\n\t"
00128      "  addps   %%xmm1, %%xmm6\n\t"
00129      "  mulps   %%xmm4, %%xmm2\n\t"
00130      "  movaps  32(%%r9), %%xmm0\n\t"
00131      "  addps   %%xmm2, %%xmm7\n\t"
00132      "  mulps   %%xmm5, %%xmm3\n\t"
00133      "  add     $32, %%r9\n\t"
00134      "  movaps  32(%%r10), %%xmm2\n\t"
00135      "  addps   %%xmm3, %%xmm7\n\t"
00136      "  add     $32, %%r10\n\t"
00137      ".%=L1_test:\n\t"
00138      "  dec     %%rax\n\t"
00139      "  jge     .%=Loop1\n\t"
00140      "  # We've handled the bulk of multiplies up to here.\n\t"
00141      "  # Let's sse if original n_2_ccomplex_blocks was odd.\n\t"
00142      "  # If so, we've got 2 more taps to do.\n\t"
00143      "  and     $1, %%r8\n\t"
00144      "  je      .%=Leven\n\t"
00145      "  # The count was odd, do 2 more taps.\n\t"
00146      "  # Note that we've already got mm0/mm2 preloaded\n\t"
00147      "  # from the main loop.\n\t"
00148      "  movaps  %%xmm0, %%xmm4\n\t"
00149      "  mulps   %%xmm2, %%xmm0\n\t"
00150      "  shufps  $0xb1, %%xmm4, %%xmm4   # swap internals\n\t"
00151      "  addps   %%xmm0, %%xmm6\n\t"
00152      "  mulps   %%xmm4, %%xmm2\n\t"
00153      "  addps   %%xmm2, %%xmm7\n\t"
00154      ".%=Leven:\n\t"
00155      "  # neg inversor\n\t"
00156      "  xorps   %%xmm1, %%xmm1\n\t"
00157      "  mov     $0x80000000, %%r9\n\t"
00158      "  movd    %%r9, %%xmm1\n\t"
00159      "  shufps  $0x11, %%xmm1, %%xmm1   # b00010001 # 0 -0 0 -0\n\t"
00160      "  # pfpnacc\n\t"
00161      "  xorps   %%xmm1, %%xmm6\n\t"
00162      "  movaps  %%xmm6, %%xmm2\n\t"
00163      "  unpcklps %%xmm7, %%xmm6\n\t"
00164      "  unpckhps %%xmm7, %%xmm2\n\t"
00165      "  movaps  %%xmm2, %%xmm3\n\t"
00166      "  shufps  $0x44, %%xmm6, %%xmm2   # b01000100\n\t"
00167      "  shufps  $0xee, %%xmm3, %%xmm6   # b11101110\n\t"
00168      "  addps   %%xmm2, %%xmm6\n\t"
00169      "                                  # xmm6 = r1 i2 r3 i4\n\t"
00170      "  movhlps %%xmm6, %%xmm4          # xmm4 = r3 i4 ?? ??\n\t"
00171      "  addps   %%xmm4, %%xmm6          # xmm6 = r1+r3 i2+i4 ?? ??\n\t"
00172      "  movlps  %%xmm6, (%[rdi])                # store low 2x32 bits (complex) to memory\n\t"
00173      :
00174      :[rsi] "r" (input), [rdx] "r" (taps), "c" (num_bytes), [rdi] "r" (result)
00175      :"rax", "r8", "r9", "r10"
00176      );
00177   
00178   
00179   int getem = num_bytes % 16;
00180   
00181   
00182   for(; getem > 0; getem -= 8) {
00183   
00184     
00185     *result += (input[(num_bytes >> 3) - 1] * taps[(num_bytes >> 3) - 1]);
00186   
00187   }
00188 
00189   return;
00190   
00191 }
00192 
00193 #endif
00194 
00195 #if LV_HAVE_SSE && LV_HAVE_32
00196 
00197 static inline void volk_32fc_x2_dot_prod_32fc_a16_sse_32(lv_32fc_t* result, const lv_32fc_t* input, const lv_32fc_t* taps, unsigned int num_bytes) {
00198   
00199   asm volatile 
00200     (
00201      "  #pushl  %%ebp\n\t"
00202      "  #movl   %%esp, %%ebp\n\t"
00203      "  movl    12(%%ebp), %%eax                # input\n\t"
00204      "  movl    16(%%ebp), %%edx                # taps\n\t"
00205      "  movl    20(%%ebp), %%ecx                # n_bytes\n\t"
00206      "  xorps   %%xmm6, %%xmm6          # zero accumulators\n\t"
00207      "  movaps  0(%%eax), %%xmm0\n\t"
00208      "  xorps   %%xmm7, %%xmm7          # zero accumulators\n\t"
00209      "  movaps  0(%%edx), %%xmm2\n\t"
00210      "  shrl    $5, %%ecx               # ecx = n_2_ccomplex_blocks / 2\n\t"
00211      "  jmp     .%=L1_test\n\t"
00212      "  # 4 taps / loop\n\t"
00213      "  # something like ?? cycles / loop\n\t"
00214      ".%=Loop1: \n\t"
00215      "# complex prod: C += A * B,  w/ temp Z & Y (or B), xmmPN=$0x8000000080000000\n\t"
00216      "# movaps  (%%eax), %%xmmA\n\t"
00217      "# movaps  (%%edx), %%xmmB\n\t"
00218      "# movaps  %%xmmA, %%xmmZ\n\t"
00219      "# shufps  $0xb1, %%xmmZ, %%xmmZ   # swap internals\n\t"
00220      "# mulps   %%xmmB, %%xmmA\n\t"
00221      "# mulps   %%xmmZ, %%xmmB\n\t"
00222      "# # SSE replacement for: pfpnacc %%xmmB, %%xmmA\n\t"
00223      "# xorps   %%xmmPN, %%xmmA\n\t"
00224      "# movaps  %%xmmA, %%xmmZ\n\t"
00225      "# unpcklps %%xmmB, %%xmmA\n\t"
00226      "# unpckhps %%xmmB, %%xmmZ\n\t"
00227      "# movaps  %%xmmZ, %%xmmY\n\t"
00228      "# shufps  $0x44, %%xmmA, %%xmmZ   # b01000100\n\t"
00229      "# shufps  $0xee, %%xmmY, %%xmmA   # b11101110\n\t"
00230      "# addps   %%xmmZ, %%xmmA\n\t"
00231      "# addps   %%xmmA, %%xmmC\n\t"
00232      "# A=xmm0, B=xmm2, Z=xmm4\n\t"
00233      "# A'=xmm1, B'=xmm3, Z'=xmm5\n\t"
00234      "  movaps  16(%%eax), %%xmm1\n\t"
00235      "  movaps  %%xmm0, %%xmm4\n\t"
00236      "  mulps   %%xmm2, %%xmm0\n\t"
00237      "  shufps  $0xb1, %%xmm4, %%xmm4   # swap internals\n\t"
00238      "  movaps  16(%%edx), %%xmm3\n\t"
00239      "  movaps  %%xmm1, %%xmm5\n\t"
00240      "  addps   %%xmm0, %%xmm6\n\t"
00241      "  mulps   %%xmm3, %%xmm1\n\t"
00242      "  shufps  $0xb1, %%xmm5, %%xmm5   # swap internals\n\t"
00243      "  addps   %%xmm1, %%xmm6\n\t"
00244      "  mulps   %%xmm4, %%xmm2\n\t"
00245      "  movaps  32(%%eax), %%xmm0\n\t"
00246      "  addps   %%xmm2, %%xmm7\n\t"
00247      "  mulps   %%xmm5, %%xmm3\n\t"
00248      "  addl    $32, %%eax\n\t"
00249      "  movaps  32(%%edx), %%xmm2\n\t"
00250      "  addps   %%xmm3, %%xmm7\n\t"
00251      "  addl    $32, %%edx\n\t"
00252      ".%=L1_test:\n\t"
00253      "  decl    %%ecx\n\t"
00254      "  jge     .%=Loop1\n\t"
00255      "  # We've handled the bulk of multiplies up to here.\n\t"
00256      "  # Let's sse if original n_2_ccomplex_blocks was odd.\n\t"
00257      "  # If so, we've got 2 more taps to do.\n\t"
00258      "  movl    20(%%ebp), %%ecx                # n_2_ccomplex_blocks\n\t"
00259      "  shrl    $4, %%ecx\n\t"
00260      "  andl    $1, %%ecx\n\t"
00261      "  je      .%=Leven\n\t"
00262      "  # The count was odd, do 2 more taps.\n\t"
00263      "  # Note that we've already got mm0/mm2 preloaded\n\t"
00264      "  # from the main loop.\n\t"
00265      "  movaps  %%xmm0, %%xmm4\n\t"
00266      "  mulps   %%xmm2, %%xmm0\n\t"
00267      "  shufps  $0xb1, %%xmm4, %%xmm4   # swap internals\n\t"
00268      "  addps   %%xmm0, %%xmm6\n\t"
00269      "  mulps   %%xmm4, %%xmm2\n\t"
00270      "  addps   %%xmm2, %%xmm7\n\t"
00271      ".%=Leven:\n\t"
00272      "  # neg inversor\n\t"
00273      "  movl 8(%%ebp), %%eax \n\t"
00274      "  xorps   %%xmm1, %%xmm1\n\t"
00275      "  movl    $0x80000000, (%%eax)\n\t"
00276      "  movss   (%%eax), %%xmm1\n\t"
00277      "  shufps  $0x11, %%xmm1, %%xmm1   # b00010001 # 0 -0 0 -0\n\t"
00278      "  # pfpnacc\n\t"
00279      "  xorps   %%xmm1, %%xmm6\n\t"
00280      "  movaps  %%xmm6, %%xmm2\n\t"
00281      "  unpcklps %%xmm7, %%xmm6\n\t"
00282      "  unpckhps %%xmm7, %%xmm2\n\t"
00283      "  movaps  %%xmm2, %%xmm3\n\t"
00284      "  shufps  $0x44, %%xmm6, %%xmm2   # b01000100\n\t"
00285      "  shufps  $0xee, %%xmm3, %%xmm6   # b11101110\n\t"
00286      "  addps   %%xmm2, %%xmm6\n\t"
00287      "                                  # xmm6 = r1 i2 r3 i4\n\t"
00288      "  #movl   8(%%ebp), %%eax         # @result\n\t"
00289      "  movhlps %%xmm6, %%xmm4          # xmm4 = r3 i4 ?? ??\n\t"
00290      "  addps   %%xmm4, %%xmm6          # xmm6 = r1+r3 i2+i4 ?? ??\n\t"
00291      "  movlps  %%xmm6, (%%eax)         # store low 2x32 bits (complex) to memory\n\t"
00292      "  #popl   %%ebp\n\t"
00293      :
00294      :
00295      : "eax", "ecx", "edx"
00296      );
00297 
00298   
00299   int getem = num_bytes % 16;
00300   
00301   for(; getem > 0; getem -= 8) {
00302     
00303     
00304     *result += (input[(num_bytes >> 3) - 1] * taps[(num_bytes >> 3) - 1]);
00305     
00306   }
00307   
00308   return;
00309   
00310   
00311   
00312 
00313   
00314   
00315 }
00316 
00317 #endif /*LV_HAVE_SSE*/  
00318 
00319 #if LV_HAVE_SSE3
00320 
00321 #include <pmmintrin.h>
00322 
00323 static inline void volk_32fc_x2_dot_prod_32fc_a16_sse3(lv_32fc_t* result, const lv_32fc_t* input, const lv_32fc_t* taps, unsigned int num_bytes) {
00324   
00325 
00326   lv_32fc_t dotProduct;
00327   memset(&dotProduct, 0x0, 2*sizeof(float));
00328 
00329   unsigned int number = 0;
00330   const unsigned int halfPoints = num_bytes >> 4;
00331 
00332   __m128 x, y, yl, yh, z, tmp1, tmp2, dotProdVal;
00333 
00334   const lv_32fc_t* a = input;
00335   const lv_32fc_t* b = taps;
00336 
00337   dotProdVal = _mm_setzero_ps();
00338 
00339   for(;number < halfPoints; number++){
00340       
00341     x = _mm_load_ps((float*)a); // Load the ar + ai, br + bi as ar,ai,br,bi
00342     y = _mm_load_ps((float*)b); // Load the cr + ci, dr + di as cr,ci,dr,di
00343       
00344     yl = _mm_moveldup_ps(y); // Load yl with cr,cr,dr,dr
00345     yh = _mm_movehdup_ps(y); // Load yh with ci,ci,di,di
00346       
00347     tmp1 = _mm_mul_ps(x,yl); // tmp1 = ar*cr,ai*cr,br*dr,bi*dr
00348       
00349     x = _mm_shuffle_ps(x,x,0xB1); // Re-arrange x to be ai,ar,bi,br
00350       
00351     tmp2 = _mm_mul_ps(x,yh); // tmp2 = ai*ci,ar*ci,bi*di,br*di
00352       
00353     z = _mm_addsub_ps(tmp1,tmp2); // ar*cr-ai*ci, ai*cr+ar*ci, br*dr-bi*di, bi*dr+br*di
00354 
00355     dotProdVal = _mm_add_ps(dotProdVal, z); // Add the complex multiplication results together
00356 
00357     a += 2;
00358     b += 2;
00359   }
00360 
00361   lv_32fc_t dotProductVector[2] __attribute__((aligned(16)));
00362 
00363   _mm_store_ps((float*)dotProductVector,dotProdVal); // Store the results back into the dot product vector
00364 
00365   dotProduct += ( dotProductVector[0] + dotProductVector[1] );
00366 
00367   if((num_bytes >> 2) != 0) {
00368     dotProduct += (*a) * (*b);
00369   }
00370 
00371   *result = dotProduct;
00372 }  
00373 
00374 #endif /*LV_HAVE_SSE3*/
00375 
00376 #if LV_HAVE_SSE4_1
00377 
00378 #include <smmintrin.h>
00379 
00380 static inline void volk_32fc_x2_dot_prod_32fc_a16_sse4_1(lv_32fc_t* result, const lv_32fc_t* input, const lv_32fc_t* taps, unsigned int num_bytes) {
00381   volk_32fc_x2_dot_prod_32fc_a16_sse3(result, input, taps, num_bytes);
00382   // SSE3 version runs twice as fast as the SSE4.1 version, so turning off SSE4 version for now
00383    /* 
00384     __m128 xmm0, xmm1, xmm2, xmm3, xmm4, xmm5, xmm6, xmm7, real0, real1, im0, im1;
00385     float *p_input, *p_taps;
00386     __m64 *p_result;
00387 
00388     p_result = (__m64*)result;
00389     p_input = (float*)input;
00390     p_taps = (float*)taps;
00391 
00392     static const __m128i neg = {0x000000000000000080000000};
00393 
00394     int i = 0;
00395   
00396     int bound = (num_bytes >> 5);
00397     int leftovers = (num_bytes & 24) >> 3;
00398 
00399     real0 = _mm_sub_ps(real0, real0);
00400     real1 = _mm_sub_ps(real1, real1);
00401     im0 = _mm_sub_ps(im0, im0);
00402     im1 = _mm_sub_ps(im1, im1);
00403   
00404     for(; i < bound; ++i) {
00405   
00406     
00407     xmm0 = _mm_load_ps(p_input);
00408     xmm1 = _mm_load_ps(p_taps);
00409     
00410     p_input += 4;
00411     p_taps += 4;
00412     
00413     xmm2 = _mm_load_ps(p_input);
00414     xmm3 = _mm_load_ps(p_taps);
00415     
00416     p_input += 4;
00417     p_taps += 4;
00418     
00419     xmm4 = _mm_unpackhi_ps(xmm0, xmm2);
00420     xmm5 = _mm_unpackhi_ps(xmm1, xmm3);
00421     xmm0 = _mm_unpacklo_ps(xmm0, xmm2);
00422     xmm2 = _mm_unpacklo_ps(xmm1, xmm3);
00423     
00424     //imaginary vector from input
00425     xmm1 = _mm_unpackhi_ps(xmm0, xmm4);
00426     //real vector from input
00427     xmm3 = _mm_unpacklo_ps(xmm0, xmm4);
00428     //imaginary vector from taps
00429     xmm0 = _mm_unpackhi_ps(xmm2, xmm5);
00430     //real vector from taps
00431     xmm2 = _mm_unpacklo_ps(xmm2, xmm5);
00432     
00433     xmm4 = _mm_dp_ps(xmm3, xmm2, 0xf1);
00434     xmm5 = _mm_dp_ps(xmm1, xmm0, 0xf1);
00435     
00436     xmm6 = _mm_dp_ps(xmm3, xmm0, 0xf2);
00437     xmm7 = _mm_dp_ps(xmm1, xmm2, 0xf2);
00438     
00439     real0 = _mm_add_ps(xmm4, real0);
00440     real1 = _mm_add_ps(xmm5, real1);
00441     im0 = _mm_add_ps(xmm6, im0);
00442     im1 = _mm_add_ps(xmm7, im1);
00443     
00444     }
00445 
00446 
00447     
00448     
00449     real1 = _mm_xor_ps(real1, (__m128)neg);
00450     
00451   
00452     im0 = _mm_add_ps(im0, im1);
00453     real0 = _mm_add_ps(real0, real1);
00454   
00455     im0 = _mm_add_ps(im0, real0);
00456   
00457     _mm_storel_pi(p_result, im0);
00458   
00459     for(i = bound * 4; i < (bound * 4) + leftovers; ++i) {
00460     
00461     *result += input[i] * taps[i];
00462     }
00463   */
00464 }  
00465 
00466 #endif /*LV_HAVE_SSE4_1*/
00467 
00468 #endif /*INCLUDED_volk_32fc_x2_dot_prod_32fc_a16_H*/