auto import from //depot/cupcake/@135843
[android/platform/external/neven.git] / Embedded / common / src / b_BasicEm / Math.c
1 /*
2  * Copyright (C) 2008 The Android Open Source Project
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *      http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16
17 /* ---- includes ----------------------------------------------------------- */
18
19 #include "b_BasicEm/Math.h"
20 #include "b_BasicEm/Functions.h"
21
22 /* ---- related objects  --------------------------------------------------- */
23
24 /* ---- typedefs ----------------------------------------------------------- */
25
26 /* ---- constants ---------------------------------------------------------- */
27
28 /* ------------------------------------------------------------------------- */
29
30 /* ========================================================================= */
31 /*                                                                           */
32 /* ---- \ghd{ external functions } ----------------------------------------- */
33 /*                                                                           */
34 /* ========================================================================= */
35
36 #if defined( HW_SSE2 )
37         extern int32 bbs_dotProduct_128SSE2( const int16* vec1A, const int16* vec2A, uint32 sizeA );
38         extern int32 bbs_dotProduct_u128SSE2( const int16* vec1A, const int16* vec2A, uint32 sizeA );
39 #endif
40
41 #if defined( HW_FR71 )
42         int32 bbs_dotProduct_fr71( const int16* vec1A, const int16* vec2A, uint32 sizeA );
43 #endif
44
45 /* ------------------------------------------------------------------------- */
46
47 uint16 bbs_sqrt32( uint32 valA )
48 {
49         uint32 rootL = 0;
50         uint32 expL = 0;
51         expL += ( ( valA >> ( expL + 0x10 ) ) != 0 ) << 4;
52         expL += ( ( valA >> ( expL + 0x08 ) ) != 0 ) << 3;
53         expL += ( ( valA >> ( expL + 0x04 ) ) != 0 ) << 2;
54         expL += ( ( valA >> ( expL + 0x02 ) ) != 0 ) << 1;
55         switch( expL >> 1 )
56         {
57                 case 15: rootL += ( ( rootL + 0x8000 ) * ( rootL + 0x8000 ) <= valA ) << 15;
58                 case 14: rootL += ( ( rootL + 0x4000 ) * ( rootL + 0x4000 ) <= valA ) << 14;
59                 case 13: rootL += ( ( rootL + 0x2000 ) * ( rootL + 0x2000 ) <= valA ) << 13;
60                 case 12: rootL += ( ( rootL + 0x1000 ) * ( rootL + 0x1000 ) <= valA ) << 12;
61                 case 11: rootL += ( ( rootL + 0x0800 ) * ( rootL + 0x0800 ) <= valA ) << 11;
62                 case 10: rootL += ( ( rootL + 0x0400 ) * ( rootL + 0x0400 ) <= valA ) << 10;
63                 case 9:  rootL += ( ( rootL + 0x0200 ) * ( rootL + 0x0200 ) <= valA ) << 9;
64                 case 8:  rootL += ( ( rootL + 0x0100 ) * ( rootL + 0x0100 ) <= valA ) << 8;
65                 case 7:  rootL += ( ( rootL + 0x0080 ) * ( rootL + 0x0080 ) <= valA ) << 7;
66                 case 6:  rootL += ( ( rootL + 0x0040 ) * ( rootL + 0x0040 ) <= valA ) << 6;
67                 case 5:  rootL += ( ( rootL + 0x0020 ) * ( rootL + 0x0020 ) <= valA ) << 5;
68                 case 4:  rootL += ( ( rootL + 0x0010 ) * ( rootL + 0x0010 ) <= valA ) << 4;
69                 case 3:  rootL += ( ( rootL + 0x0008 ) * ( rootL + 0x0008 ) <= valA ) << 3;
70                 case 2:  rootL += ( ( rootL + 0x0004 ) * ( rootL + 0x0004 ) <= valA ) << 2;
71                 case 1:  rootL += ( ( rootL + 0x0002 ) * ( rootL + 0x0002 ) <= valA ) << 1;
72                 case 0:  rootL += ( ( rootL + 0x0001 ) * ( rootL + 0x0001 ) <= valA ) << 0;
73         }
74
75         return ( uint16 )rootL;
76 }
77
78 /* ------------------------------------------------------------------------- */
79
80 uint8 bbs_sqrt16( uint16 valA )
81 {
82         uint16 rootL = 0;
83         rootL += ( ( rootL + 0x80 ) * ( rootL + 0x80 ) <= valA ) << 7;
84         rootL += ( ( rootL + 0x40 ) * ( rootL + 0x40 ) <= valA ) << 6;
85         rootL += ( ( rootL + 0x20 ) * ( rootL + 0x20 ) <= valA ) << 5;
86         rootL += ( ( rootL + 0x10 ) * ( rootL + 0x10 ) <= valA ) << 4;
87         rootL += ( ( rootL + 0x08 ) * ( rootL + 0x08 ) <= valA ) << 3;
88         rootL += ( ( rootL + 0x04 ) * ( rootL + 0x04 ) <= valA ) << 2;
89         rootL += ( ( rootL + 0x02 ) * ( rootL + 0x02 ) <= valA ) << 1;
90         rootL += ( ( rootL + 0x01 ) * ( rootL + 0x01 ) <= valA ) << 0;
91         return ( uint8 )rootL;
92 }
93
94 /* ------------------------------------------------------------------------- */
95
96 /* table of sqrt and slope values */
97 const uint32 bbs_fastSqrt32_tableG[] = 
98 {
99         268435456, 1016, 272596992, 1000, 276692992, 987, 280735744, 972, 
100         284717056, 959, 288645120, 946, 292519936, 933, 296341504, 922, 
101         300118016, 910, 303845376, 899, 307527680, 889, 311169024, 878, 
102         314765312, 869, 318324736, 858, 321839104, 850, 325320704, 840, 
103         328761344, 832, 332169216, 824, 335544320, 815, 338882560, 807, 
104         342188032, 799, 345460736, 792, 348704768, 785, 351920128, 777, 
105         355102720, 771, 358260736, 764, 361390080, 757, 364490752, 751, 
106         367566848, 745, 370618368, 739, 373645312, 732, 376643584, 727, 
107         379621376, 722, 382578688, 715, 385507328, 711, 388419584, 705, 
108         391307264, 700, 394174464, 695, 397021184, 689, 399843328, 686, 
109         402653184, 680, 405438464, 675, 408203264, 672, 410955776, 666, 
110         413683712, 663, 416399360, 658, 419094528, 653, 421769216, 650, 
111         424431616, 646, 427077632, 641, 429703168, 638, 432316416, 634, 
112         434913280, 630, 437493760, 627, 440061952, 622, 442609664, 620, 
113         445149184, 615, 447668224, 613, 450179072, 609, 452673536, 605, 
114         455151616, 602, 457617408, 600, 460075008, 595, 462512128, 593, 
115         464941056, 590, 467357696, 587, 469762048, 583, 472150016, 581, 
116         474529792, 578, 476897280, 575, 479252480, 572, 481595392, 569, 
117         483926016, 567, 486248448, 564, 488558592, 561, 490856448, 559, 
118         493146112, 556, 495423488, 553, 497688576, 552, 499949568, 548, 
119         502194176, 546, 504430592, 544, 506658816, 541, 508874752, 539, 
120         511082496, 537, 513282048, 534, 515469312, 533, 517652480, 529, 
121         519819264, 528, 521981952, 526, 524136448, 523, 526278656, 521, 
122         528412672, 519, 530538496, 517, 532656128, 515, 534765568, 514 
123 };
124
125 uint16 bbs_fastSqrt32( uint32 valA )
126 {
127         uint32 expL = 0;
128         uint32 valL;
129         uint32 offsL;
130         uint32 indexL = 0;
131
132         if( valA == 0 ) return 0;
133
134         /* compute closest even size exponent of valA */
135         expL += ( ( valA >> ( expL + 0x10 ) ) != 0 ) << 4;
136         expL += ( ( valA >> ( expL + 0x08 ) ) != 0 ) << 3;
137         expL += ( ( valA >> ( expL + 0x04 ) ) != 0 ) << 2;
138         expL += ( ( valA >> ( expL + 0x02 ) ) != 0 ) << 1;
139
140         valL = ( ( valA << ( 30 - expL ) ) - 1073741824 ); /* ( 1 << 30 ) */
141         offsL = ( ( valL & 0x01FFFFFF ) + ( 1 << 12 ) ) >> 13;
142         indexL = ( valL >> 24 ) & 0xFE;
143
144         return ( bbs_fastSqrt32_tableG[ indexL ] + offsL * bbs_fastSqrt32_tableG[ indexL + 1 ] ) >> ( 28 - ( expL >> 1 ) );
145 }
146
147 /* ------------------------------------------------------------------------- */
148
149 /* table of 1/sqrt (1.31) and negative slope (.15) values 
150    referenced in b_GaborCueEm/focusDispAsm.s55, do not rename or remove ! */
151 const uint32 bbs_invSqrt32_tableG[] = 
152 {
153         2147483648u, 1001, 2114682880, 956, 2083356672, 915, 2053373952, 877,
154         2024636416, 840, 1997111296, 808, 1970634752, 776, 1945206784, 746,
155         1920761856, 720, 1897168896, 693, 1874460672, 669, 1852538880, 646,
156         1831370752, 625, 1810890752, 604, 1791098880, 584, 1771962368, 567,
157         1753382912, 548, 1735426048, 533, 1717960704, 516, 1701052416, 502,
158         1684602880, 487, 1668644864, 474, 1653112832, 461, 1638006784, 448,
159         1623326720, 436, 1609039872, 426, 1595080704, 414, 1581514752, 404,
160         1568276480, 394, 1555365888, 384, 1542782976, 375, 1530494976, 367,
161         1518469120, 357, 1506770944, 350, 1495302144, 342, 1484095488, 334,
162         1473150976, 327, 1462435840, 320, 1451950080, 313, 1441693696, 307,
163         1431633920, 300, 1421803520, 294, 1412169728, 289, 1402699776, 282,
164         1393459200, 277, 1384382464, 272, 1375469568, 266, 1366753280, 262,
165         1358168064, 257, 1349746688, 251, 1341521920, 248, 1333395456, 243,
166         1325432832, 238, 1317634048, 235, 1309933568, 230, 1302396928, 227,
167         1294958592, 222, 1287684096, 219, 1280507904, 216, 1273430016, 211,
168         1266515968, 209, 1259667456, 205, 1252950016, 202, 1246330880, 198,
169         1239842816, 196, 1233420288, 192, 1227128832, 190, 1220902912, 187,
170         1214775296, 184, 1208745984, 181, 1202814976, 179, 1196949504, 176,
171         1191182336, 173, 1185513472, 171, 1179910144, 169, 1174372352, 166,
172         1168932864, 164, 1163558912, 162, 1158250496, 160, 1153007616, 157,
173         1147863040, 155, 1142784000, 154, 1137737728, 151, 1132789760, 149,
174         1127907328, 148, 1123057664, 145, 1118306304, 144, 1113587712, 142,
175         1108934656, 140, 1104347136, 138, 1099825152, 137, 1095335936, 135,
176         1090912256, 134, 1086521344, 131, 1082228736, 131, 1077936128, 128              
177 };
178
179 uint32 bbs_invSqrt32( uint32 valA )
180 {
181
182         uint32 expL = 0;
183         uint32 valL;
184         uint32 offsL;
185         uint32 indexL = 0;
186
187         if( valA == 0U ) return 0x80000000;
188
189         /* compute closest even size exponent of valA */
190         expL += ( ( valA >> ( expL + 0x10 ) ) != 0 ) << 4;
191         expL += ( ( valA >> ( expL + 0x08 ) ) != 0 ) << 3;
192         expL += ( ( valA >> ( expL + 0x04 ) ) != 0 ) << 2;
193         expL += ( ( valA >> ( expL + 0x02 ) ) != 0 ) << 1;
194         
195         valL = ( ( valA << ( 30 - expL ) ) - 1073741824 ); /* ( 1 << 30 ) */
196         offsL = ( ( valL & 0x01FFFFFF ) + ( 1 << 9 ) ) >> 10;
197         indexL = ( valL >> 24 ) & 0xFE;
198         
199         return ( bbs_invSqrt32_tableG[ indexL ] - offsL * bbs_invSqrt32_tableG[ indexL + 1 ] ) >> ( expL >> 1 );
200 }
201
202 /* ------------------------------------------------------------------------- */
203
204 /* table of 1/( x + 1 ) (2.30) and negative slope (.14) values
205    referenced in b_GaborCueEm/focusDispAsm.s55, do not rename or remove ! */
206 const int32 bbs_inv32_tableG[] = 
207 {
208         1073741824, 1986, 1041203200, 1870, 1010565120, 1762, 981696512, 1664,
209         954433536,  1575, 928628736,  1491, 904200192,  1415, 881016832, 1345,
210         858980352,  1278, 838041600,  1218, 818085888,  1162, 799047680, 1108,
211         780894208,  1059, 763543552,  1013, 746946560,  970,  731054080, 930,
212         715816960,  891,  701218816,  856,  687194112,  823,  673710080, 791,
213         660750336,  761,  648282112,  732,  636289024,  706,  624721920, 681,
214         613564416,  657,  602800128,  635,  592396288,  613,  582352896, 592,
215         572653568,  573,  563265536,  554,  554188800,  537,  545390592, 520,
216 };
217
218 int32 bbs_inv32( int32 valA )
219 {
220
221         uint32 expL = 0;
222         int32 signL = ( ( valA >> 30 ) & 0xFFFFFFFE ) + 1;
223         int32 valL = signL * valA;
224         int32 offsL;
225         uint32 indexL = 0;
226
227         if( valL <= ( int32 ) 1 ) return 0x40000000 * signL;
228
229         /* compute size exponent of valL */
230         expL += ( ( valL >> ( expL + 0x10 ) ) != 0 ) << 4;
231         expL += ( ( valL >> ( expL + 0x08 ) ) != 0 ) << 3;
232         expL += ( ( valL >> ( expL + 0x04 ) ) != 0 ) << 2;
233         expL += ( ( valL >> ( expL + 0x02 ) ) != 0 ) << 1;
234         expL += ( ( valL >> ( expL + 0x01 ) ) != 0 );
235         
236         valL = ( ( valL << ( 30 - expL ) ) - 1073741824 ); /*( 1U << 30 )*/
237         offsL = ( ( valL & 0x01FFFFFF ) + ( 1 << 10 ) ) >> 11;
238         indexL = ( valL >> 24 ) & 0xFE;
239         
240         return signL * ( ( ( ( bbs_inv32_tableG[ indexL ] - offsL * bbs_inv32_tableG[ indexL + 1 ] ) >> ( expL - 1 ) ) + 1 ) >> 1 );
241 }
242
243 /* ------------------------------------------------------------------------- */
244
245 uint32 bbs_intLog2( uint32 valA )
246 {
247         uint32 expL = 0;
248         expL += 0x10 * ( ( valA >> ( expL + 0x10 ) ) != 0 );
249         expL += 0x08 * ( ( valA >> ( expL + 0x08 ) ) != 0 );
250         expL += 0x04 * ( ( valA >> ( expL + 0x04 ) ) != 0 );
251         expL += 0x02 * ( ( valA >> ( expL + 0x02 ) ) != 0 );
252         expL += 0x01 * ( ( valA >> ( expL + 0x01 ) ) != 0 );
253         return expL;
254 }
255
256 /* ------------------------------------------------------------------------- */
257
258 const uint32 bbs_pow2M1_tableG[] = 
259 {
260         0,                      713,    46769127,       721,    94047537,       729,    141840775,      737,
261         190154447,      745,    238994221,      753,    288365825,      761,    338275050,      769,
262         388727751,      778,    439729846,      786,    491287318,      795,    543406214,      803,
263         596092647,      812,    649352798,      821,    703192913,      830,    757619310,      839,
264         812638371,      848,    868256550,      857,    924480371,      867,    981316430,      876,
265         1038771393, 886,        1096851999, 895,        1155565062, 905,        1214917468, 915,
266         1274916179, 925,        1335568233, 935,        1396880745, 945,        1458860907, 956,
267         1521515988, 966,        1584853338, 976,        1648880387, 987,        1713604645, 998,
268         1779033703, 1009,       1845175238, 1020,       1912037006, 1031,       1979626852, 1042,
269         2047952702, 1053,       2117022572, 1065,       2186844564u, 1077,      2257426868u, 1088,
270         2328777762u, 1100,      2400905617u, 1112,      2473818892u, 1124,      2547526141u, 1136,
271         2622036010u, 1149,      2697357237u, 1161,      2773498659u, 1174,      2850469207u, 1187,
272         2928277909u, 1200,      3006933892u, 1213,      3086446383u, 1226,      3166824708u, 1239,
273         3248078296u, 1253,      3330216677u, 1266,      3413249486u, 1280,      3497186464u, 1294,
274         3582037455u, 1308,      3667812413u, 1323,      3754521399u, 1337,      3842174584u, 1352,
275         3930782250u, 1366,      4020354790u, 1381,      4110902710u, 1396,      4202436633u, 1411
276 };
277
278 uint32 bbs_pow2M1( uint32 valA )
279 {
280         uint32 offsL = ( valA & 0x03FFFFFF ) >> 10;
281         uint16 indexL = ( ( valA & 0xFC000000 ) >> 26 ) << 1;
282         return bbs_pow2M1_tableG[ indexL ] + offsL * bbs_pow2M1_tableG[ indexL + 1 ];   
283 }
284
285 /* ------------------------------------------------------------------------- */
286
287 uint32 bbs_pow2( int32 valA )
288 {
289         int32 shiftL = 16 - ( valA >> 27 );
290         uint32 offsL  = ( uint32 )( valA << 5 );
291         if( shiftL == 32 ) return 1;
292         return ( 1 << ( 32 - shiftL ) ) + ( bbs_pow2M1( offsL ) >> shiftL );
293 }
294
295 /* ------------------------------------------------------------------------- */
296
297 uint32 bbs_exp( int32 valA )
298 {
299         int32 adjustedL;
300         int32 shiftL;
301         int32 offsL;
302
303         /* check boundaries to avoid overflow */
304         if( valA < -1488522236 )
305         {
306                 return 0;
307         }
308         else if( valA > 1488522236 )
309         {
310                 return 0xFFFFFFFF;
311         }
312
313         /* multily valA with 1/ln(2) in order to use function 2^x instead of e^x */
314         adjustedL = ( valA >> 16 ) * 94548 + ( ( ( ( ( uint32 )valA ) & 0x0FFFF ) * 47274 ) >> 15 );
315
316         shiftL = 16 - ( adjustedL >> 27 );
317         if( shiftL == 32 ) return 1;
318         offsL  = ( uint32 )( adjustedL << 5 );
319         return ( ( int32 ) 1 << ( 32 - shiftL ) ) + ( bbs_pow2M1( offsL ) >> shiftL );
320 }
321
322 /* ------------------------------------------------------------------------- */
323
324 int16 bbs_satS16( int32 valA )
325 {
326         if( valA > 32767 ) return 32767;
327         if( valA < -32768 ) return -32768;
328         return valA;
329 }
330
331 /* ------------------------------------------------------------------------- */
332
333 #if defined( HW_i586 ) || defined( HW_i686 )
334
335 /* Windows section */
336 #if defined( WIN32 ) && !defined( WIN64 )
337
338 /* disable warning "no return value"*/
339 #pragma warning( disable : 4035 )
340
341 /** 
342  * computes a fast dot product using intel MMX, sizeA must be multiple of 16 and >0 
343  */
344 int32 bbs_dotProduct_intelMMX16( const int16* vec1A, const int16* vec2A, uint32 sizeA )
345 {
346         __asm
347         {       
348                         push    esi
349                         push    edi
350
351                         mov     eax, vec1A
352                         mov     ebx, vec2A
353
354                         mov     ecx, sizeA
355
356                         pxor    mm4, mm4
357                         pxor    mm6, mm6
358
359                         pxor    mm7, mm7
360                         shr             ecx, 4
361
362                 inner_loop_start:
363                         movq    mm0, 0[eax]
364                         paddd   mm7, mm4
365
366                         movq    mm1, 0[ebx]
367                         paddd   mm7, mm6
368
369                         movq    mm2, 8[eax]
370                         pmaddwd mm0, mm1
371
372                         movq    mm3, 8[ebx]
373
374                         movq    mm4, 16[eax]
375                         pmaddwd mm2, mm3
376
377                         movq    mm5, 16[ebx]
378                         paddd   mm7, mm0
379
380                         movq    mm6, 24[eax]
381                         pmaddwd mm4, mm5
382
383                         pmaddwd mm6, 24[ebx]
384                         paddd   mm7, mm2
385
386                         add     eax, 32
387                         add     ebx, 32
388
389                         dec     ecx
390                         jnz     inner_loop_start
391
392                         paddd   mm7, mm4
393
394                         paddd   mm7, mm6
395         
396                         movq    mm0, mm7
397
398                         psrlq   mm0, 32
399
400                         paddd   mm7, mm0
401
402                         movd    eax, mm7
403                         
404                         emms
405                         pop     edi
406                         pop     esi
407         }
408 }
409
410 #pragma warning( default : 4035 )
411
412 /* gcc compiler section */
413 #elif defined( epl_LINUX ) || defined( CYGWIN )
414
415 /**
416  * computes a fast dot product using intel MMX, sizeA must be multiple of 16 and >0
417  */
418 int32 bbs_dotProduct_intelMMX16( const int16* vec1A, const int16* vec2A, uint32 sizeA )
419 {
420         int32 resultL;
421
422         __asm__ __volatile__(
423
424                         "movl %1,%%eax\n\t"
425                         "movl %2,%%ebx\n\t"
426
427                         "movl %3,%%ecx\n\t"
428
429                         "pxor %%mm4,%%mm4\n\t"
430                         "pxor %%mm6,%%mm6\n\t"
431
432                         "pxor %%mm7, %%mm7\n\t"
433                         "shrl $4, %%ecx\n\t"
434
435                         "\n1:\t"
436                         "movq 0(%%eax),%%mm0\n\t"
437                         "paddd %%mm4,%%mm7\n\t"
438
439                         "movq 0( %%ebx ),%%mm1\n\t"
440                         "paddd %%mm6,%%mm7\n\t"
441                         
442                         "movq 8( %%eax ),%%mm2\n\t"
443                         "pmaddwd %%mm1,%%mm0\n\t"
444
445                         "movq 8( %%ebx ),%%mm3\n\t"
446
447                         "movq 16( %%eax ),%%mm4\n\t"
448                         "pmaddwd %%mm3,%%mm2\n\t"
449
450                         "movq 16( %%ebx ),%%mm5\n\t"
451                         "paddd %%mm0,%%mm7\n\t"
452
453                         "movq 24( %%eax ),%%mm6\n\t"
454                         "pmaddwd %%mm5,%%mm4\n\t"
455
456                         "pmaddwd 24( %%ebx ),%%mm6\n\t"
457                         "paddd %%mm2,%%mm7\n\t"
458
459                         "addl $32,%%eax\n\t"
460                         "addl $32,%%ebx\n\t"
461
462                         "decl %%ecx\n\t"
463                         "jnz 1b\n\t"
464
465                         "paddd %%mm4,%%mm7\n\t"
466                         "paddd %%mm6,%%mm7\n\t"
467         
468                         "movq  %%mm7,%%mm0\n\t"
469
470                         "psrlq $32,%%mm0\n\t"
471
472                         "paddd %%mm0,%%mm7\n\t"
473
474                         "movd %%mm7,%0\n\t"
475                         
476                         "emms\n\t"
477
478                 : "=&g" ( resultL )
479                 : "g" ( vec1A ), "g" ( vec2A ), "g" ( sizeA )
480                 : "si", "di", "ax", "bx", "cx", "st", "memory" );
481
482         return resultL;
483 }
484
485 #endif /* epl_LINUX, CYGWIN */
486
487 #endif /* HW_i586 || HW_i686 */
488
489 /* ------------------------------------------------------------------------- */
490
491 #ifdef HW_TMS320C6x
492 /**
493  * Calls fast assembler version of dotproduct for DSP. 
494  * dotProduct_C62x is implemented in file dotprod.asm and expects input vectors
495  * of even length.
496  */
497 int32 bbs_dotProduct_dsp( const int16* vec1A, const int16* vec2A, uint32 sizeA )
498 {
499         if( sizeA & 1 )
500         {
501                 int32 resultL;          
502                 resultL = dotProduct_C62x( vec1A, vec2A, sizeA - 1 );
503                 return resultL + ( int32 ) *( vec1A + sizeA - 1 ) * *( vec2A + sizeA - 1 );
504         }
505         else
506         {
507                 return dotProduct_C62x( vec1A, vec2A, sizeA );
508         }
509 }
510 #endif /* HW_TMS320C6x */
511
512 /* ------------------------------------------------------------------------- */
513
514 /* 16 dot product for the PS2/EE processor */
515 /* input vectors MUST be 128 bit aligned ! */
516
517 #if defined( epl_LINUX ) && defined( HW_EE )
518
519 int32 bbs_dotProduct_EE( const int16* vec1A, const int16* vec2A, uint32 sizeA )
520 {
521         int32 resultL = 0,
522               iL = sizeA >> 3,
523               jL = sizeA - ( iL << 3 );
524
525         if( iL > 0 )
526         {
527                 /* multiply-add elements of input vectors in sets of 8 */
528                 int32 accL[ 4 ], t1L, t2L, t3L;
529                 asm volatile (
530                         "pxor %4, %2, %2\n\t"                   /* reset 8 accumulators (LO and HI register) to 0 */
531                         "pmtlo %4\n\t"          
532                         "pmthi %4\n\t"
533
534                         "\n__begin_loop:\t"
535
536                         "lq %2,0(%0)\n\t"                               /* load 8 pairs of int16 */
537                         "lq %3,0(%1)\n\t"                       
538
539                         "addi %0,%0,16\n\t"                             /* vec1L += 16 */
540                         "addi %1,%1,16\n\t"                             /* vec2L += 16 */
541                         "addi %7,%7,-1\n\t"                             /* iL-- */
542
543                         "pmaddh %4,%2,%3\n\t"                   /* parallel multiply-add of 8 pairs of int16 */
544
545                         "bgtzl %7,__begin_loop\n\t"             /* if iL > 0 goto _begin_loop */
546
547                         "pmflo %2\n\t"                                  /* parallel add 8 accumulators , store remaining 4 accumulators in tmpL */
548                         "pmfhi %3\n\t"
549                         "paddw %4,%2,%3\n\t"                                            
550                         "sq %4,0(%8)\n\t"
551                 : "=r" ( vec1A ), "=r" ( vec2A ), "=r" ( t1L ), "=r" ( t2L ), "=r" ( t3L )
552                 : "0" ( vec1A ), "1" ( vec2A ), "r" ( iL ), "r" ( accL )
553                 : "memory" );
554
555                 /* add 4 parallel accumulators */
556                 resultL += accL[ 0 ] + accL[ 1 ] + accL[ 2 ] + accL[ 3 ];
557         }
558         
559         /* multiply-add remaining elements of input vectors */
560         for( ; jL--; ) resultL += ( int32 ) *vec1A++ * *vec2A++;
561
562         return resultL;
563 }
564
565 #endif
566
567 /* ------------------------------------------------------------------------- */
568
569 #if defined( HW_ARMv5TE )
570
571 /* fast 16 dot product for ARM9E cores (DSP extensions).
572  * input vectors must be 32 bit aligned
573  */
574 int32 bbs_dotProduct_arm9e( const int16* vec1A, const int16* vec2A, uint32 sizeA )
575 {
576         int32 accuL = 0;
577
578         int32* v1PtrL = ( int32* )vec1A;
579         int32* v2PtrL = ( int32* )vec2A;
580
581         for( ; sizeA >= 4; sizeA -= 4 )
582         {
583                 __asm {
584                     smlabb accuL, *v1PtrL, *v2PtrL, accuL;
585                     smlatt accuL, *v1PtrL, *v2PtrL, accuL;
586                 }
587                 v1PtrL++; v2PtrL++;
588             __asm {
589                     smlabb accuL, *v1PtrL, *v2PtrL, accuL;
590                     smlatt accuL, *v1PtrL, *v2PtrL, accuL;
591                 }
592                 v1PtrL++; v2PtrL++;
593         }
594
595         vec1A = ( int16* )v1PtrL;
596         vec2A = ( int16* )v2PtrL;
597
598         /* multiply-add remaining elements of input vectors */
599         for( ; sizeA > 0; sizeA-- ) accuL += ( int32 )*vec1A++ * *vec2A++;
600
601         return accuL;
602 }
603
604 #endif
605
606 /* ------------------------------------------------------------------------- */
607
608 /**
609  * Computes a fast dot product using standard C
610  */
611 int32 bbs_dotProduct_stdc( const int16* vec1A, const int16* vec2A, uint32 sizeA )
612 {
613         int32 accuL = 0;
614
615         for( ; sizeA >= 8; sizeA -= 8 )
616         {
617                 accuL += ( int32 ) *vec1A * *vec2A;
618                 accuL += ( int32 ) *( vec1A + 1 ) * *( vec2A + 1 );
619                 accuL += ( int32 ) *( vec1A + 2 ) * *( vec2A + 2 );
620                 accuL += ( int32 ) *( vec1A + 3 ) * *( vec2A + 3 );
621
622                 accuL += ( int32 ) *( vec1A + 4 ) * *( vec2A + 4 );
623                 accuL += ( int32 ) *( vec1A + 5 ) * *( vec2A + 5 );
624                 accuL += ( int32 ) *( vec1A + 6 ) * *( vec2A + 6 );
625                 accuL += ( int32 ) *( vec1A + 7 ) * *( vec2A + 7 );
626
627                 vec1A += 8;
628                 vec2A += 8;
629         }
630
631         for( ; sizeA; sizeA-- ) accuL += ( int32 ) *vec1A++ * *vec2A++;
632
633         return accuL;
634 }
635
636 /* ------------------------------------------------------------------------- */
637
638 int32 bbs_dotProductInt16( const int16* vec1A, const int16* vec2A, uint32 sizeA )
639 {
640 /* PC */
641 #if ( defined( HW_i586 ) || defined( HW_i686 ) )
642
643         #if defined( HW_SSE2 )
644                 uint32 size16L = sizeA & 0xfffffff0;
645                 if( size16L )
646                 {       
647                         if( ( (uint32)vec1A & 0xF ) == 0 && ( (uint32)vec2A & 0xF ) == 0 ) 
648                         {
649                                 return bbs_dotProduct_128SSE2( vec1A, vec2A, sizeA );
650                         }
651                         else
652                         {
653                                 return bbs_dotProduct_u128SSE2( vec1A, vec2A, sizeA );
654                         }
655                 }
656         #elif !defined( WIN64 )
657                 /* MMX version (not supported by 64-bit compiler) */
658                 uint32 size16L = sizeA & 0xfffffff0;
659                 if( size16L )
660                 {       
661                         if( sizeA == size16L )
662                         {
663                                 return bbs_dotProduct_intelMMX16( vec1A, vec2A, size16L );
664                         }
665                         return bbs_dotProduct_intelMMX16( vec1A, vec2A, size16L )
666                                         + bbs_dotProduct_stdc( vec1A + size16L, vec2A + size16L, sizeA - size16L );
667                 } /* if( size16L ) */
668         #endif
669
670         return bbs_dotProduct_stdc( vec1A, vec2A, sizeA );
671
672 /* Playstation 2 */
673 #elif defined( HW_EE ) && defined( epl_LINUX ) 
674
675         if( ( (uint32)vec1A & 0xF ) == 0 && ( (uint32)vec2A & 0xF ) == 0 ) 
676         {
677                 return bbs_dotProduct_EE( vec1A, vec2A, sizeA );
678         }
679         return bbs_dotProduct_stdc( vec1A, vec2A, sizeA );
680
681 /* ARM9E */
682 #elif defined( HW_ARMv5TE )
683
684         return bbs_dotProduct_arm9e( vec1A, vec2A, sizeA );
685
686 /* TI C6000 */
687 #elif defined( HW_TMS320C6x )
688
689         return bbs_dotProduct_dsp( vec1A, vec2A, sizeA );
690         
691 #elif defined( HW_FR71 )
692
693         uint32 size16L = sizeA & 0xfffffff0;
694         if( size16L )
695         {       
696                 if( sizeA == size16L )
697                 {
698                         return bbs_dotProduct_fr71( vec1A, vec2A, size16L );
699                 }
700                 return bbs_dotProduct_fr71( vec1A, vec2A, size16L )
701                                 + bbs_dotProduct_stdc( vec1A + size16L, vec2A + size16L, sizeA - size16L );
702         }
703
704         return bbs_dotProduct_stdc( vec1A, vec2A, sizeA );
705
706 #endif
707
708         return bbs_dotProduct_stdc( vec1A, vec2A, sizeA );
709 }
710
711 /* ------------------------------------------------------------------------- */
712
713 /* table of fermi and slope values (result: 2.30; offset: .12) 
714    referenced in b_NeuralNetEm/FastMlpNet.c, not not rename or remove */
715 const uint32 bbs_fermi_tableG[] = 
716 {
717         45056,      8,     77824,      13,    131072,     21,    217088,     34,
718         356352,     57,    589824,     94,    974848,     155,   1609728,    255,
719         2654208,    418,   4366336,    688,   7184384,    1126,  11796480,   1834,
720         19308544,   2970,  31473664,   4748,  50921472,   7453,  81448960,   11363,
721         127991808,  16573, 195874816,  22680, 288772096,  28469, 405381120,  32102,
722         536870912,  32101, 668356608,  28469, 784965632,  22680, 877862912,  16573,
723         945745920,  11363, 992288768,  7453,  1022816256, 4748,  1042264064, 2970,
724         1054429184, 1834,  1061941248, 1126,  1066553344, 688,   1069371392, 418,
725         1071083520, 255,   1072128000, 155,   1072762880, 94,    1073147904, 57,
726         1073381376, 34,    1073520640, 21,    1073606656, 13,    1073659904, 8,
727 };
728
729 int32 bbs_fermi( int32 valA )
730 {
731         uint32 indexL = ( ( valA >> 15 ) + 20 ) << 1;
732         uint32 offsL  = ( ( valA & 0x00007FFF ) + 4 ) >> 3;
733         if( valA <  -655360 ) return 1;
734         if( valA >=  655360 ) return 1073741824 - 1; /* ( 1 << 30 ) */
735         return ( bbs_fermi_tableG[ indexL ] + offsL * bbs_fermi_tableG[ indexL + 1 ] );
736 }
737
738 /* ------------------------------------------------------------------------- */
739
740 void bbs_uint32ReduceToNBits( uint32* argPtrA, int32* bbpPtrA, uint32 nBitsA )
741 {
742         int32 posHighestBitL = bbs_intLog2( *argPtrA ) + 1;
743         int32 shiftL = posHighestBitL - nBitsA;
744         if( shiftL > 0 )
745         {
746                 ( *argPtrA ) >>= shiftL;
747                 ( *bbpPtrA ) -= shiftL;
748         }
749 }
750
751 /* ------------------------------------------------------------------------- */
752
753 void bbs_int32ReduceToNBits( int32* argPtrA, int32* bbpPtrA, uint32 nBitsA )
754 {
755         int32 posHighestBitL = bbs_intLog2( bbs_abs( *argPtrA ) ) + 1;
756         int32 shiftL = posHighestBitL - nBitsA;
757         if( shiftL > 0 )
758         {
759                 ( *argPtrA ) >>= shiftL;
760                 ( *bbpPtrA ) -= shiftL;
761         }
762 }
763
764 /* ------------------------------------------------------------------------- */
765
766 uint32 bbs_convertU32( uint32 srcA, int32 srcBbpA, int32 dstBbpA )
767 {
768         if( dstBbpA >= srcBbpA )
769         {
770                 uint32 shiftL = dstBbpA - srcBbpA;
771                 if( srcA > ( ( uint32 )0xFFFFFFFF >> shiftL ) )
772                 {
773                         /* overflow */
774                         return ( uint32 )0xFFFFFFFF;
775                 }
776                 else
777                 {
778                         return srcA << shiftL;
779                 }
780         }
781         else
782         {
783                 uint32 shiftL = srcBbpA - dstBbpA;
784                 uint32 addL = 1L << ( shiftL - 1 );
785                 if( srcA + addL < addL )
786                 {
787                         /* rounding would cause overflow */
788                         return srcA >> shiftL;
789                 }
790                 else
791                 {
792                         return ( srcA + addL ) >> shiftL;
793                 }
794         }
795 }
796
797 /* ------------------------------------------------------------------------- */
798
799 int32 bbs_convertS32( int32 srcA, int32 srcBbpA, int32 dstBbpA )
800 {
801         if( dstBbpA >= srcBbpA )
802         {
803                 uint32 shiftL = ( uint32 )( dstBbpA - srcBbpA );
804                 if( srcA > ( ( int32 )0x7FFFFFFF >> shiftL ) )
805                 {
806                         /* overflow */
807                         return ( uint32 )0x7FFFFFFF;
808                 }
809                 else if( srcA < ( ( int32 )0x80000000 >> shiftL ) )
810                 {
811                         /* underflow */
812                         return ( int32 )0x80000000;
813                 }
814                 else
815                 {
816                         return srcA << shiftL;
817                 }
818         }
819         else
820         {
821                 uint32 shiftL = ( uint32 )( srcBbpA - dstBbpA );
822                 int32 addL = 1L << ( shiftL - 1 );
823                 if( srcA + addL < addL )
824                 {
825                         /* rounding would cause overflow */
826                         return srcA >> shiftL;
827                 }
828                 else
829                 {
830                         return ( srcA + addL ) >> shiftL;
831                 }
832         }
833 }
834
835 /* ------------------------------------------------------------------------- */
836
837 int32 bbs_vecPowerFlt16( const int16 *xA, int16 nxA )
838 {
839 /*      #if defined( HW_TMS320C5x )
840                 uint32 rL;
841                 power( ( int16* ) xA, ( int32* ) &rL, nxA );  // does not work properly in DSPLib version 2.20.02
842                 return ( rL >> 1 );
843         #else*/
844                 /* needs to be optimized */
845                 int32 rL = 0;
846                 for( ; nxA--; )
847                 {
848                         rL += ( int32 ) *xA * *xA;
849                         xA++;
850                 }
851                 return rL;
852 /*      #endif */
853 }
854
855 /* ------------------------------------------------------------------------- */
856
857 void bbs_mulU32( uint32 v1A, uint32 v2A, uint32* manPtrA, int32* expPtrA )
858 {
859         uint32 log1L = bbs_intLog2( v1A );
860         uint32 log2L = bbs_intLog2( v2A );
861
862         if( log1L + log2L < 32 )
863         {
864                 *manPtrA = v1A * v2A;
865                 *expPtrA = 0;
866         }
867         else
868         {
869                 uint32 v1L = v1A;
870                 uint32 v2L = v2A;
871                 uint32 exp1L = 0;
872                 uint32 exp2L = 0;
873                 if( log1L > 15 && log2L > 15 ) 
874                 {
875                         exp1L = log1L - 15;
876                         exp2L = log2L - 15;
877                         v1L = ( ( v1L >> ( exp1L - 1 ) ) + 1 ) >> 1;
878                         v2L = ( ( v2L >> ( exp2L - 1 ) ) + 1 ) >> 1;
879                 }
880                 else if( log1L > 15 ) 
881                 {
882                         exp1L = log1L + log2L - 31;
883                         v1L = ( ( v1L >> ( exp1L - 1 ) ) + 1 ) >> 1;
884                 }
885                 else
886                 {
887                         exp2L = log1L + log2L - 31;
888                         v2L = ( ( v2L >> ( exp2L - 1 ) ) + 1 ) >> 1;
889                 }
890
891                 *manPtrA = v1L * v2L;
892                 *expPtrA = exp1L + exp2L;
893         }
894 }
895
896 /* ------------------------------------------------------------------------- */
897
898 void bbs_mulS32( int32 v1A, int32 v2A, int32* manPtrA, int32* expPtrA )
899 {
900         uint32 log1L = bbs_intLog2( v1A > 0 ? v1A : -v1A );
901         uint32 log2L = bbs_intLog2( v2A > 0 ? v2A : -v2A );
902
903         if( log1L + log2L < 30 )
904         {
905                 *manPtrA = v1A * v2A;
906                 *expPtrA = 0;
907         }
908         else
909         {
910                 int32 v1L = v1A;
911                 int32 v2L = v2A;
912                 int32 exp1L = 0;
913                 int32 exp2L = 0;
914                 if( log1L > 14 && log2L > 14 ) 
915                 {
916                         exp1L = log1L - 14;
917                         exp2L = log2L - 14;
918                         v1L = ( ( v1L >> ( exp1L - 1 ) ) + 1 ) >> 1;
919                         v2L = ( ( v2L >> ( exp2L - 1 ) ) + 1 ) >> 1;
920                 }
921                 else if( log1L > 14 ) 
922                 {
923                         exp1L = log1L + log2L - 29;
924                         v1L = ( ( v1L >> ( exp1L - 1 ) ) + 1 ) >> 1;
925                 }
926                 else
927                 {
928                         exp2L = log1L + log2L - 29;
929                         v2L = ( ( v2L >> ( exp2L - 1 ) ) + 1 ) >> 1;
930                 }
931
932                 *manPtrA = v1L * v2L;
933                 *expPtrA = exp1L + exp2L;
934         }
935 }
936
937 /* ------------------------------------------------------------------------- */
938
939 void bbs_vecSqrNorm32( const int32* vecA, uint32 sizeA, uint32* manPtrA, uint32* expPtrA )
940 {
941         uint32 sumL = 0;
942         int32 sumExpL = 0;
943
944         uint32 iL;
945         for( iL = 0; iL < sizeA; iL++ )
946         {
947                 int32 vL = vecA[ iL ];
948                 int32 logL = bbs_intLog2( vL > 0 ? vL : -vL );
949                 int32 expL = ( logL > 14 ) ? logL - 14 : 0;
950                 uint32 prdL;
951
952                 if( expL >= 1 )
953                 {
954                         vL = ( ( vL >> ( expL - 1 ) ) + 1 ) >> 1;
955                 }
956                 else
957                 {
958                         vL = vL >> expL;
959                 }
960
961                 prdL = vL * vL;
962                 expL <<= 1; /* now exponent of product */
963
964                 if( sumExpL > expL )
965                 {
966                         uint32 shrL = sumExpL - expL;
967                         prdL = ( ( prdL >> ( shrL - 1 ) ) + 1 ) >> 1;
968                 }
969                 else if( expL > sumExpL )
970                 {
971                         uint32 shrL = expL - sumExpL;
972                         sumL = ( ( sumL >> ( shrL - 1 ) ) + 1 ) >> 1;
973                         sumExpL += shrL;
974                 }
975
976                 sumL += prdL;
977
978                 if( sumL > 0x80000000 )
979                 {
980                         sumL = ( sumL + 1 ) >> 1;
981                         sumExpL++;
982                 }
983         }
984
985         /* make exponent even */
986         if( ( sumExpL & 1 ) != 0 )
987         {
988                 sumL = ( sumL + 1 ) >> 1;
989                 sumExpL++;
990         }
991
992         if( manPtrA != NULL ) *manPtrA = sumL;
993         if( expPtrA != NULL ) *expPtrA = sumExpL;
994 }
995
996 /* ------------------------------------------------------------------------- */
997
998 void bbs_vecSqrNorm16( const int16* vecA, uint32 sizeA, uint32* manPtrA, uint32* expPtrA )
999 {
1000         uint32 sumL = 0;
1001         int32 sumExpL = 0;
1002
1003         uint32 iL;
1004         for( iL = 0; iL < sizeA; iL++ )
1005         {
1006                 int32 vL = vecA[ iL ];
1007                 uint32 prdL = vL * vL;
1008
1009                 if( sumExpL > 0 )
1010                 {
1011                         uint32 shrL = sumExpL;
1012                         prdL = ( ( prdL >> ( shrL - 1 ) ) + 1 ) >> 1;
1013                 }
1014
1015                 sumL += prdL;
1016
1017                 if( sumL > 0x80000000 )
1018                 {
1019                         sumL = ( sumL + 1 ) >> 1;
1020                         sumExpL++;
1021                 }
1022         }
1023
1024         /* make exponent even */
1025         if( ( sumExpL & 1 ) != 0 )
1026         {
1027                 sumL = ( sumL + 1 ) >> 1;
1028                 sumExpL++;
1029         }
1030
1031         if( manPtrA != NULL ) *manPtrA = sumL;
1032         if( expPtrA != NULL ) *expPtrA = sumExpL;
1033 }
1034
1035 /* ------------------------------------------------------------------------- */
1036
1037 uint32 bbs_vecNorm16( const int16* vecA, uint32 sizeA )
1038 {
1039         uint32 manL;
1040         uint32 expL;
1041         bbs_vecSqrNorm16( vecA, sizeA, &manL, &expL );
1042         manL = bbs_sqrt32( manL );
1043         return manL << ( expL >> 1 );
1044 }
1045
1046 /* ------------------------------------------------------------------------- */
1047
1048 void bbs_matMultiplyFlt16( const int16 *x1A, int16 row1A, int16 col1A, const int16 *x2A, int16 col2A, int16 *rA )
1049 {
1050         #if defined( HW_TMS320C5x )
1051                 /* operands need to be in internal memory for mmul*/
1052                 if( x1A > ( int16* ) bbs_C5X_INTERNAL_MEMORY_SIZE ||
1053                         x2A > ( int16* ) bbs_C5X_INTERNAL_MEMORY_SIZE )
1054                 {
1055                         int16 iL,jL,kL;
1056                         int16 *ptr1L, *ptr2L;
1057                         int32 sumL;
1058                         
1059                         for( iL = 0; iL < row1A; iL++ )
1060                         {
1061                                 for( jL = 0; jL < col2A; jL++ )
1062                                 {
1063                                         ptr1L = ( int16* ) x1A + iL * col1A;
1064                                         ptr2L = ( int16* ) x2A + jL;
1065                                         sumL = 0;
1066                                         for( kL = 0; kL < col1A; kL++ )
1067                                         {
1068                                                 sumL += ( ( int32 ) *ptr1L++ * *ptr2L );
1069                                                 ptr2L += col2A;
1070                                         }
1071                                         *rA++ = ( sumL + ( 1 << 14 ) ) >> 15; /* round result to 1.15 */
1072                                 }
1073                         }
1074                 }
1075                 else mmul( ( int16* ) x1A, row1A, col1A, ( int16* ) x2A, col1A, col2A, rA );
1076         
1077         #elif defined( HW_ARMv4 ) || defined( HW_ARMv5TE )
1078
1079                 int32 iL, jL, kL;
1080                 int16 *ptr1L, *ptr2L;
1081                 int32 sumL;
1082                 for( iL = 0; iL < row1A; iL++ )
1083                 {
1084                         for( jL = 0; jL < col2A; jL++ )
1085                         {
1086                                 ptr1L = ( int16* ) x1A + iL * col1A;
1087                                 ptr2L = ( int16* ) x2A + jL;
1088                                 sumL = 0;
1089                                 for( kL = col1A; kL >= 4; kL -= 4 )
1090                                 {
1091                                         sumL += ( ( int32 ) *ptr1L++ * *ptr2L );
1092                                         sumL += ( ( int32 ) *ptr1L++ * *( ptr2L += col2A ) );
1093                                         sumL += ( ( int32 ) *ptr1L++ * *( ptr2L += col2A ) );
1094                                         sumL += ( ( int32 ) *ptr1L++ * *( ptr2L += col2A ) );
1095                                         ptr2L += col2A;
1096                                 }
1097                                 for( ; kL > 0; kL-- )
1098                                 {
1099                                         sumL += ( ( int32 ) *ptr1L++ * *ptr2L );
1100                                         ptr2L += col2A;
1101                                 }
1102                                 *rA++ = ( sumL + ( 1 << 14 ) ) >> 15; /* round result to 1.15 */
1103                         }
1104                 }
1105         #else
1106                 /* needs to be optimized */
1107                 int16 iL,jL,kL;
1108                 int16 *ptr1L, *ptr2L;
1109                 int32 sumL;
1110                 
1111                 for( iL = 0; iL < row1A; iL++ )
1112                 {
1113                         for( jL = 0; jL < col2A; jL++ )
1114                         {
1115                                 ptr1L = ( int16* ) x1A + iL * col1A;
1116                                 ptr2L = ( int16* ) x2A + jL;
1117                                 sumL = 0;
1118                                 for( kL = 0; kL < col1A; kL++ )
1119                                 {
1120                                         sumL += ( ( int32 ) *ptr1L++ * *ptr2L );
1121                                         ptr2L += col2A;
1122                                 }
1123                                 *rA++ = ( sumL + ( 1 << 14 ) ) >> 15; /* round result to 1.15 */
1124                         }
1125                 }
1126         #endif
1127 }
1128
1129 /* ------------------------------------------------------------------------- */
1130
1131 void bbs_matMultiplyTranspFlt16( const int16 *x1A, int16 row1A, int16 col1A, 
1132                                                                  const int16 *x2A, int16 col2A, int16 *rA )
1133 {
1134         const int16* ptr1L = x1A;
1135
1136         int32 iL;
1137         for( iL = row1A; iL > 0 ; iL-- )
1138         {
1139                 int32 jL;
1140                 const int16* ptr2L = x2A;
1141                 for( jL = col2A; jL > 0 ; jL-- )
1142                 {
1143                         int32 kL;
1144                         int32 sumL = 0;
1145                         for( kL = col1A >> 2; kL > 0; kL-- )
1146                         {
1147                                 sumL += ( ( int32 ) *ptr1L++ * *ptr2L++ );
1148                                 sumL += ( ( int32 ) *ptr1L++ * *ptr2L++ );
1149                                 sumL += ( ( int32 ) *ptr1L++ * *ptr2L++ );
1150                                 sumL += ( ( int32 ) *ptr1L++ * *ptr2L++ );
1151                         }
1152                         for( kL = col1A & 3; kL > 0; kL-- )
1153                         {
1154                                 sumL += ( ( int32 ) *ptr1L++ * *ptr2L++ );
1155                         }
1156                         *rA++ = ( sumL + ( 1 << 14 ) ) >> 15; /* round result to 1.15 */
1157                         ptr1L -= col1A;
1158                 }
1159                 ptr1L += col1A;
1160         }
1161 }
1162
1163 /* ------------------------------------------------------------------------- */
1164
1165
1166 #ifndef mtrans
1167 uint16 bbs_matTrans( int16 *xA, int16 rowA, int16 colA, int16 *rA )
1168 {
1169         /* needs to be optimized */
1170         int16 iL;
1171         for( iL = colA; iL--; )
1172         {
1173                 int16* sL = xA++;
1174                 int16 jL;
1175                 for( jL = rowA; jL--; )
1176                 {
1177                         *rA++ = *sL;
1178                         sL += colA;
1179                 }
1180         }
1181         return 0;
1182 }
1183 #endif
1184
1185 /* ------------------------------------------------------------------------- */
1186 #ifndef atan2_16
1187 int16 bbs_atan2( int16 nomA, int16 denomA )
1188 {
1189         int16 phL, argL;        
1190         
1191         if( nomA == denomA ) return 8192;
1192         argL = ( ( int32 ) nomA << 15 ) / denomA;
1193         
1194         /* 0.318253*2 x      20857 .15
1195           +0.003314*2 x^2      217 .15
1196           -0.130908*2 x^3    -8580 .15
1197           +0.068542*2 x^4     4491 .15
1198           -0.009159*2 x^5     -600 .15 */       
1199
1200         phL = -600;
1201         phL = ( ( ( int32 ) phL * argL ) >> 15 ) + 4481;
1202         phL = ( ( ( int32 ) phL * argL ) >> 15 ) - 8580;
1203         phL = ( ( ( int32 ) phL * argL ) >> 15 ) + 217;
1204         phL = ( ( ( int32 ) phL * argL ) >> 15 ) + 20857;
1205         phL = ( ( int32 ) phL * argL ) >> 15;
1206
1207         return phL >> 1; /* /2 */
1208 }
1209
1210 /* needs to be optimized */
1211 uint16 bbs_vecPhase( int16 *reA, int16 *imA, int16 *phaseA, uint16 sizeA )
1212 {
1213         for( ; sizeA--; )
1214         {
1215                 int16 reL = *reA++;
1216                 int16 imL = *imA++;
1217                 int16 phL = 0;
1218                 
1219                 if( reL < 0 )
1220                 {
1221                         reL = -reL;
1222                         if( imL < 0 )
1223                         {
1224                                 imL = -imL;
1225                                 if( reL > imL ) 
1226                                 {
1227                                         phL = -32768 + bbs_atan2( imL, reL );
1228                                 }
1229                                 else
1230                                 {
1231                                         phL = -16384 - bbs_atan2( reL, imL );
1232                                 }
1233                         }
1234                         else
1235                         {
1236                                 if( reL > imL ) 
1237                                 {
1238                                         phL = -( -32768 + bbs_atan2( imL, reL ) );
1239                                 }
1240                                 else
1241                                 {
1242                                         if( imL == 0 ) phL = 0;
1243                                         else phL = 16384 + bbs_atan2( reL, imL );
1244                                 }
1245                         }
1246                 }
1247                 else
1248                 {
1249                         if( imL < 0 )
1250                         {
1251                                 imL = -imL;
1252                                 if( reL > imL ) 
1253                                 {
1254                                         phL = -bbs_atan2( imL, reL );
1255                                 }
1256                                 else
1257                                 {
1258                                         phL = -16384 + bbs_atan2( reL, imL );
1259                                 }
1260                         }
1261                         else
1262                         {
1263                                 if( reL > imL ) 
1264                                 {
1265                                         phL = bbs_atan2( imL, reL );
1266                                 }
1267                                 else
1268                                 {
1269                                         if( imL == 0 ) phL = 0;
1270                                         else phL = 16384 - bbs_atan2( reL, imL );
1271                                 }
1272                         }
1273                 }
1274                 
1275                 *phaseA++ = phL;
1276         }
1277         return 0;
1278 }
1279
1280 #endif
1281
1282 /* ------------------------------------------------------------------------- */