60be4e0c1d151ea84b62f0b6ea8f32e818790bf8
[WebKit-https.git] / Source / WebCore / platform / audio / VectorMath.cpp
1 /*
2  * Copyright (C) 2010, Google Inc. All rights reserved.
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  * 1.  Redistributions of source code must retain the above copyright
8  *    notice, this list of conditions and the following disclaimer.
9  * 2.  Redistributions in binary form must reproduce the above copyright
10  *    notice, this list of conditions and the following disclaimer in the
11  *    documentation and/or other materials provided with the distribution.
12  *
13  * THIS SOFTWARE IS PROVIDED BY APPLE INC. AND ITS CONTRIBUTORS ``AS IS'' AND ANY
14  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
15  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
16  * DISCLAIMED. IN NO EVENT SHALL APPLE INC. OR ITS CONTRIBUTORS BE LIABLE FOR ANY
17  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
18  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
19  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
20  * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
21  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
22  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23  */
24
25 #include "config.h"
26
27 #if ENABLE(WEB_AUDIO)
28
29 #include "VectorMath.h"
30
31 #if OS(DARWIN)
32 #include <Accelerate/Accelerate.h>
33 #endif
34
35 #ifdef __SSE2__
36 #include <emmintrin.h>
37 #endif
38
39 #include <algorithm>
40 #include <math.h>
41
42 namespace WebCore {
43
44 namespace VectorMath {
45
46 #if OS(DARWIN)
47 // On the Mac we use the highly optimized versions in Accelerate.framework
48 // In 32-bit mode (__ppc__ or __i386__) <Accelerate/Accelerate.h> includes <vecLib/vDSP_translate.h> which defines macros of the same name as
49 // our namespaced function names, so we must handle this case differently. Other architectures (64bit, ARM, etc.) do not include this header file.
50
51 void vsmul(const float* sourceP, int sourceStride, const float* scale, float* destP, int destStride, size_t framesToProcess)
52 {
53 #if defined(__ppc__) || defined(__i386__)
54     ::vsmul(sourceP, sourceStride, scale, destP, destStride, framesToProcess);
55 #else
56     vDSP_vsmul(sourceP, sourceStride, scale, destP, destStride, framesToProcess);
57 #endif
58 }
59
60 void vadd(const float* source1P, int sourceStride1, const float* source2P, int sourceStride2, float* destP, int destStride, size_t framesToProcess)
61 {
62 #if defined(__ppc__) || defined(__i386__)
63     ::vadd(source1P, sourceStride1, source2P, sourceStride2, destP, destStride, framesToProcess);
64 #else
65     vDSP_vadd(source1P, sourceStride1, source2P, sourceStride2, destP, destStride, framesToProcess);
66 #endif
67 }
68
69 void vmul(const float* source1P, int sourceStride1, const float* source2P, int sourceStride2, float* destP, int destStride, size_t framesToProcess)
70 {
71 #if defined(__ppc__) || defined(__i386__)
72     ::vmul(source1P, sourceStride1, source2P, sourceStride2, destP, destStride, framesToProcess);
73 #else
74     vDSP_vmul(source1P, sourceStride1, source2P, sourceStride2, destP, destStride, framesToProcess);
75 #endif
76 }
77
78 void zvmul(const float* real1P, const float* imag1P, const float* real2P, const float* imag2P, float* realDestP, float* imagDestP, size_t framesToProcess)
79 {
80     DSPSplitComplex sc1;
81     DSPSplitComplex sc2;
82     DSPSplitComplex dest;
83     sc1.realp = const_cast<float*>(real1P);
84     sc1.imagp = const_cast<float*>(imag1P);
85     sc2.realp = const_cast<float*>(real2P);
86     sc2.imagp = const_cast<float*>(imag2P);
87     dest.realp = realDestP;
88     dest.imagp = imagDestP;
89 #if defined(__ppc__) || defined(__i386__)
90     ::zvmul(&sc1, 1, &sc2, 1, &dest, 1, framesToProcess, 1);
91 #else
92     vDSP_zvmul(&sc1, 1, &sc2, 1, &dest, 1, framesToProcess, 1);
93 #endif
94 }
95
96 void vsma(const float* sourceP, int sourceStride, const float* scale, float* destP, int destStride, size_t framesToProcess)
97 {
98     vDSP_vsma(sourceP, sourceStride, scale, destP, destStride, destP, destStride, framesToProcess);
99 }
100
101 void vmaxmgv(const float* sourceP, int sourceStride, float* maxP, size_t framesToProcess)
102 {
103     vDSP_maxmgv(sourceP, sourceStride, maxP, framesToProcess);
104 }
105
106 void vsvesq(const float* sourceP, int sourceStride, float* sumP, size_t framesToProcess)
107 {
108     vDSP_svesq(const_cast<float*>(sourceP), sourceStride, sumP, framesToProcess);
109 }
110 #else
111
112 void vsma(const float* sourceP, int sourceStride, const float* scale, float* destP, int destStride, size_t framesToProcess)
113 {
114     int n = framesToProcess;
115
116 #ifdef __SSE2__
117     if ((sourceStride == 1) && (destStride == 1)) {
118         float k = *scale;
119
120         // If the sourceP address is not 16-byte aligned, the first several frames (at most three) should be processed separately.
121         while ((reinterpret_cast<uintptr_t>(sourceP) & 0x0F) && n) {
122             *destP += k * *sourceP;
123             sourceP++;
124             destP++;
125             n--;
126         }
127
128         // Now the sourceP is aligned, use SSE.
129         int tailFrames = n % 4;
130         const float* endP = destP + n - tailFrames;
131
132         __m128 pSource;
133         __m128 dest;
134         __m128 temp;
135         __m128 mScale = _mm_set_ps1(k);
136
137         bool destAligned = !(reinterpret_cast<uintptr_t>(destP) & 0x0F);
138
139 #define SSE2_MULT_ADD(loadInstr, storeInstr)        \
140             while (destP < endP)                    \
141             {                                       \
142                 pSource = _mm_load_ps(sourceP);     \
143                 temp = _mm_mul_ps(pSource, mScale); \
144                 dest = _mm_##loadInstr##_ps(destP); \
145                 dest = _mm_add_ps(dest, temp);      \
146                 _mm_##storeInstr##_ps(destP, dest); \
147                 sourceP += 4;                       \
148                 destP += 4;                         \
149             }
150
151         if (destAligned) 
152             SSE2_MULT_ADD(load, store)
153         else 
154             SSE2_MULT_ADD(loadu, storeu)
155
156         n = tailFrames;
157     }
158 #endif
159     while (n) {
160         *destP += *sourceP * *scale;
161         sourceP += sourceStride;
162         destP += destStride;
163         n--;
164     }
165 }
166
167 void vsmul(const float* sourceP, int sourceStride, const float* scale, float* destP, int destStride, size_t framesToProcess)
168 {
169 #ifdef __SSE2__
170     if ((sourceStride == 1) && (destStride == 1)) {
171         
172         int n = framesToProcess;
173         float k = *scale;
174
175         // If the sourceP address is not 16-byte aligned, the first several frames (at most three) should be processed separately.
176         while ((reinterpret_cast<size_t>(sourceP) & 0x0F) && n) {
177             *destP = k * *sourceP;
178             sourceP++;
179             destP++;
180             n--;
181         }
182
183         // Now the sourceP address is aligned and start to apply SSE.
184         int group = n / 4;
185         __m128 mScale = _mm_set_ps1(k);
186         __m128* pSource;
187         __m128* pDest;
188         __m128 dest;
189
190
191         if (reinterpret_cast<size_t>(destP) & 0x0F) {
192             while (group--) {
193                 pSource = reinterpret_cast<__m128*>(const_cast<float*>(sourceP));
194                 dest = _mm_mul_ps(*pSource, mScale);
195                 _mm_storeu_ps(destP, dest);
196
197                 sourceP += 4;
198                 destP += 4;
199             }
200         } else {
201             while (group--) {
202                 pSource = reinterpret_cast<__m128*>(const_cast<float*>(sourceP));
203                 pDest = reinterpret_cast<__m128*>(destP);
204                 *pDest = _mm_mul_ps(*pSource, mScale);
205
206                 sourceP += 4;
207                 destP += 4;
208             }
209         }
210
211         // Non-SSE handling for remaining frames which is less than 4.
212         n %= 4;
213         while (n) {
214             *destP = k * *sourceP;
215             sourceP++;
216             destP++;
217             n--;
218         }
219     } else { // If strides are not 1, rollback to normal algorithm.
220 #endif
221     int n = framesToProcess;
222     float k = *scale;
223     while (n--) {
224         *destP = k * *sourceP;
225         sourceP += sourceStride;
226         destP += destStride;
227     }
228 #ifdef __SSE2__
229     }
230 #endif
231 }
232
233 void vadd(const float* source1P, int sourceStride1, const float* source2P, int sourceStride2, float* destP, int destStride, size_t framesToProcess)
234 {
235 #ifdef __SSE2__
236     if ((sourceStride1 ==1) && (sourceStride2 == 1) && (destStride == 1)) {
237
238         int n = framesToProcess;
239
240         // If the sourceP address is not 16-byte aligned, the first several frames (at most three) should be processed separately.
241         while ((reinterpret_cast<size_t>(source1P) & 0x0F) && n) {
242             *destP = *source1P + *source2P;
243             source1P++;
244             source2P++;
245             destP++;
246             n--;
247         }
248
249         // Now the source1P address is aligned and start to apply SSE.
250         int group = n / 4;
251         __m128* pSource1;
252         __m128* pSource2;
253         __m128* pDest;
254         __m128 source2;
255         __m128 dest;
256
257         bool source2Aligned = !(reinterpret_cast<size_t>(source2P) & 0x0F);
258         bool destAligned = !(reinterpret_cast<size_t>(destP) & 0x0F);
259
260         if (source2Aligned && destAligned) { // all aligned
261             while (group--) {
262                 pSource1 = reinterpret_cast<__m128*>(const_cast<float*>(source1P));
263                 pSource2 = reinterpret_cast<__m128*>(const_cast<float*>(source2P));
264                 pDest = reinterpret_cast<__m128*>(destP);
265                 *pDest = _mm_add_ps(*pSource1, *pSource2);
266
267                 source1P += 4;
268                 source2P += 4;
269                 destP += 4;
270             }
271
272         } else if (source2Aligned && !destAligned) { // source2 aligned but dest not aligned 
273             while (group--) {
274                 pSource1 = reinterpret_cast<__m128*>(const_cast<float*>(source1P));
275                 pSource2 = reinterpret_cast<__m128*>(const_cast<float*>(source2P));
276                 dest = _mm_add_ps(*pSource1, *pSource2);
277                 _mm_storeu_ps(destP, dest);
278
279                 source1P += 4;
280                 source2P += 4;
281                 destP += 4;
282             }
283
284         } else if (!source2Aligned && destAligned) { // source2 not aligned but dest aligned 
285             while (group--) {
286                 pSource1 = reinterpret_cast<__m128*>(const_cast<float*>(source1P));
287                 source2 = _mm_loadu_ps(source2P);
288                 pDest = reinterpret_cast<__m128*>(destP);
289                 *pDest = _mm_add_ps(*pSource1, source2);
290
291                 source1P += 4;
292                 source2P += 4;
293                 destP += 4;
294             }
295         } else if (!source2Aligned && !destAligned) { // both source2 and dest not aligned 
296             while (group--) {
297                 pSource1 = reinterpret_cast<__m128*>(const_cast<float*>(source1P));
298                 source2 = _mm_loadu_ps(source2P);
299                 dest = _mm_add_ps(*pSource1, source2);
300                 _mm_storeu_ps(destP, dest);
301
302                 source1P += 4;
303                 source2P += 4;
304                 destP += 4;
305             }
306         }
307
308         // Non-SSE handling for remaining frames which is less than 4.
309         n %= 4;
310         while (n) {
311             *destP = *source1P + *source2P;
312             source1P++;
313             source2P++;
314             destP++;
315             n--;
316         }
317     } else { // if strides are not 1, rollback to normal algorithm
318 #endif
319     int n = framesToProcess;
320     while (n--) {
321         *destP = *source1P + *source2P;
322         source1P += sourceStride1;
323         source2P += sourceStride2;
324         destP += destStride;
325     }
326 #ifdef __SSE2__
327     }
328 #endif
329 }
330
331 void vmul(const float* source1P, int sourceStride1, const float* source2P, int sourceStride2, float* destP, int destStride, size_t framesToProcess)
332 {
333
334     int n = framesToProcess;
335
336 #ifdef __SSE2__
337     if ((sourceStride1 == 1) && (sourceStride2 == 1) && (destStride == 1)) {
338         // If the source1P address is not 16-byte aligned, the first several frames (at most three) should be processed separately.
339         while ((reinterpret_cast<uintptr_t>(source1P) & 0x0F) && n) {
340             *destP = *source1P * *source2P;
341             source1P++;
342             source2P++;
343             destP++;
344             n--;
345         }
346
347         // Now the source1P address aligned and start to apply SSE.
348         int tailFrames = n % 4;
349         const float* endP = destP + n - tailFrames;
350         __m128 pSource1;
351         __m128 pSource2;
352         __m128 dest;
353
354         bool source2Aligned = !(reinterpret_cast<uintptr_t>(source2P) & 0x0F);
355         bool destAligned = !(reinterpret_cast<uintptr_t>(destP) & 0x0F);
356
357 #define SSE2_MULT(loadInstr, storeInstr)                   \
358             while (destP < endP)                           \
359             {                                              \
360                 pSource1 = _mm_load_ps(source1P);          \
361                 pSource2 = _mm_##loadInstr##_ps(source2P); \
362                 dest = _mm_mul_ps(pSource1, pSource2);     \
363                 _mm_##storeInstr##_ps(destP, dest);        \
364                 source1P += 4;                             \
365                 source2P += 4;                             \
366                 destP += 4;                                \
367             }
368
369         if (source2Aligned && destAligned) // Both aligned.
370             SSE2_MULT(load, store)
371         else if (source2Aligned && !destAligned) // Source2 is aligned but dest not.
372             SSE2_MULT(load, storeu)
373         else if (!source2Aligned && destAligned) // Dest is aligned but source2 not.
374             SSE2_MULT(loadu, store)
375         else // Neither aligned.
376             SSE2_MULT(loadu, storeu)
377
378         n = tailFrames;
379     }
380 #endif
381     while (n) {
382         *destP = *source1P * *source2P;
383         source1P += sourceStride1;
384         source2P += sourceStride2;
385         destP += destStride;
386         n--;
387     }
388 }
389
390 void zvmul(const float* real1P, const float* imag1P, const float* real2P, const float* imag2P, float* realDestP, float* imagDestP, size_t framesToProcess)
391 {
392     unsigned i = 0;
393 #ifdef __SSE2__
394     // Only use the SSE optimization in the very common case that all addresses are 16-byte aligned. 
395     // Otherwise, fall through to the scalar code below.
396     if (!(reinterpret_cast<uintptr_t>(real1P) & 0x0F)
397         && !(reinterpret_cast<uintptr_t>(imag1P) & 0x0F)
398         && !(reinterpret_cast<uintptr_t>(real2P) & 0x0F)
399         && !(reinterpret_cast<uintptr_t>(imag2P) & 0x0F)
400         && !(reinterpret_cast<uintptr_t>(realDestP) & 0x0F)
401         && !(reinterpret_cast<uintptr_t>(imagDestP) & 0x0F)) {
402         
403         unsigned endSize = framesToProcess - framesToProcess % 4;
404         while (i < endSize) {
405             __m128 real1 = _mm_load_ps(real1P + i);
406             __m128 real2 = _mm_load_ps(real2P + i);
407             __m128 imag1 = _mm_load_ps(imag1P + i);
408             __m128 imag2 = _mm_load_ps(imag2P + i);
409             __m128 real = _mm_mul_ps(real1, real2);
410             real = _mm_sub_ps(real, _mm_mul_ps(imag1, imag2));
411             __m128 imag = _mm_mul_ps(real1, imag2);
412             imag = _mm_add_ps(imag, _mm_mul_ps(imag1, real2));
413             _mm_store_ps(realDestP + i, real);
414             _mm_store_ps(imagDestP + i, imag);
415             i += 4;
416         }
417     }
418 #endif
419     for (; i < framesToProcess; ++i) {
420         // Read and compute result before storing them, in case the
421         // destination is the same as one of the sources.
422         float realResult = real1P[i] * real2P[i] - imag1P[i] * imag2P[i];
423         float imagResult = real1P[i] * imag2P[i] + imag1P[i] * real2P[i];
424
425         realDestP[i] = realResult;
426         imagDestP[i] = imagResult;
427     }
428 }
429
430 void vsvesq(const float* sourceP, int sourceStride, float* sumP, size_t framesToProcess)
431 {
432     int n = framesToProcess;
433     float sum = 0;
434
435 #ifdef __SSE2__ 
436     if (sourceStride == 1) { 
437         // If the sourceP address is not 16-byte aligned, the first several frames (at most three) should be processed separately. 
438         while ((reinterpret_cast<uintptr_t>(sourceP) & 0x0F) && n) { 
439             float sample = *sourceP; 
440             sum += sample * sample; 
441             sourceP++; 
442             n--; 
443         } 
444  
445         // Now the sourceP is aligned, use SSE.
446         int tailFrames = n % 4; 
447         const float* endP = sourceP + n - tailFrames; 
448         __m128 source; 
449         __m128 mSum = _mm_setzero_ps(); 
450  
451         while (sourceP < endP) { 
452             source = _mm_load_ps(sourceP); 
453             source = _mm_mul_ps(source, source); 
454             mSum = _mm_add_ps(mSum, source); 
455             sourceP += 4; 
456         } 
457  
458         // Summarize the SSE results. 
459         const float* groupSumP = reinterpret_cast<float*>(&mSum); 
460         sum += groupSumP[0] + groupSumP[1] + groupSumP[2] + groupSumP[3]; 
461  
462         n = tailFrames; 
463     } 
464 #endif
465
466     while (n--) {
467         float sample = *sourceP;
468         sum += sample * sample;
469         sourceP += sourceStride;
470     }
471
472     ASSERT(sumP);
473     *sumP = sum;
474 }
475
476 void vmaxmgv(const float* sourceP, int sourceStride, float* maxP, size_t framesToProcess)
477 {
478     int n = framesToProcess;
479     float max = 0;
480
481 #ifdef __SSE2__
482     if (sourceStride == 1) {
483         // If the sourceP address is not 16-byte aligned, the first several frames (at most three) should be processed separately.
484         while ((reinterpret_cast<uintptr_t>(sourceP) & 0x0F) && n) {
485             max = std::max(max, fabsf(*sourceP));
486             sourceP++;
487             n--;
488         }
489
490         // Now the sourceP is aligned, use SSE.
491         int tailFrames = n % 4;
492         const float* endP = sourceP + n - tailFrames;
493         __m128 source;
494         __m128 mMax = _mm_setzero_ps();
495         int mask = 0x7FFFFFFF;
496         __m128 mMask = _mm_set1_ps(*reinterpret_cast<float*>(&mask));
497
498         while (sourceP < endP) {
499             source = _mm_load_ps(sourceP);
500             // Calculate the absolute value by anding source with mask, the sign bit is set to 0.
501             source = _mm_and_ps(source, mMask);
502             mMax = _mm_max_ps(mMax, source);
503             sourceP += 4;
504         }
505
506         // Get max from the SSE results.
507         const float* groupMaxP = reinterpret_cast<float*>(&mMax);
508         max = std::max(max, groupMaxP[0]);
509         max = std::max(max, groupMaxP[1]);
510         max = std::max(max, groupMaxP[2]);
511         max = std::max(max, groupMaxP[3]);
512
513         n = tailFrames;
514     }
515 #endif
516
517     while (n--) {
518         max = std::max(max, fabsf(*sourceP));
519         sourceP += sourceStride;
520     }
521
522     ASSERT(maxP);
523     *maxP = max;
524 }
525 #endif // OS(DARWIN)
526
527 } // namespace VectorMath
528
529 } // namespace WebCore
530
531 #endif // ENABLE(WEB_AUDIO)