| /* |
| * Copyright (C) 2008 The Android Open Source Project |
| * |
| * Licensed under the Apache License, Version 2.0 (the "License"); |
| * you may not use this file except in compliance with the License. |
| * You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, software |
| * distributed under the License is distributed on an "AS IS" BASIS, |
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| * See the License for the specific language governing permissions and |
| * limitations under the License. |
| */ |
| |
| /* ---- includes ----------------------------------------------------------- */ |
| |
| #include "b_BasicEm/Math.h" |
| #include "b_BasicEm/Functions.h" |
| |
| /* ---- related objects --------------------------------------------------- */ |
| |
| /* ---- typedefs ----------------------------------------------------------- */ |
| |
| /* ---- constants ---------------------------------------------------------- */ |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| /* ========================================================================= */ |
| /* */ |
| /* ---- \ghd{ external functions } ----------------------------------------- */ |
| /* */ |
| /* ========================================================================= */ |
| |
| #if defined( HW_SSE2 ) |
| extern int32 bbs_dotProduct_128SSE2( const int16* vec1A, const int16* vec2A, uint32 sizeA ); |
| extern int32 bbs_dotProduct_u128SSE2( const int16* vec1A, const int16* vec2A, uint32 sizeA ); |
| #endif |
| |
| #if defined( HW_FR71 ) |
| int32 bbs_dotProduct_fr71( const int16* vec1A, const int16* vec2A, uint32 sizeA ); |
| #endif |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| uint16 bbs_sqrt32( uint32 valA ) |
| { |
| uint32 rootL = 0; |
| uint32 expL = 0; |
| expL += ( ( valA >> ( expL + 0x10 ) ) != 0 ) << 4; |
| expL += ( ( valA >> ( expL + 0x08 ) ) != 0 ) << 3; |
| expL += ( ( valA >> ( expL + 0x04 ) ) != 0 ) << 2; |
| expL += ( ( valA >> ( expL + 0x02 ) ) != 0 ) << 1; |
| switch( expL >> 1 ) |
| { |
| case 15: rootL += ( ( rootL + 0x8000 ) * ( rootL + 0x8000 ) <= valA ) << 15; |
| case 14: rootL += ( ( rootL + 0x4000 ) * ( rootL + 0x4000 ) <= valA ) << 14; |
| case 13: rootL += ( ( rootL + 0x2000 ) * ( rootL + 0x2000 ) <= valA ) << 13; |
| case 12: rootL += ( ( rootL + 0x1000 ) * ( rootL + 0x1000 ) <= valA ) << 12; |
| case 11: rootL += ( ( rootL + 0x0800 ) * ( rootL + 0x0800 ) <= valA ) << 11; |
| case 10: rootL += ( ( rootL + 0x0400 ) * ( rootL + 0x0400 ) <= valA ) << 10; |
| case 9: rootL += ( ( rootL + 0x0200 ) * ( rootL + 0x0200 ) <= valA ) << 9; |
| case 8: rootL += ( ( rootL + 0x0100 ) * ( rootL + 0x0100 ) <= valA ) << 8; |
| case 7: rootL += ( ( rootL + 0x0080 ) * ( rootL + 0x0080 ) <= valA ) << 7; |
| case 6: rootL += ( ( rootL + 0x0040 ) * ( rootL + 0x0040 ) <= valA ) << 6; |
| case 5: rootL += ( ( rootL + 0x0020 ) * ( rootL + 0x0020 ) <= valA ) << 5; |
| case 4: rootL += ( ( rootL + 0x0010 ) * ( rootL + 0x0010 ) <= valA ) << 4; |
| case 3: rootL += ( ( rootL + 0x0008 ) * ( rootL + 0x0008 ) <= valA ) << 3; |
| case 2: rootL += ( ( rootL + 0x0004 ) * ( rootL + 0x0004 ) <= valA ) << 2; |
| case 1: rootL += ( ( rootL + 0x0002 ) * ( rootL + 0x0002 ) <= valA ) << 1; |
| case 0: rootL += ( ( rootL + 0x0001 ) * ( rootL + 0x0001 ) <= valA ) << 0; |
| } |
| |
| return ( uint16 )rootL; |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| uint8 bbs_sqrt16( uint16 valA ) |
| { |
| uint16 rootL = 0; |
| rootL += ( ( rootL + 0x80 ) * ( rootL + 0x80 ) <= valA ) << 7; |
| rootL += ( ( rootL + 0x40 ) * ( rootL + 0x40 ) <= valA ) << 6; |
| rootL += ( ( rootL + 0x20 ) * ( rootL + 0x20 ) <= valA ) << 5; |
| rootL += ( ( rootL + 0x10 ) * ( rootL + 0x10 ) <= valA ) << 4; |
| rootL += ( ( rootL + 0x08 ) * ( rootL + 0x08 ) <= valA ) << 3; |
| rootL += ( ( rootL + 0x04 ) * ( rootL + 0x04 ) <= valA ) << 2; |
| rootL += ( ( rootL + 0x02 ) * ( rootL + 0x02 ) <= valA ) << 1; |
| rootL += ( ( rootL + 0x01 ) * ( rootL + 0x01 ) <= valA ) << 0; |
| return ( uint8 )rootL; |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| /* table of sqrt and slope values */ |
| const uint32 bbs_fastSqrt32_tableG[] = |
| { |
| 268435456, 1016, 272596992, 1000, 276692992, 987, 280735744, 972, |
| 284717056, 959, 288645120, 946, 292519936, 933, 296341504, 922, |
| 300118016, 910, 303845376, 899, 307527680, 889, 311169024, 878, |
| 314765312, 869, 318324736, 858, 321839104, 850, 325320704, 840, |
| 328761344, 832, 332169216, 824, 335544320, 815, 338882560, 807, |
| 342188032, 799, 345460736, 792, 348704768, 785, 351920128, 777, |
| 355102720, 771, 358260736, 764, 361390080, 757, 364490752, 751, |
| 367566848, 745, 370618368, 739, 373645312, 732, 376643584, 727, |
| 379621376, 722, 382578688, 715, 385507328, 711, 388419584, 705, |
| 391307264, 700, 394174464, 695, 397021184, 689, 399843328, 686, |
| 402653184, 680, 405438464, 675, 408203264, 672, 410955776, 666, |
| 413683712, 663, 416399360, 658, 419094528, 653, 421769216, 650, |
| 424431616, 646, 427077632, 641, 429703168, 638, 432316416, 634, |
| 434913280, 630, 437493760, 627, 440061952, 622, 442609664, 620, |
| 445149184, 615, 447668224, 613, 450179072, 609, 452673536, 605, |
| 455151616, 602, 457617408, 600, 460075008, 595, 462512128, 593, |
| 464941056, 590, 467357696, 587, 469762048, 583, 472150016, 581, |
| 474529792, 578, 476897280, 575, 479252480, 572, 481595392, 569, |
| 483926016, 567, 486248448, 564, 488558592, 561, 490856448, 559, |
| 493146112, 556, 495423488, 553, 497688576, 552, 499949568, 548, |
| 502194176, 546, 504430592, 544, 506658816, 541, 508874752, 539, |
| 511082496, 537, 513282048, 534, 515469312, 533, 517652480, 529, |
| 519819264, 528, 521981952, 526, 524136448, 523, 526278656, 521, |
| 528412672, 519, 530538496, 517, 532656128, 515, 534765568, 514 |
| }; |
| |
| uint16 bbs_fastSqrt32( uint32 valA ) |
| { |
| uint32 expL = 0; |
| uint32 valL; |
| uint32 offsL; |
| uint32 indexL = 0; |
| |
| if( valA == 0 ) return 0; |
| |
| /* compute closest even size exponent of valA */ |
| expL += ( ( valA >> ( expL + 0x10 ) ) != 0 ) << 4; |
| expL += ( ( valA >> ( expL + 0x08 ) ) != 0 ) << 3; |
| expL += ( ( valA >> ( expL + 0x04 ) ) != 0 ) << 2; |
| expL += ( ( valA >> ( expL + 0x02 ) ) != 0 ) << 1; |
| |
| valL = ( ( valA << ( 30 - expL ) ) - 1073741824 ); /* ( 1 << 30 ) */ |
| offsL = ( ( valL & 0x01FFFFFF ) + ( 1 << 12 ) ) >> 13; |
| indexL = ( valL >> 24 ) & 0xFE; |
| |
| return ( bbs_fastSqrt32_tableG[ indexL ] + offsL * bbs_fastSqrt32_tableG[ indexL + 1 ] ) >> ( 28 - ( expL >> 1 ) ); |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| /* table of 1/sqrt (1.31) and negative slope (.15) values |
| referenced in b_GaborCueEm/focusDispAsm.s55, do not rename or remove ! */ |
| const uint32 bbs_invSqrt32_tableG[] = |
| { |
| 2147483648u, 1001, 2114682880, 956, 2083356672, 915, 2053373952, 877, |
| 2024636416, 840, 1997111296, 808, 1970634752, 776, 1945206784, 746, |
| 1920761856, 720, 1897168896, 693, 1874460672, 669, 1852538880, 646, |
| 1831370752, 625, 1810890752, 604, 1791098880, 584, 1771962368, 567, |
| 1753382912, 548, 1735426048, 533, 1717960704, 516, 1701052416, 502, |
| 1684602880, 487, 1668644864, 474, 1653112832, 461, 1638006784, 448, |
| 1623326720, 436, 1609039872, 426, 1595080704, 414, 1581514752, 404, |
| 1568276480, 394, 1555365888, 384, 1542782976, 375, 1530494976, 367, |
| 1518469120, 357, 1506770944, 350, 1495302144, 342, 1484095488, 334, |
| 1473150976, 327, 1462435840, 320, 1451950080, 313, 1441693696, 307, |
| 1431633920, 300, 1421803520, 294, 1412169728, 289, 1402699776, 282, |
| 1393459200, 277, 1384382464, 272, 1375469568, 266, 1366753280, 262, |
| 1358168064, 257, 1349746688, 251, 1341521920, 248, 1333395456, 243, |
| 1325432832, 238, 1317634048, 235, 1309933568, 230, 1302396928, 227, |
| 1294958592, 222, 1287684096, 219, 1280507904, 216, 1273430016, 211, |
| 1266515968, 209, 1259667456, 205, 1252950016, 202, 1246330880, 198, |
| 1239842816, 196, 1233420288, 192, 1227128832, 190, 1220902912, 187, |
| 1214775296, 184, 1208745984, 181, 1202814976, 179, 1196949504, 176, |
| 1191182336, 173, 1185513472, 171, 1179910144, 169, 1174372352, 166, |
| 1168932864, 164, 1163558912, 162, 1158250496, 160, 1153007616, 157, |
| 1147863040, 155, 1142784000, 154, 1137737728, 151, 1132789760, 149, |
| 1127907328, 148, 1123057664, 145, 1118306304, 144, 1113587712, 142, |
| 1108934656, 140, 1104347136, 138, 1099825152, 137, 1095335936, 135, |
| 1090912256, 134, 1086521344, 131, 1082228736, 131, 1077936128, 128 |
| }; |
| |
| uint32 bbs_invSqrt32( uint32 valA ) |
| { |
| |
| uint32 expL = 0; |
| uint32 valL; |
| uint32 offsL; |
| uint32 indexL = 0; |
| |
| if( valA == 0U ) return 0x80000000; |
| |
| /* compute closest even size exponent of valA */ |
| expL += ( ( valA >> ( expL + 0x10 ) ) != 0 ) << 4; |
| expL += ( ( valA >> ( expL + 0x08 ) ) != 0 ) << 3; |
| expL += ( ( valA >> ( expL + 0x04 ) ) != 0 ) << 2; |
| expL += ( ( valA >> ( expL + 0x02 ) ) != 0 ) << 1; |
| |
| valL = ( ( valA << ( 30 - expL ) ) - 1073741824 ); /* ( 1 << 30 ) */ |
| offsL = ( ( valL & 0x01FFFFFF ) + ( 1 << 9 ) ) >> 10; |
| indexL = ( valL >> 24 ) & 0xFE; |
| |
| return ( bbs_invSqrt32_tableG[ indexL ] - offsL * bbs_invSqrt32_tableG[ indexL + 1 ] ) >> ( expL >> 1 ); |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| /* table of 1/( x + 1 ) (2.30) and negative slope (.14) values |
| referenced in b_GaborCueEm/focusDispAsm.s55, do not rename or remove ! */ |
| const int32 bbs_inv32_tableG[] = |
| { |
| 1073741824, 1986, 1041203200, 1870, 1010565120, 1762, 981696512, 1664, |
| 954433536, 1575, 928628736, 1491, 904200192, 1415, 881016832, 1345, |
| 858980352, 1278, 838041600, 1218, 818085888, 1162, 799047680, 1108, |
| 780894208, 1059, 763543552, 1013, 746946560, 970, 731054080, 930, |
| 715816960, 891, 701218816, 856, 687194112, 823, 673710080, 791, |
| 660750336, 761, 648282112, 732, 636289024, 706, 624721920, 681, |
| 613564416, 657, 602800128, 635, 592396288, 613, 582352896, 592, |
| 572653568, 573, 563265536, 554, 554188800, 537, 545390592, 520, |
| }; |
| |
| int32 bbs_inv32( int32 valA ) |
| { |
| |
| uint32 expL = 0; |
| int32 signL = ( ( valA >> 30 ) & 0xFFFFFFFE ) + 1; |
| int32 valL = signL * valA; |
| int32 offsL; |
| uint32 indexL = 0; |
| |
| if( valL <= ( int32 ) 1 ) return 0x40000000 * signL; |
| |
| /* compute size exponent of valL */ |
| expL += ( ( valL >> ( expL + 0x10 ) ) != 0 ) << 4; |
| expL += ( ( valL >> ( expL + 0x08 ) ) != 0 ) << 3; |
| expL += ( ( valL >> ( expL + 0x04 ) ) != 0 ) << 2; |
| expL += ( ( valL >> ( expL + 0x02 ) ) != 0 ) << 1; |
| expL += ( ( valL >> ( expL + 0x01 ) ) != 0 ); |
| |
| valL = ( ( valL << ( 30 - expL ) ) - 1073741824 ); /*( 1U << 30 )*/ |
| offsL = ( ( valL & 0x01FFFFFF ) + ( 1 << 10 ) ) >> 11; |
| indexL = ( valL >> 24 ) & 0xFE; |
| |
| return signL * ( ( ( ( bbs_inv32_tableG[ indexL ] - offsL * bbs_inv32_tableG[ indexL + 1 ] ) >> ( expL - 1 ) ) + 1 ) >> 1 ); |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| uint32 bbs_intLog2( uint32 valA ) |
| { |
| uint32 expL = 0; |
| expL += 0x10 * ( ( valA >> ( expL + 0x10 ) ) != 0 ); |
| expL += 0x08 * ( ( valA >> ( expL + 0x08 ) ) != 0 ); |
| expL += 0x04 * ( ( valA >> ( expL + 0x04 ) ) != 0 ); |
| expL += 0x02 * ( ( valA >> ( expL + 0x02 ) ) != 0 ); |
| expL += 0x01 * ( ( valA >> ( expL + 0x01 ) ) != 0 ); |
| return expL; |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| const uint32 bbs_pow2M1_tableG[] = |
| { |
| 0, 713, 46769127, 721, 94047537, 729, 141840775, 737, |
| 190154447, 745, 238994221, 753, 288365825, 761, 338275050, 769, |
| 388727751, 778, 439729846, 786, 491287318, 795, 543406214, 803, |
| 596092647, 812, 649352798, 821, 703192913, 830, 757619310, 839, |
| 812638371, 848, 868256550, 857, 924480371, 867, 981316430, 876, |
| 1038771393, 886, 1096851999, 895, 1155565062, 905, 1214917468, 915, |
| 1274916179, 925, 1335568233, 935, 1396880745, 945, 1458860907, 956, |
| 1521515988, 966, 1584853338, 976, 1648880387, 987, 1713604645, 998, |
| 1779033703, 1009, 1845175238, 1020, 1912037006, 1031, 1979626852, 1042, |
| 2047952702, 1053, 2117022572, 1065, 2186844564u, 1077, 2257426868u, 1088, |
| 2328777762u, 1100, 2400905617u, 1112, 2473818892u, 1124, 2547526141u, 1136, |
| 2622036010u, 1149, 2697357237u, 1161, 2773498659u, 1174, 2850469207u, 1187, |
| 2928277909u, 1200, 3006933892u, 1213, 3086446383u, 1226, 3166824708u, 1239, |
| 3248078296u, 1253, 3330216677u, 1266, 3413249486u, 1280, 3497186464u, 1294, |
| 3582037455u, 1308, 3667812413u, 1323, 3754521399u, 1337, 3842174584u, 1352, |
| 3930782250u, 1366, 4020354790u, 1381, 4110902710u, 1396, 4202436633u, 1411 |
| }; |
| |
| uint32 bbs_pow2M1( uint32 valA ) |
| { |
| uint32 offsL = ( valA & 0x03FFFFFF ) >> 10; |
| uint16 indexL = ( ( valA & 0xFC000000 ) >> 26 ) << 1; |
| return bbs_pow2M1_tableG[ indexL ] + offsL * bbs_pow2M1_tableG[ indexL + 1 ]; |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| uint32 bbs_pow2( int32 valA ) |
| { |
| int32 shiftL = 16 - ( valA >> 27 ); |
| uint32 offsL = ( uint32 )( valA << 5 ); |
| if( shiftL == 32 ) return 1; |
| return ( 1 << ( 32 - shiftL ) ) + ( bbs_pow2M1( offsL ) >> shiftL ); |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| uint32 bbs_exp( int32 valA ) |
| { |
| int32 adjustedL; |
| int32 shiftL; |
| int32 offsL; |
| |
| /* check boundaries to avoid overflow */ |
| if( valA < -1488522236 ) |
| { |
| return 0; |
| } |
| else if( valA > 1488522236 ) |
| { |
| return 0xFFFFFFFF; |
| } |
| |
| /* multily valA with 1/ln(2) in order to use function 2^x instead of e^x */ |
| adjustedL = ( valA >> 16 ) * 94548 + ( ( ( ( ( uint32 )valA ) & 0x0FFFF ) * 47274 ) >> 15 ); |
| |
| shiftL = 16 - ( adjustedL >> 27 ); |
| if( shiftL == 32 ) return 1; |
| offsL = ( uint32 )( adjustedL << 5 ); |
| return ( ( int32 ) 1 << ( 32 - shiftL ) ) + ( bbs_pow2M1( offsL ) >> shiftL ); |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| int16 bbs_satS16( int32 valA ) |
| { |
| if( valA > 32767 ) return 32767; |
| if( valA < -32768 ) return -32768; |
| return valA; |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| #if defined( HW_i586 ) || defined( HW_i686 ) |
| |
| /* Windows section */ |
| #if defined( WIN32 ) && !defined( WIN64 ) |
| |
| /* disable warning "no return value"*/ |
| #pragma warning( disable : 4035 ) |
| |
| /** |
| * computes a fast dot product using intel MMX, sizeA must be multiple of 16 and >0 |
| */ |
| int32 bbs_dotProduct_intelMMX16( const int16* vec1A, const int16* vec2A, uint32 sizeA ) |
| { |
| __asm |
| { |
| push esi |
| push edi |
| |
| mov eax, vec1A |
| mov ebx, vec2A |
| |
| mov ecx, sizeA |
| |
| pxor mm4, mm4 |
| pxor mm6, mm6 |
| |
| pxor mm7, mm7 |
| shr ecx, 4 |
| |
| inner_loop_start: |
| movq mm0, 0[eax] |
| paddd mm7, mm4 |
| |
| movq mm1, 0[ebx] |
| paddd mm7, mm6 |
| |
| movq mm2, 8[eax] |
| pmaddwd mm0, mm1 |
| |
| movq mm3, 8[ebx] |
| |
| movq mm4, 16[eax] |
| pmaddwd mm2, mm3 |
| |
| movq mm5, 16[ebx] |
| paddd mm7, mm0 |
| |
| movq mm6, 24[eax] |
| pmaddwd mm4, mm5 |
| |
| pmaddwd mm6, 24[ebx] |
| paddd mm7, mm2 |
| |
| add eax, 32 |
| add ebx, 32 |
| |
| dec ecx |
| jnz inner_loop_start |
| |
| paddd mm7, mm4 |
| |
| paddd mm7, mm6 |
| |
| movq mm0, mm7 |
| |
| psrlq mm0, 32 |
| |
| paddd mm7, mm0 |
| |
| movd eax, mm7 |
| |
| emms |
| pop edi |
| pop esi |
| } |
| } |
| |
| #pragma warning( default : 4035 ) |
| |
| /* gcc compiler section */ |
| #elif defined( epl_LINUX ) || defined( CYGWIN ) |
| |
| /** |
| * computes a fast dot product using intel MMX, sizeA must be multiple of 16 and >0 |
| */ |
| int32 bbs_dotProduct_intelMMX16( const int16* vec1A, const int16* vec2A, uint32 sizeA ) |
| { |
| int32 resultL; |
| |
| __asm__ __volatile__( |
| |
| "movl %1,%%eax\n\t" |
| "movl %2,%%ebx\n\t" |
| |
| "movl %3,%%ecx\n\t" |
| |
| "pxor %%mm4,%%mm4\n\t" |
| "pxor %%mm6,%%mm6\n\t" |
| |
| "pxor %%mm7, %%mm7\n\t" |
| "shrl $4, %%ecx\n\t" |
| |
| "\n1:\t" |
| "movq 0(%%eax),%%mm0\n\t" |
| "paddd %%mm4,%%mm7\n\t" |
| |
| "movq 0( %%ebx ),%%mm1\n\t" |
| "paddd %%mm6,%%mm7\n\t" |
| |
| "movq 8( %%eax ),%%mm2\n\t" |
| "pmaddwd %%mm1,%%mm0\n\t" |
| |
| "movq 8( %%ebx ),%%mm3\n\t" |
| |
| "movq 16( %%eax ),%%mm4\n\t" |
| "pmaddwd %%mm3,%%mm2\n\t" |
| |
| "movq 16( %%ebx ),%%mm5\n\t" |
| "paddd %%mm0,%%mm7\n\t" |
| |
| "movq 24( %%eax ),%%mm6\n\t" |
| "pmaddwd %%mm5,%%mm4\n\t" |
| |
| "pmaddwd 24( %%ebx ),%%mm6\n\t" |
| "paddd %%mm2,%%mm7\n\t" |
| |
| "addl $32,%%eax\n\t" |
| "addl $32,%%ebx\n\t" |
| |
| "decl %%ecx\n\t" |
| "jnz 1b\n\t" |
| |
| "paddd %%mm4,%%mm7\n\t" |
| "paddd %%mm6,%%mm7\n\t" |
| |
| "movq %%mm7,%%mm0\n\t" |
| |
| "psrlq $32,%%mm0\n\t" |
| |
| "paddd %%mm0,%%mm7\n\t" |
| |
| "movd %%mm7,%0\n\t" |
| |
| "emms\n\t" |
| |
| : "=&g" ( resultL ) |
| : "g" ( vec1A ), "g" ( vec2A ), "g" ( sizeA ) |
| : "si", "di", "ax", "bx", "cx", "st", "memory" ); |
| |
| return resultL; |
| } |
| |
| #endif /* epl_LINUX, CYGWIN */ |
| |
| #endif /* HW_i586 || HW_i686 */ |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| #ifdef HW_TMS320C6x |
| /** |
| * Calls fast assembler version of dotproduct for DSP. |
| * dotProduct_C62x is implemented in file dotprod.asm and expects input vectors |
| * of even length. |
| */ |
| int32 bbs_dotProduct_dsp( const int16* vec1A, const int16* vec2A, uint32 sizeA ) |
| { |
| if( sizeA & 1 ) |
| { |
| int32 resultL; |
| resultL = dotProduct_C62x( vec1A, vec2A, sizeA - 1 ); |
| return resultL + ( int32 ) *( vec1A + sizeA - 1 ) * *( vec2A + sizeA - 1 ); |
| } |
| else |
| { |
| return dotProduct_C62x( vec1A, vec2A, sizeA ); |
| } |
| } |
| #endif /* HW_TMS320C6x */ |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| /* 16 dot product for the PS2/EE processor */ |
| /* input vectors MUST be 128 bit aligned ! */ |
| |
| #if defined( epl_LINUX ) && defined( HW_EE ) |
| |
| int32 bbs_dotProduct_EE( const int16* vec1A, const int16* vec2A, uint32 sizeA ) |
| { |
| int32 resultL = 0, |
| iL = sizeA >> 3, |
| jL = sizeA - ( iL << 3 ); |
| |
| if( iL > 0 ) |
| { |
| /* multiply-add elements of input vectors in sets of 8 */ |
| int32 accL[ 4 ], t1L, t2L, t3L; |
| asm volatile ( |
| "pxor %4, %2, %2\n\t" /* reset 8 accumulators (LO and HI register) to 0 */ |
| "pmtlo %4\n\t" |
| "pmthi %4\n\t" |
| |
| "\n__begin_loop:\t" |
| |
| "lq %2,0(%0)\n\t" /* load 8 pairs of int16 */ |
| "lq %3,0(%1)\n\t" |
| |
| "addi %0,%0,16\n\t" /* vec1L += 16 */ |
| "addi %1,%1,16\n\t" /* vec2L += 16 */ |
| "addi %7,%7,-1\n\t" /* iL-- */ |
| |
| "pmaddh %4,%2,%3\n\t" /* parallel multiply-add of 8 pairs of int16 */ |
| |
| "bgtzl %7,__begin_loop\n\t" /* if iL > 0 goto _begin_loop */ |
| |
| "pmflo %2\n\t" /* parallel add 8 accumulators , store remaining 4 accumulators in tmpL */ |
| "pmfhi %3\n\t" |
| "paddw %4,%2,%3\n\t" |
| "sq %4,0(%8)\n\t" |
| : "=r" ( vec1A ), "=r" ( vec2A ), "=r" ( t1L ), "=r" ( t2L ), "=r" ( t3L ) |
| : "0" ( vec1A ), "1" ( vec2A ), "r" ( iL ), "r" ( accL ) |
| : "memory" ); |
| |
| /* add 4 parallel accumulators */ |
| resultL += accL[ 0 ] + accL[ 1 ] + accL[ 2 ] + accL[ 3 ]; |
| } |
| |
| /* multiply-add remaining elements of input vectors */ |
| for( ; jL--; ) resultL += ( int32 ) *vec1A++ * *vec2A++; |
| |
| return resultL; |
| } |
| |
| #endif |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| #if defined( HW_ARMv5TE ) |
| |
| /* fast 16 dot product for ARM9E cores (DSP extensions). |
| * input vectors must be 32 bit aligned |
| */ |
| int32 bbs_dotProduct_arm9e( const int16* vec1A, const int16* vec2A, uint32 sizeA ) |
| { |
| int32 accuL = 0; |
| |
| int32* v1PtrL = ( int32* )vec1A; |
| int32* v2PtrL = ( int32* )vec2A; |
| |
| for( ; sizeA >= 4; sizeA -= 4 ) |
| { |
| __asm { |
| smlabb accuL, *v1PtrL, *v2PtrL, accuL; |
| smlatt accuL, *v1PtrL, *v2PtrL, accuL; |
| } |
| v1PtrL++; v2PtrL++; |
| __asm { |
| smlabb accuL, *v1PtrL, *v2PtrL, accuL; |
| smlatt accuL, *v1PtrL, *v2PtrL, accuL; |
| } |
| v1PtrL++; v2PtrL++; |
| } |
| |
| vec1A = ( int16* )v1PtrL; |
| vec2A = ( int16* )v2PtrL; |
| |
| /* multiply-add remaining elements of input vectors */ |
| for( ; sizeA > 0; sizeA-- ) accuL += ( int32 )*vec1A++ * *vec2A++; |
| |
| return accuL; |
| } |
| |
| #endif |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| /** |
| * Computes a fast dot product using standard C |
| */ |
| int32 bbs_dotProduct_stdc( const int16* vec1A, const int16* vec2A, uint32 sizeA ) |
| { |
| int32 accuL = 0; |
| |
| for( ; sizeA >= 8; sizeA -= 8 ) |
| { |
| accuL += ( int32 ) *vec1A * *vec2A; |
| accuL += ( int32 ) *( vec1A + 1 ) * *( vec2A + 1 ); |
| accuL += ( int32 ) *( vec1A + 2 ) * *( vec2A + 2 ); |
| accuL += ( int32 ) *( vec1A + 3 ) * *( vec2A + 3 ); |
| |
| accuL += ( int32 ) *( vec1A + 4 ) * *( vec2A + 4 ); |
| accuL += ( int32 ) *( vec1A + 5 ) * *( vec2A + 5 ); |
| accuL += ( int32 ) *( vec1A + 6 ) * *( vec2A + 6 ); |
| accuL += ( int32 ) *( vec1A + 7 ) * *( vec2A + 7 ); |
| |
| vec1A += 8; |
| vec2A += 8; |
| } |
| |
| for( ; sizeA; sizeA-- ) accuL += ( int32 ) *vec1A++ * *vec2A++; |
| |
| return accuL; |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| int32 bbs_dotProductInt16( const int16* vec1A, const int16* vec2A, uint32 sizeA ) |
| { |
| /* PC */ |
| #if ( defined( HW_i586 ) || defined( HW_i686 ) ) |
| |
| #if defined( HW_SSE2 ) |
| uint32 size16L = sizeA & 0xfffffff0; |
| if( size16L ) |
| { |
| if( ( (uint32)vec1A & 0xF ) == 0 && ( (uint32)vec2A & 0xF ) == 0 ) |
| { |
| return bbs_dotProduct_128SSE2( vec1A, vec2A, sizeA ); |
| } |
| else |
| { |
| return bbs_dotProduct_u128SSE2( vec1A, vec2A, sizeA ); |
| } |
| } |
| #elif !defined( WIN64 ) |
| /* MMX version (not supported by 64-bit compiler) */ |
| uint32 size16L = sizeA & 0xfffffff0; |
| if( size16L ) |
| { |
| if( sizeA == size16L ) |
| { |
| return bbs_dotProduct_intelMMX16( vec1A, vec2A, size16L ); |
| } |
| return bbs_dotProduct_intelMMX16( vec1A, vec2A, size16L ) |
| + bbs_dotProduct_stdc( vec1A + size16L, vec2A + size16L, sizeA - size16L ); |
| } /* if( size16L ) */ |
| #endif |
| |
| return bbs_dotProduct_stdc( vec1A, vec2A, sizeA ); |
| |
| /* Playstation 2 */ |
| #elif defined( HW_EE ) && defined( epl_LINUX ) |
| |
| if( ( (uint32)vec1A & 0xF ) == 0 && ( (uint32)vec2A & 0xF ) == 0 ) |
| { |
| return bbs_dotProduct_EE( vec1A, vec2A, sizeA ); |
| } |
| return bbs_dotProduct_stdc( vec1A, vec2A, sizeA ); |
| |
| /* ARM9E */ |
| #elif defined( HW_ARMv5TE ) |
| |
| return bbs_dotProduct_arm9e( vec1A, vec2A, sizeA ); |
| |
| /* TI C6000 */ |
| #elif defined( HW_TMS320C6x ) |
| |
| return bbs_dotProduct_dsp( vec1A, vec2A, sizeA ); |
| |
| #elif defined( HW_FR71 ) |
| |
| uint32 size16L = sizeA & 0xfffffff0; |
| if( size16L ) |
| { |
| if( sizeA == size16L ) |
| { |
| return bbs_dotProduct_fr71( vec1A, vec2A, size16L ); |
| } |
| return bbs_dotProduct_fr71( vec1A, vec2A, size16L ) |
| + bbs_dotProduct_stdc( vec1A + size16L, vec2A + size16L, sizeA - size16L ); |
| } |
| |
| return bbs_dotProduct_stdc( vec1A, vec2A, sizeA ); |
| |
| #endif |
| |
| return bbs_dotProduct_stdc( vec1A, vec2A, sizeA ); |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| /* table of fermi and slope values (result: 2.30; offset: .12) |
| referenced in b_NeuralNetEm/FastMlpNet.c, not not rename or remove */ |
| const uint32 bbs_fermi_tableG[] = |
| { |
| 45056, 8, 77824, 13, 131072, 21, 217088, 34, |
| 356352, 57, 589824, 94, 974848, 155, 1609728, 255, |
| 2654208, 418, 4366336, 688, 7184384, 1126, 11796480, 1834, |
| 19308544, 2970, 31473664, 4748, 50921472, 7453, 81448960, 11363, |
| 127991808, 16573, 195874816, 22680, 288772096, 28469, 405381120, 32102, |
| 536870912, 32101, 668356608, 28469, 784965632, 22680, 877862912, 16573, |
| 945745920, 11363, 992288768, 7453, 1022816256, 4748, 1042264064, 2970, |
| 1054429184, 1834, 1061941248, 1126, 1066553344, 688, 1069371392, 418, |
| 1071083520, 255, 1072128000, 155, 1072762880, 94, 1073147904, 57, |
| 1073381376, 34, 1073520640, 21, 1073606656, 13, 1073659904, 8, |
| }; |
| |
| int32 bbs_fermi( int32 valA ) |
| { |
| uint32 indexL = ( ( valA >> 15 ) + 20 ) << 1; |
| uint32 offsL = ( ( valA & 0x00007FFF ) + 4 ) >> 3; |
| if( valA < -655360 ) return 1; |
| if( valA >= 655360 ) return 1073741824 - 1; /* ( 1 << 30 ) */ |
| return ( bbs_fermi_tableG[ indexL ] + offsL * bbs_fermi_tableG[ indexL + 1 ] ); |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| void bbs_uint32ReduceToNBits( uint32* argPtrA, int32* bbpPtrA, uint32 nBitsA ) |
| { |
| int32 posHighestBitL = bbs_intLog2( *argPtrA ) + 1; |
| int32 shiftL = posHighestBitL - nBitsA; |
| if( shiftL > 0 ) |
| { |
| ( *argPtrA ) >>= shiftL; |
| ( *bbpPtrA ) -= shiftL; |
| } |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| void bbs_int32ReduceToNBits( int32* argPtrA, int32* bbpPtrA, uint32 nBitsA ) |
| { |
| int32 posHighestBitL = bbs_intLog2( bbs_abs( *argPtrA ) ) + 1; |
| int32 shiftL = posHighestBitL - nBitsA; |
| if( shiftL > 0 ) |
| { |
| ( *argPtrA ) >>= shiftL; |
| ( *bbpPtrA ) -= shiftL; |
| } |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| uint32 bbs_convertU32( uint32 srcA, int32 srcBbpA, int32 dstBbpA ) |
| { |
| if( dstBbpA >= srcBbpA ) |
| { |
| uint32 shiftL = dstBbpA - srcBbpA; |
| if( srcA > ( ( uint32 )0xFFFFFFFF >> shiftL ) ) |
| { |
| /* overflow */ |
| return ( uint32 )0xFFFFFFFF; |
| } |
| else |
| { |
| return srcA << shiftL; |
| } |
| } |
| else |
| { |
| uint32 shiftL = srcBbpA - dstBbpA; |
| uint32 addL = 1L << ( shiftL - 1 ); |
| if( srcA + addL < addL ) |
| { |
| /* rounding would cause overflow */ |
| return srcA >> shiftL; |
| } |
| else |
| { |
| return ( srcA + addL ) >> shiftL; |
| } |
| } |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| int32 bbs_convertS32( int32 srcA, int32 srcBbpA, int32 dstBbpA ) |
| { |
| if( dstBbpA >= srcBbpA ) |
| { |
| uint32 shiftL = ( uint32 )( dstBbpA - srcBbpA ); |
| if( srcA > ( ( int32 )0x7FFFFFFF >> shiftL ) ) |
| { |
| /* overflow */ |
| return ( uint32 )0x7FFFFFFF; |
| } |
| else if( srcA < ( ( int32 )0x80000000 >> shiftL ) ) |
| { |
| /* underflow */ |
| return ( int32 )0x80000000; |
| } |
| else |
| { |
| return srcA << shiftL; |
| } |
| } |
| else |
| { |
| uint32 shiftL = ( uint32 )( srcBbpA - dstBbpA ); |
| int32 addL = 1L << ( shiftL - 1 ); |
| if( srcA + addL < addL ) |
| { |
| /* rounding would cause overflow */ |
| return srcA >> shiftL; |
| } |
| else |
| { |
| return ( srcA + addL ) >> shiftL; |
| } |
| } |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| int32 bbs_vecPowerFlt16( const int16 *xA, int16 nxA ) |
| { |
| /* #if defined( HW_TMS320C5x ) |
| uint32 rL; |
| power( ( int16* ) xA, ( int32* ) &rL, nxA ); // does not work properly in DSPLib version 2.20.02 |
| return ( rL >> 1 ); |
| #else*/ |
| /* needs to be optimized */ |
| int32 rL = 0; |
| for( ; nxA--; ) |
| { |
| rL += ( int32 ) *xA * *xA; |
| xA++; |
| } |
| return rL; |
| /* #endif */ |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| void bbs_mulU32( uint32 v1A, uint32 v2A, uint32* manPtrA, int32* expPtrA ) |
| { |
| uint32 log1L = bbs_intLog2( v1A ); |
| uint32 log2L = bbs_intLog2( v2A ); |
| |
| if( log1L + log2L < 32 ) |
| { |
| *manPtrA = v1A * v2A; |
| *expPtrA = 0; |
| } |
| else |
| { |
| uint32 v1L = v1A; |
| uint32 v2L = v2A; |
| uint32 exp1L = 0; |
| uint32 exp2L = 0; |
| if( log1L > 15 && log2L > 15 ) |
| { |
| exp1L = log1L - 15; |
| exp2L = log2L - 15; |
| v1L = ( ( v1L >> ( exp1L - 1 ) ) + 1 ) >> 1; |
| v2L = ( ( v2L >> ( exp2L - 1 ) ) + 1 ) >> 1; |
| } |
| else if( log1L > 15 ) |
| { |
| exp1L = log1L + log2L - 31; |
| v1L = ( ( v1L >> ( exp1L - 1 ) ) + 1 ) >> 1; |
| } |
| else |
| { |
| exp2L = log1L + log2L - 31; |
| v2L = ( ( v2L >> ( exp2L - 1 ) ) + 1 ) >> 1; |
| } |
| |
| *manPtrA = v1L * v2L; |
| *expPtrA = exp1L + exp2L; |
| } |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| void bbs_mulS32( int32 v1A, int32 v2A, int32* manPtrA, int32* expPtrA ) |
| { |
| uint32 log1L = bbs_intLog2( v1A > 0 ? v1A : -v1A ); |
| uint32 log2L = bbs_intLog2( v2A > 0 ? v2A : -v2A ); |
| |
| if( log1L + log2L < 30 ) |
| { |
| *manPtrA = v1A * v2A; |
| *expPtrA = 0; |
| } |
| else |
| { |
| int32 v1L = v1A; |
| int32 v2L = v2A; |
| int32 exp1L = 0; |
| int32 exp2L = 0; |
| if( log1L > 14 && log2L > 14 ) |
| { |
| exp1L = log1L - 14; |
| exp2L = log2L - 14; |
| v1L = ( ( v1L >> ( exp1L - 1 ) ) + 1 ) >> 1; |
| v2L = ( ( v2L >> ( exp2L - 1 ) ) + 1 ) >> 1; |
| } |
| else if( log1L > 14 ) |
| { |
| exp1L = log1L + log2L - 29; |
| v1L = ( ( v1L >> ( exp1L - 1 ) ) + 1 ) >> 1; |
| } |
| else |
| { |
| exp2L = log1L + log2L - 29; |
| v2L = ( ( v2L >> ( exp2L - 1 ) ) + 1 ) >> 1; |
| } |
| |
| *manPtrA = v1L * v2L; |
| *expPtrA = exp1L + exp2L; |
| } |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| void bbs_vecSqrNorm32( const int32* vecA, uint32 sizeA, uint32* manPtrA, uint32* expPtrA ) |
| { |
| uint32 sumL = 0; |
| int32 sumExpL = 0; |
| |
| uint32 iL; |
| for( iL = 0; iL < sizeA; iL++ ) |
| { |
| int32 vL = vecA[ iL ]; |
| int32 logL = bbs_intLog2( vL > 0 ? vL : -vL ); |
| int32 expL = ( logL > 14 ) ? logL - 14 : 0; |
| uint32 prdL; |
| |
| if( expL >= 1 ) |
| { |
| vL = ( ( vL >> ( expL - 1 ) ) + 1 ) >> 1; |
| } |
| else |
| { |
| vL = vL >> expL; |
| } |
| |
| prdL = vL * vL; |
| expL <<= 1; /* now exponent of product */ |
| |
| if( sumExpL > expL ) |
| { |
| uint32 shrL = sumExpL - expL; |
| prdL = ( ( prdL >> ( shrL - 1 ) ) + 1 ) >> 1; |
| } |
| else if( expL > sumExpL ) |
| { |
| uint32 shrL = expL - sumExpL; |
| sumL = ( ( sumL >> ( shrL - 1 ) ) + 1 ) >> 1; |
| sumExpL += shrL; |
| } |
| |
| sumL += prdL; |
| |
| if( sumL > 0x80000000 ) |
| { |
| sumL = ( sumL + 1 ) >> 1; |
| sumExpL++; |
| } |
| } |
| |
| /* make exponent even */ |
| if( ( sumExpL & 1 ) != 0 ) |
| { |
| sumL = ( sumL + 1 ) >> 1; |
| sumExpL++; |
| } |
| |
| if( manPtrA != NULL ) *manPtrA = sumL; |
| if( expPtrA != NULL ) *expPtrA = sumExpL; |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| void bbs_vecSqrNorm16( const int16* vecA, uint32 sizeA, uint32* manPtrA, uint32* expPtrA ) |
| { |
| uint32 sumL = 0; |
| int32 sumExpL = 0; |
| |
| uint32 iL; |
| for( iL = 0; iL < sizeA; iL++ ) |
| { |
| int32 vL = vecA[ iL ]; |
| uint32 prdL = vL * vL; |
| |
| if( sumExpL > 0 ) |
| { |
| uint32 shrL = sumExpL; |
| prdL = ( ( prdL >> ( shrL - 1 ) ) + 1 ) >> 1; |
| } |
| |
| sumL += prdL; |
| |
| if( sumL > 0x80000000 ) |
| { |
| sumL = ( sumL + 1 ) >> 1; |
| sumExpL++; |
| } |
| } |
| |
| /* make exponent even */ |
| if( ( sumExpL & 1 ) != 0 ) |
| { |
| sumL = ( sumL + 1 ) >> 1; |
| sumExpL++; |
| } |
| |
| if( manPtrA != NULL ) *manPtrA = sumL; |
| if( expPtrA != NULL ) *expPtrA = sumExpL; |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| uint32 bbs_vecNorm16( const int16* vecA, uint32 sizeA ) |
| { |
| uint32 manL; |
| uint32 expL; |
| bbs_vecSqrNorm16( vecA, sizeA, &manL, &expL ); |
| manL = bbs_sqrt32( manL ); |
| return manL << ( expL >> 1 ); |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| void bbs_matMultiplyFlt16( const int16 *x1A, int16 row1A, int16 col1A, const int16 *x2A, int16 col2A, int16 *rA ) |
| { |
| #if defined( HW_TMS320C5x ) |
| /* operands need to be in internal memory for mmul*/ |
| if( x1A > ( int16* ) bbs_C5X_INTERNAL_MEMORY_SIZE || |
| x2A > ( int16* ) bbs_C5X_INTERNAL_MEMORY_SIZE ) |
| { |
| int16 iL,jL,kL; |
| int16 *ptr1L, *ptr2L; |
| int32 sumL; |
| |
| for( iL = 0; iL < row1A; iL++ ) |
| { |
| for( jL = 0; jL < col2A; jL++ ) |
| { |
| ptr1L = ( int16* ) x1A + iL * col1A; |
| ptr2L = ( int16* ) x2A + jL; |
| sumL = 0; |
| for( kL = 0; kL < col1A; kL++ ) |
| { |
| sumL += ( ( int32 ) *ptr1L++ * *ptr2L ); |
| ptr2L += col2A; |
| } |
| *rA++ = ( sumL + ( 1 << 14 ) ) >> 15; /* round result to 1.15 */ |
| } |
| } |
| } |
| else mmul( ( int16* ) x1A, row1A, col1A, ( int16* ) x2A, col1A, col2A, rA ); |
| |
| #elif defined( HW_ARMv4 ) || defined( HW_ARMv5TE ) |
| |
| int32 iL, jL, kL; |
| int16 *ptr1L, *ptr2L; |
| int32 sumL; |
| for( iL = 0; iL < row1A; iL++ ) |
| { |
| for( jL = 0; jL < col2A; jL++ ) |
| { |
| ptr1L = ( int16* ) x1A + iL * col1A; |
| ptr2L = ( int16* ) x2A + jL; |
| sumL = 0; |
| for( kL = col1A; kL >= 4; kL -= 4 ) |
| { |
| sumL += ( ( int32 ) *ptr1L++ * *ptr2L ); |
| sumL += ( ( int32 ) *ptr1L++ * *( ptr2L += col2A ) ); |
| sumL += ( ( int32 ) *ptr1L++ * *( ptr2L += col2A ) ); |
| sumL += ( ( int32 ) *ptr1L++ * *( ptr2L += col2A ) ); |
| ptr2L += col2A; |
| } |
| for( ; kL > 0; kL-- ) |
| { |
| sumL += ( ( int32 ) *ptr1L++ * *ptr2L ); |
| ptr2L += col2A; |
| } |
| *rA++ = ( sumL + ( 1 << 14 ) ) >> 15; /* round result to 1.15 */ |
| } |
| } |
| #else |
| /* needs to be optimized */ |
| int16 iL,jL,kL; |
| int16 *ptr1L, *ptr2L; |
| int32 sumL; |
| |
| for( iL = 0; iL < row1A; iL++ ) |
| { |
| for( jL = 0; jL < col2A; jL++ ) |
| { |
| ptr1L = ( int16* ) x1A + iL * col1A; |
| ptr2L = ( int16* ) x2A + jL; |
| sumL = 0; |
| for( kL = 0; kL < col1A; kL++ ) |
| { |
| sumL += ( ( int32 ) *ptr1L++ * *ptr2L ); |
| ptr2L += col2A; |
| } |
| *rA++ = ( sumL + ( 1 << 14 ) ) >> 15; /* round result to 1.15 */ |
| } |
| } |
| #endif |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| void bbs_matMultiplyTranspFlt16( const int16 *x1A, int16 row1A, int16 col1A, |
| const int16 *x2A, int16 col2A, int16 *rA ) |
| { |
| const int16* ptr1L = x1A; |
| |
| int32 iL; |
| for( iL = row1A; iL > 0 ; iL-- ) |
| { |
| int32 jL; |
| const int16* ptr2L = x2A; |
| for( jL = col2A; jL > 0 ; jL-- ) |
| { |
| int32 kL; |
| int32 sumL = 0; |
| for( kL = col1A >> 2; kL > 0; kL-- ) |
| { |
| sumL += ( ( int32 ) *ptr1L++ * *ptr2L++ ); |
| sumL += ( ( int32 ) *ptr1L++ * *ptr2L++ ); |
| sumL += ( ( int32 ) *ptr1L++ * *ptr2L++ ); |
| sumL += ( ( int32 ) *ptr1L++ * *ptr2L++ ); |
| } |
| for( kL = col1A & 3; kL > 0; kL-- ) |
| { |
| sumL += ( ( int32 ) *ptr1L++ * *ptr2L++ ); |
| } |
| *rA++ = ( sumL + ( 1 << 14 ) ) >> 15; /* round result to 1.15 */ |
| ptr1L -= col1A; |
| } |
| ptr1L += col1A; |
| } |
| } |
| |
| /* ------------------------------------------------------------------------- */ |
| |
| |
| #ifndef mtrans |
| uint16 bbs_matTrans( int16 *xA, int16 rowA, int16 colA, int16 *rA ) |
| { |
| /* needs to be optimized */ |
| int16 iL; |
| for( iL = colA; iL--; ) |
| { |
| int16* sL = xA++; |
| int16 jL; |
| for( jL = rowA; jL--; ) |
| { |
| *rA++ = *sL; |
| sL += colA; |
| } |
| } |
| return 0; |
| } |
| #endif |
| |
| /* ------------------------------------------------------------------------- */ |
| #ifndef atan2_16 |
| int16 bbs_atan2( int16 nomA, int16 denomA ) |
| { |
| int16 phL, argL; |
| |
| if( nomA == denomA ) return 8192; |
| argL = ( ( int32 ) nomA << 15 ) / denomA; |
| |
| /* 0.318253*2 x 20857 .15 |
| +0.003314*2 x^2 217 .15 |
| -0.130908*2 x^3 -8580 .15 |
| +0.068542*2 x^4 4491 .15 |
| -0.009159*2 x^5 -600 .15 */ |
| |
| phL = -600; |
| phL = ( ( ( int32 ) phL * argL ) >> 15 ) + 4481; |
| phL = ( ( ( int32 ) phL * argL ) >> 15 ) - 8580; |
| phL = ( ( ( int32 ) phL * argL ) >> 15 ) + 217; |
| phL = ( ( ( int32 ) phL * argL ) >> 15 ) + 20857; |
| phL = ( ( int32 ) phL * argL ) >> 15; |
| |
| return phL >> 1; /* /2 */ |
| } |
| |
| /* needs to be optimized */ |
| uint16 bbs_vecPhase( int16 *reA, int16 *imA, int16 *phaseA, uint16 sizeA ) |
| { |
| for( ; sizeA--; ) |
| { |
| int16 reL = *reA++; |
| int16 imL = *imA++; |
| int16 phL = 0; |
| |
| if( reL < 0 ) |
| { |
| reL = -reL; |
| if( imL < 0 ) |
| { |
| imL = -imL; |
| if( reL > imL ) |
| { |
| phL = -32768 + bbs_atan2( imL, reL ); |
| } |
| else |
| { |
| phL = -16384 - bbs_atan2( reL, imL ); |
| } |
| } |
| else |
| { |
| if( reL > imL ) |
| { |
| phL = -( -32768 + bbs_atan2( imL, reL ) ); |
| } |
| else |
| { |
| if( imL == 0 ) phL = 0; |
| else phL = 16384 + bbs_atan2( reL, imL ); |
| } |
| } |
| } |
| else |
| { |
| if( imL < 0 ) |
| { |
| imL = -imL; |
| if( reL > imL ) |
| { |
| phL = -bbs_atan2( imL, reL ); |
| } |
| else |
| { |
| phL = -16384 + bbs_atan2( reL, imL ); |
| } |
| } |
| else |
| { |
| if( reL > imL ) |
| { |
| phL = bbs_atan2( imL, reL ); |
| } |
| else |
| { |
| if( imL == 0 ) phL = 0; |
| else phL = 16384 - bbs_atan2( reL, imL ); |
| } |
| } |
| } |
| |
| *phaseA++ = phL; |
| } |
| return 0; |
| } |
| |
| #endif |
| |
| /* ------------------------------------------------------------------------- */ |
