WebAudio: Optimize calculateNormalizationScale().
[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 namespace WebCore {
40
41 namespace VectorMath {
42
43 #if OS(DARWIN)
44 // On the Mac we use the highly optimized versions in Accelerate.framework
45 // In 32-bit mode (__ppc__ or __i386__) <Accelerate/Accelerate.h> includes <vecLib/vDSP_translate.h> which defines macros of the same name as
46 // our namespaced function names, so we must handle this case differently. Other architectures (64bit, ARM, etc.) do not include this header file.
47
48 void vsmul(const float* sourceP, int sourceStride, const float* scale, float* destP, int destStride, size_t framesToProcess)
49 {
50 #if defined(__ppc__) || defined(__i386__)
51     ::vsmul(sourceP, sourceStride, scale, destP, destStride, framesToProcess);
52 #else
53     vDSP_vsmul(sourceP, sourceStride, scale, destP, destStride, framesToProcess);
54 #endif
55 }
56
57 void vadd(const float* source1P, int sourceStride1, const float* source2P, int sourceStride2, float* destP, int destStride, size_t framesToProcess)
58 {
59 #if defined(__ppc__) || defined(__i386__)
60     ::vadd(source1P, sourceStride1, source2P, sourceStride2, destP, destStride, framesToProcess);
61 #else
62     vDSP_vadd(source1P, sourceStride1, source2P, sourceStride2, destP, destStride, framesToProcess);
63 #endif
64 }
65
66 void vmul(const float* source1P, int sourceStride1, const float* source2P, int sourceStride2, float* destP, int destStride, size_t framesToProcess)
67 {
68 #if defined(__ppc__) || defined(__i386__)
69     ::vmul(source1P, sourceStride1, source2P, sourceStride2, destP, destStride, framesToProcess);
70 #else
71     vDSP_vmul(source1P, sourceStride1, source2P, sourceStride2, destP, destStride, framesToProcess);
72 #endif
73 }
74
75 void zvmul(const float* real1P, const float* imag1P, const float* real2P, const float* imag2P, float* realDestP, float* imagDestP, size_t framesToProcess)
76 {
77     DSPSplitComplex sc1;
78     DSPSplitComplex sc2;
79     DSPSplitComplex dest;
80     sc1.realp = const_cast<float*>(real1P);
81     sc1.imagp = const_cast<float*>(imag1P);
82     sc2.realp = const_cast<float*>(real2P);
83     sc2.imagp = const_cast<float*>(imag2P);
84     dest.realp = realDestP;
85     dest.imagp = imagDestP;
86 #if defined(__ppc__) || defined(__i386__)
87     ::zvmul(&sc1, 1, &sc2, 1, &dest, 1, framesToProcess, 1);
88 #else
89     vDSP_zvmul(&sc1, 1, &sc2, 1, &dest, 1, framesToProcess, 1);
90 #endif
91 }
92
93 void vsma(const float* sourceP, int sourceStride, const float* scale, float* destP, int destStride, size_t framesToProcess)
94 {
95     vDSP_vsma(sourceP, sourceStride, scale, destP, destStride, destP, destStride, framesToProcess);
96 }
97
98 void vsvesq(const float* sourceP, int sourceStride, float* sumP, size_t framesToProcess)
99 {
100     vDSP_svesq(const_cast<float*>(sourceP), sourceStride, sumP, framesToProcess);
101 }
102 #else
103
104 void vsma(const float* sourceP, int sourceStride, const float* scale, float* destP, int destStride, size_t framesToProcess)
105 {
106     int n = framesToProcess;
107
108 #ifdef __SSE2__
109     if ((sourceStride == 1) && (destStride == 1)) {
110         float k = *scale;
111
112         // If the sourceP address is not 16-byte aligned, the first several frames (at most three) should be processed seperately.
113         while ((reinterpret_cast<uintptr_t>(sourceP) & 0x0F) && n) {
114             *destP += k * *sourceP;
115             sourceP++;
116             destP++;
117             n--;
118         }
119
120         // Now the sourceP address aligned and start to apply SSE.
121         int tailFrames = n % 4;
122         float* endP = destP + n - tailFrames;
123
124         __m128 pSource;
125         __m128 dest;
126         __m128 temp;
127         __m128 mScale = _mm_set_ps1(k);
128
129         bool destAligned = !(reinterpret_cast<uintptr_t>(destP) & 0x0F);
130
131 #define SSE2_MULT_ADD(loadInstr, storeInstr)        \
132             while (destP < endP)                    \
133             {                                       \
134                 pSource = _mm_load_ps(sourceP);     \
135                 temp = _mm_mul_ps(pSource, mScale); \
136                 dest = _mm_##loadInstr##_ps(destP); \
137                 dest = _mm_add_ps(dest, temp);      \
138                 _mm_##storeInstr##_ps(destP, dest); \
139                 sourceP += 4;                       \
140                 destP += 4;                         \
141             }
142
143         if (destAligned) 
144             SSE2_MULT_ADD(load, store)
145         else 
146             SSE2_MULT_ADD(loadu, storeu)
147
148         n = tailFrames;
149     }
150 #endif
151     while (n) {
152         *destP += *sourceP * *scale;
153         sourceP += sourceStride;
154         destP += destStride;
155         n--;
156     }
157 }
158
159 void vsmul(const float* sourceP, int sourceStride, const float* scale, float* destP, int destStride, size_t framesToProcess)
160 {
161 #ifdef __SSE2__
162     if ((sourceStride == 1) && (destStride == 1)) {
163         
164         int n = framesToProcess;
165         float k = *scale;
166
167         // If the sourceP address is not 16-byte aligned, the first several frames (at most three) should be processed seperately.
168         while ((reinterpret_cast<size_t>(sourceP) & 0x0F) && n) {
169             *destP = k * *sourceP;
170             sourceP++;
171             destP++;
172             n--;
173         }
174
175         // Now the sourceP address is aligned and start to apply SSE.
176         int group = n / 4;
177         __m128 mScale = _mm_set_ps1(k);
178         __m128* pSource;
179         __m128* pDest;
180         __m128 dest;
181
182
183         if (reinterpret_cast<size_t>(destP) & 0x0F) {
184             while (group--) {
185                 pSource = reinterpret_cast<__m128*>(const_cast<float*>(sourceP));
186                 dest = _mm_mul_ps(*pSource, mScale);
187                 _mm_storeu_ps(destP, dest);
188
189                 sourceP += 4;
190                 destP += 4;
191             }
192         } else {
193             while (group--) {
194                 pSource = reinterpret_cast<__m128*>(const_cast<float*>(sourceP));
195                 pDest = reinterpret_cast<__m128*>(destP);
196                 *pDest = _mm_mul_ps(*pSource, mScale);
197
198                 sourceP += 4;
199                 destP += 4;
200             }
201         }
202
203         // Non-SSE handling for remaining frames which is less than 4.
204         n %= 4;
205         while (n) {
206             *destP = k * *sourceP;
207             sourceP++;
208             destP++;
209             n--;
210         }
211     } else { // If strides are not 1, rollback to normal algorithm.
212 #endif
213     int n = framesToProcess;
214     float k = *scale;
215     while (n--) {
216         *destP = k * *sourceP;
217         sourceP += sourceStride;
218         destP += destStride;
219     }
220 #ifdef __SSE2__
221     }
222 #endif
223 }
224
225 void vadd(const float* source1P, int sourceStride1, const float* source2P, int sourceStride2, float* destP, int destStride, size_t framesToProcess)
226 {
227 #ifdef __SSE2__
228     if ((sourceStride1 ==1) && (sourceStride2 == 1) && (destStride == 1)) {
229
230         int n = framesToProcess;
231
232         // If the sourceP address is not 16-byte aligned, the first several frames (at most three) should be processed seperately.
233         while ((reinterpret_cast<size_t>(source1P) & 0x0F) && n) {
234             *destP = *source1P + *source2P;
235             source1P++;
236             source2P++;
237             destP++;
238             n--;
239         }
240
241         // Now the source1P address is aligned and start to apply SSE.
242         int group = n / 4;
243         __m128* pSource1;
244         __m128* pSource2;
245         __m128* pDest;
246         __m128 source2;
247         __m128 dest;
248
249         bool source2Aligned = !(reinterpret_cast<size_t>(source2P) & 0x0F);
250         bool destAligned = !(reinterpret_cast<size_t>(destP) & 0x0F);
251
252         if (source2Aligned && destAligned) { // all aligned
253             while (group--) {
254                 pSource1 = reinterpret_cast<__m128*>(const_cast<float*>(source1P));
255                 pSource2 = reinterpret_cast<__m128*>(const_cast<float*>(source2P));
256                 pDest = reinterpret_cast<__m128*>(destP);
257                 *pDest = _mm_add_ps(*pSource1, *pSource2);
258
259                 source1P += 4;
260                 source2P += 4;
261                 destP += 4;
262             }
263
264         } else if (source2Aligned && !destAligned) { // source2 aligned but dest not aligned 
265             while (group--) {
266                 pSource1 = reinterpret_cast<__m128*>(const_cast<float*>(source1P));
267                 pSource2 = reinterpret_cast<__m128*>(const_cast<float*>(source2P));
268                 dest = _mm_add_ps(*pSource1, *pSource2);
269                 _mm_storeu_ps(destP, dest);
270
271                 source1P += 4;
272                 source2P += 4;
273                 destP += 4;
274             }
275
276         } else if (!source2Aligned && destAligned) { // source2 not aligned but dest aligned 
277             while (group--) {
278                 pSource1 = reinterpret_cast<__m128*>(const_cast<float*>(source1P));
279                 source2 = _mm_loadu_ps(source2P);
280                 pDest = reinterpret_cast<__m128*>(destP);
281                 *pDest = _mm_add_ps(*pSource1, source2);
282
283                 source1P += 4;
284                 source2P += 4;
285                 destP += 4;
286             }
287         } else if (!source2Aligned && !destAligned) { // both source2 and dest not aligned 
288             while (group--) {
289                 pSource1 = reinterpret_cast<__m128*>(const_cast<float*>(source1P));
290                 source2 = _mm_loadu_ps(source2P);
291                 dest = _mm_add_ps(*pSource1, source2);
292                 _mm_storeu_ps(destP, dest);
293
294                 source1P += 4;
295                 source2P += 4;
296                 destP += 4;
297             }
298         }
299
300         // Non-SSE handling for remaining frames which is less than 4.
301         n %= 4;
302         while (n) {
303             *destP = *source1P + *source2P;
304             source1P++;
305             source2P++;
306             destP++;
307             n--;
308         }
309     } else { // if strides are not 1, rollback to normal algorithm
310 #endif
311     int n = framesToProcess;
312     while (n--) {
313         *destP = *source1P + *source2P;
314         source1P += sourceStride1;
315         source2P += sourceStride2;
316         destP += destStride;
317     }
318 #ifdef __SSE2__
319     }
320 #endif
321 }
322
323 void vmul(const float* source1P, int sourceStride1, const float* source2P, int sourceStride2, float* destP, int destStride, size_t framesToProcess)
324 {
325
326     int n = framesToProcess;
327
328 #ifdef __SSE2__
329     if ((sourceStride1 == 1) && (sourceStride2 == 1) && (destStride == 1)) {
330
331         // If the source1P address is not 16-byte aligned, the first several frames (at most three) should be processed seperately.
332         while ((reinterpret_cast<uintptr_t>(source1P) & 0x0F) && n) {
333             *destP = *source1P * *source2P;
334             source1P++;
335             source2P++;
336             destP++;
337             n--;
338         }
339
340         // Now the source1P address aligned and start to apply SSE.
341         int tailFrames = n % 4;
342         float* endP = destP + n - tailFrames;
343         __m128 pSource1;
344         __m128 pSource2;
345         __m128 dest;
346
347         bool source2Aligned = !(reinterpret_cast<uintptr_t>(source2P) & 0x0F);
348         bool destAligned = !(reinterpret_cast<uintptr_t>(destP) & 0x0F);
349
350 #define SSE2_MULT(loadInstr, storeInstr)                   \
351             while (destP < endP)                           \
352             {                                              \
353                 pSource1 = _mm_load_ps(source1P);          \
354                 pSource2 = _mm_##loadInstr##_ps(source2P); \
355                 dest = _mm_mul_ps(pSource1, pSource2);     \
356                 _mm_##storeInstr##_ps(destP, dest);        \
357                 source1P += 4;                             \
358                 source2P += 4;                             \
359                 destP += 4;                                \
360             }
361
362         if (source2Aligned && destAligned) // Both aligned.
363             SSE2_MULT(load, store)
364         else if (source2Aligned && !destAligned) // Source2 is aligned but dest not.
365             SSE2_MULT(load, storeu)
366         else if (!source2Aligned && destAligned) // Dest is aligned but source2 not.
367             SSE2_MULT(loadu, store)
368         else // Neither aligned.
369             SSE2_MULT(loadu, storeu)
370
371         n = tailFrames;
372     }
373 #endif
374     while (n) {
375         *destP = *source1P * *source2P;
376         source1P += sourceStride1;
377         source2P += sourceStride2;
378         destP += destStride;
379         n--;
380     }
381 }
382
383 void zvmul(const float* real1P, const float* imag1P, const float* real2P, const float* imag2P, float* realDestP, float* imagDestP, size_t framesToProcess)
384 {
385     unsigned i = 0;
386 #ifdef __SSE2__
387     // Only use the SSE optimization in the very common case that all addresses are 16-byte aligned. 
388     // Otherwise, fall through to the scalar code below.
389     if (!(reinterpret_cast<uintptr_t>(real1P) & 0x0F)
390         && !(reinterpret_cast<uintptr_t>(imag1P) & 0x0F)
391         && !(reinterpret_cast<uintptr_t>(real2P) & 0x0F)
392         && !(reinterpret_cast<uintptr_t>(imag2P) & 0x0F)
393         && !(reinterpret_cast<uintptr_t>(realDestP) & 0x0F)
394         && !(reinterpret_cast<uintptr_t>(imagDestP) & 0x0F)) {
395         
396         unsigned endSize = framesToProcess - framesToProcess % 4;
397         while (i < endSize) {
398             __m128 real1 = _mm_load_ps(real1P + i);
399             __m128 real2 = _mm_load_ps(real2P + i);
400             __m128 imag1 = _mm_load_ps(imag1P + i);
401             __m128 imag2 = _mm_load_ps(imag2P + i);
402             __m128 real = _mm_mul_ps(real1, real2);
403             real = _mm_sub_ps(real, _mm_mul_ps(imag1, imag2));
404             __m128 imag = _mm_mul_ps(real1, imag2);
405             imag = _mm_add_ps(imag, _mm_mul_ps(imag1, real2));
406             _mm_store_ps(realDestP + i, real);
407             _mm_store_ps(imagDestP + i, imag);
408             i += 4;
409         }
410     }
411 #endif
412     for (; i < framesToProcess; ++i) {
413         realDestP[i] = real1P[i] * real2P[i] - imag1P[i] * imag2P[i];
414         imagDestP[i] = real1P[i] * imag2P[i] + imag1P[i] * real2P[i];
415     }
416 }
417
418 void vsvesq(const float* sourceP, int sourceStride, float* sumP, size_t framesToProcess)
419 {
420     // FIXME: optimize for SSE
421     int n = framesToProcess;
422     float sum = 0;
423     while (n--) {
424         float sample = *sourceP;
425         sum += sample * sample;
426         sourceP += sourceStride;
427     }
428
429     ASSERT(sumP);
430     *sumP = sum;
431 }
432 #endif // OS(DARWIN)
433
434 } // namespace VectorMath
435
436 } // namespace WebCore
437
438 #endif // ENABLE(WEB_AUDIO)