GNU Radio 3.4.0 C++ API
|
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*/