219298faa22c500ff0b06eec8ec47addd4591ffe
[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 #else
99
100 void vsma(const float* sourceP, int sourceStride, const float* scale, float* destP, int destStride, size_t framesToProcess)
101 {
102     int n = framesToProcess;
103
104 #ifdef __SSE2__
105     if ((sourceStride == 1) && (destStride == 1)) {
106         float k = *scale;
107
108         // If the sourceP address is not 16-byte aligned, the first several frames (at most three) should be processed seperately.
109         while ((reinterpret_cast<uintptr_t>(sourceP) & 0x0F) && n) {
110             *destP += k * *sourceP;
111             sourceP++;
112             destP++;
113             n--;
114         }
115
116         // Now the sourceP address aligned and start to apply SSE.
117         int tailFrames = n % 4;
118         float* endP = destP + n - tailFrames;
119
120         __m128 pSource;
121         __m128 dest;
122         __m128 temp;
123         __m128 mScale = _mm_set_ps1(k);
124
125         bool destAligned = !(reinterpret_cast<uintptr_t>(destP) & 0x0F);
126
127 #define SSE2_MULT_ADD(loadInstr, storeInstr)        \
128             while (destP < endP)                    \
129             {                                       \
130                 pSource = _mm_load_ps(sourceP);     \
131                 temp = _mm_mul_ps(pSource, mScale); \
132                 dest = _mm_##loadInstr##_ps(destP); \
133                 dest = _mm_add_ps(dest, temp);      \
134                 _mm_##storeInstr##_ps(destP, dest); \
135                 sourceP += 4;                       \
136                 destP += 4;                         \
137             }
138
139         if (destAligned) 
140             SSE2_MULT_ADD(load, store)
141         else 
142             SSE2_MULT_ADD(loadu, storeu)
143
144         n = tailFrames;
145     }
146 #endif
147     while (n) {
148         *destP += *sourceP * *scale;
149         sourceP += sourceStride;
150         destP += destStride;
151         n--;
152     }
153 }
154
155
156 void vsmul(const float* sourceP, int sourceStride, const float* scale, float* destP, int destStride, size_t framesToProcess)
157 {
158 #ifdef __SSE2__
159     if ((sourceStride == 1) && (destStride == 1)) {
160         
161         int n = framesToProcess;
162         float k = *scale;
163
164         // If the sourceP address is not 16-byte aligned, the first several frames (at most three) should be processed seperately.
165         while ((reinterpret_cast<size_t>(sourceP) & 0x0F) && n) {
166             *destP = k * *sourceP;
167             sourceP++;
168             destP++;
169             n--;
170         }
171
172         // Now the sourceP address is aligned and start to apply SSE.
173         int group = n / 4;
174         __m128 mScale = _mm_set_ps1(k);
175         __m128* pSource;
176         __m128* pDest;
177         __m128 dest;
178
179
180         if (reinterpret_cast<size_t>(destP) & 0x0F) {
181             while (group--) {
182                 pSource = reinterpret_cast<__m128*>(const_cast<float*>(sourceP));
183                 dest = _mm_mul_ps(*pSource, mScale);
184                 _mm_storeu_ps(destP, dest);
185
186                 sourceP += 4;
187                 destP += 4;
188             }
189         } else {
190             while (group--) {
191                 pSource = reinterpret_cast<__m128*>(const_cast<float*>(sourceP));
192                 pDest = reinterpret_cast<__m128*>(destP);
193                 *pDest = _mm_mul_ps(*pSource, mScale);
194
195                 sourceP += 4;
196                 destP += 4;
197             }
198         }
199
200         // Non-SSE handling for remaining frames which is less than 4.
201         n %= 4;
202         while (n) {
203             *destP = k * *sourceP;
204             sourceP++;
205             destP++;
206             n--;
207         }
208     } else { // If strides are not 1, rollback to normal algorithm.
209 #endif
210     int n = framesToProcess;
211     float k = *scale;
212     while (n--) {
213         *destP = k * *sourceP;
214         sourceP += sourceStride;
215         destP += destStride;
216     }
217 #ifdef __SSE2__
218     }
219 #endif
220 }
221
222 void vadd(const float* source1P, int sourceStride1, const float* source2P, int sourceStride2, float* destP, int destStride, size_t framesToProcess)
223 {
224 #ifdef __SSE2__
225     if ((sourceStride1 ==1) && (sourceStride2 == 1) && (destStride == 1)) {
226
227         int n = framesToProcess;
228
229         // If the sourceP address is not 16-byte aligned, the first several frames (at most three) should be processed seperately.
230         while ((reinterpret_cast<size_t>(source1P) & 0x0F) && n) {
231             *destP = *source1P + *source2P;
232             source1P++;
233             source2P++;
234             destP++;
235             n--;
236         }
237
238         // Now the source1P address is aligned and start to apply SSE.
239         int group = n / 4;
240         __m128* pSource1;
241         __m128* pSource2;
242         __m128* pDest;
243         __m128 source2;
244         __m128 dest;
245
246         bool source2Aligned = !(reinterpret_cast<size_t>(source2P) & 0x0F);
247         bool destAligned = !(reinterpret_cast<size_t>(destP) & 0x0F);
248
249         if (source2Aligned && destAligned) { // all aligned
250             while (group--) {
251                 pSource1 = reinterpret_cast<__m128*>(const_cast<float*>(source1P));
252                 pSource2 = reinterpret_cast<__m128*>(const_cast<float*>(source2P));
253                 pDest = reinterpret_cast<__m128*>(destP);
254                 *pDest = _mm_add_ps(*pSource1, *pSource2);
255
256                 source1P += 4;
257                 source2P += 4;
258                 destP += 4;
259             }
260
261         } else if (source2Aligned && !destAligned) { // source2 aligned but dest not aligned 
262             while (group--) {
263                 pSource1 = reinterpret_cast<__m128*>(const_cast<float*>(source1P));
264                 pSource2 = reinterpret_cast<__m128*>(const_cast<float*>(source2P));
265                 dest = _mm_add_ps(*pSource1, *pSource2);
266                 _mm_storeu_ps(destP, dest);
267
268                 source1P += 4;
269                 source2P += 4;
270                 destP += 4;
271             }
272
273         } else if (!source2Aligned && destAligned) { // source2 not aligned but dest aligned 
274             while (group--) {
275                 pSource1 = reinterpret_cast<__m128*>(const_cast<float*>(source1P));
276                 source2 = _mm_loadu_ps(source2P);
277                 pDest = reinterpret_cast<__m128*>(destP);
278                 *pDest = _mm_add_ps(*pSource1, source2);
279
280                 source1P += 4;
281                 source2P += 4;
282                 destP += 4;
283             }
284         } else if (!source2Aligned && !destAligned) { // both source2 and dest not aligned 
285             while (group--) {
286                 pSource1 = reinterpret_cast<__m128*>(const_cast<float*>(source1P));
287                 source2 = _mm_loadu_ps(source2P);
288                 dest = _mm_add_ps(*pSource1, source2);
289                 _mm_storeu_ps(destP, dest);
290
291                 source1P += 4;
292                 source2P += 4;
293                 destP += 4;
294             }
295         }
296
297         // Non-SSE handling for remaining frames which is less than 4.
298         n %= 4;
299         while (n) {
300             *destP = *source1P + *source2P;
301             source1P++;
302             source2P++;
303             destP++;
304             n--;
305         }
306     } else { // if strides are not 1, rollback to normal algorithm
307 #endif
308     int n = framesToProcess;
309     while (n--) {
310         *destP = *source1P + *source2P;
311         source1P += sourceStride1;
312         source2P += sourceStride2;
313         destP += destStride;
314     }
315 #ifdef __SSE2__
316     }
317 #endif
318 }
319
320 void vmul(const float* source1P, int sourceStride1, const float* source2P, int sourceStride2, float* destP, int destStride, size_t framesToProcess)
321 {
322
323     int n = framesToProcess;
324
325 #ifdef __SSE2__
326     if ((sourceStride1 == 1) && (sourceStride2 == 1) && (destStride == 1)) {
327
328         // If the source1P address is not 16-byte aligned, the first several frames (at most three) should be processed seperately.
329         while ((reinterpret_cast<uintptr_t>(source1P) & 0x0F) && n) {
330             *destP = *source1P * *source2P;
331             source1P++;
332             source2P++;
333             destP++;
334             n--;
335         }
336
337         // Now the source1P address aligned and start to apply SSE.
338         int tailFrames = n % 4;
339         float* endP = destP + n - tailFrames;
340         __m128 pSource1;
341         __m128 pSource2;
342         __m128 dest;
343
344         bool source2Aligned = !(reinterpret_cast<uintptr_t>(source2P) & 0x0F);
345         bool destAligned = !(reinterpret_cast<uintptr_t>(destP) & 0x0F);
346
347 #define SSE2_MULT(loadInstr, storeInstr)                   \
348             while (destP < endP)                           \
349             {                                              \
350                 pSource1 = _mm_load_ps(source1P);          \
351                 pSource2 = _mm_##loadInstr##_ps(source2P); \
352                 dest = _mm_mul_ps(pSource1, pSource2);     \
353                 _mm_##storeInstr##_ps(destP, dest);        \
354                 source1P += 4;                             \
355                 source2P += 4;                             \
356                 destP += 4;                                \
357             }
358
359         if (source2Aligned && destAligned) // Both aligned.
360             SSE2_MULT(load, store)
361         else if (source2Aligned && !destAligned) // Source2 is aligned but dest not.
362             SSE2_MULT(load, storeu)
363         else if (!source2Aligned && destAligned) // Dest is aligned but source2 not.
364             SSE2_MULT(loadu, store)
365         else // Neither aligned.
366             SSE2_MULT(loadu, storeu)
367
368         n = tailFrames;
369     }
370 #endif
371     while (n) {
372         *destP = *source1P * *source2P;
373         source1P += sourceStride1;
374         source2P += sourceStride2;
375         destP += destStride;
376         n--;
377     }
378 }
379
380 void zvmul(const float* real1P, const float* imag1P, const float* real2P, const float* imag2P, float* realDestP, float* imagDestP, size_t framesToProcess)
381 {
382     unsigned i = 0;
383 #ifdef __SSE2__
384     // Only use the SSE optimization in the very common case that all addresses are 16-byte aligned. 
385     // Otherwise, fall through to the scalar code below.
386     if (!(reinterpret_cast<uintptr_t>(real1P) & 0x0F)
387         && !(reinterpret_cast<uintptr_t>(imag1P) & 0x0F)
388         && !(reinterpret_cast<uintptr_t>(real2P) & 0x0F)
389         && !(reinterpret_cast<uintptr_t>(imag2P) & 0x0F)
390         && !(reinterpret_cast<uintptr_t>(realDestP) & 0x0F)
391         && !(reinterpret_cast<uintptr_t>(imagDestP) & 0x0F)) {
392         
393         unsigned endSize = framesToProcess - framesToProcess % 4;
394         while (i < endSize) {
395             __m128 real1 = _mm_load_ps(real1P + i);
396             __m128 real2 = _mm_load_ps(real2P + i);
397             __m128 imag1 = _mm_load_ps(imag1P + i);
398             __m128 imag2 = _mm_load_ps(imag2P + i);
399             __m128 real = _mm_mul_ps(real1, real2);
400             real = _mm_sub_ps(real, _mm_mul_ps(imag1, imag2));
401             __m128 imag = _mm_mul_ps(real1, imag2);
402             imag = _mm_add_ps(imag, _mm_mul_ps(imag1, real2));
403             _mm_store_ps(realDestP + i, real);
404             _mm_store_ps(imagDestP + i, imag);
405             i += 4;
406         }
407     }
408 #endif
409     for (; i < framesToProcess; ++i) {
410         realDestP[i] = real1P[i] * real2P[i] - imag1P[i] * imag2P[i];
411         imagDestP[i] = real1P[i] * imag2P[i] + imag1P[i] * real2P[i];
412     }
413 }
414
415 #endif // OS(DARWIN)
416
417 } // namespace VectorMath
418
419 } // namespace WebCore
420
421 #endif // ENABLE(WEB_AUDIO)