src/pulsecore/svolume_sse.c

   1 /***
   2   This file is part of PulseAudio.
   3
   4   Copyright 2004-2006 Lennart Poettering
   5   Copyright 2009 Wim Taymans <wim.taymans@collabora.co.uk>
   6
   7   PulseAudio is free software; you can redistribute it and/or modify
   8   it under the terms of the GNU Lesser General Public License as published
   9   by the Free Software Foundation; either version 2.1 of the License,
  10   or (at your option) any later version.
  11
  12   PulseAudio is distributed in the hope that it will be useful, but
  13   WITHOUT ANY WARRANTY; without even the implied warranty of
  14   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  15   General Public License for more details.
  16
  17   You should have received a copy of the GNU Lesser General Public License
  18   along with PulseAudio; if not, write to the Free Software
  19   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
  20   USA.
  21 ***/
  22
  23 #ifdef HAVE_CONFIG_H
  24 #include <config.h>
  25 #endif
  26
  27 #include <pulse/timeval.h>
  28 #include <pulse/rtclock.h>
  29
  30 #include <pulsecore/random.h>
  31 #include <pulsecore/macro.h>
  32 #include <pulsecore/g711.h>
  33 #include <pulsecore/core-util.h>
  34
  35 #include "cpu-x86.h"
  36
  37 #include "sample-util.h"
  38 #include "endianmacros.h"
  39
  40 #if defined (__i386__) || defined (__amd64__)
  41
  42 #define VOLUME_32x16(s,v)                  /* .. |   vh  |   vl  | */                   \
  43       " pxor %%xmm4, %%xmm4          \n\t" /* .. |    0  |    0  | */                   \
  44       " punpcklwd %%xmm4, "#s"       \n\t" /* .. |    0  |   p0  | */                   \
  45       " pcmpgtw "#s", %%xmm4         \n\t" /* .. |    0  | s(p0) | */                   \
  46       " pand "#v", %%xmm4            \n\t" /* .. |    0  |  (vl) | */                   \
  47       " movdqa "#s", %%xmm5          \n\t"                                              \
  48       " pmulhuw "#v", "#s"           \n\t" /* .. |    0  | vl*p0 | */                   \
  49       " psubd %%xmm4, "#s"           \n\t" /* .. |    0  | vl*p0 | + sign correct */    \
  50       " psrld $16, "#v"              \n\t" /* .. |    0  |   vh  | */                   \
  51       " pmaddwd %%xmm5, "#v"         \n\t" /* .. |    p0 * vh    | */                   \
  52       " paddd "#s", "#v"             \n\t" /* .. |    p0 * v0    | */                   \
  53       " packssdw "#v", "#v"          \n\t" /* .. | p1*v1 | p0*v0 | */
  54
  55 #define MOD_ADD(a,b) \
  56       " add "#a", %3                 \n\t" /* channel += inc           */ \
  57       " mov %3, %4                   \n\t"                                \
  58       " sub "#b", %4                 \n\t" /* tmp = channel - channels */ \
  59       " cmovae %4, %3                \n\t" /* if (tmp >= 0) channel = tmp  */
  60
  61 /* swap 16 bits */
  62 #define SWAP_16(s) \
  63       " movdqa "#s", %%xmm4          \n\t" /* .. |  h  l |  */ \
  64       " psrlw $8, %%xmm4             \n\t" /* .. |  0  h |  */ \
  65       " psllw $8, "#s"               \n\t" /* .. |  l  0 |  */ \
  66       " por %%xmm4, "#s"             \n\t" /* .. |  l  h |  */
  67
  68 /* swap 2 registers 16 bits for better pairing */
  69 #define SWAP_16_2(s1,s2) \
  70       " movdqa "#s1", %%xmm4         \n\t" /* .. |  h  l |  */ \
  71       " movdqa "#s2", %%xmm5         \n\t"                     \
  72       " psrlw $8, %%xmm4             \n\t" /* .. |  0  h |  */ \
  73       " psrlw $8, %%xmm5             \n\t"                     \
  74       " psllw $8, "#s1"              \n\t" /* .. |  l  0 |  */ \
  75       " psllw $8, "#s2"              \n\t"                     \
  76       " por %%xmm4, "#s1"            \n\t" /* .. |  l  h |  */ \
  77       " por %%xmm5, "#s2"            \n\t"
  78
  79
  80 static int channel_overread_table[8] = {8,8,8,12,8,10,12,14};
  81
  82 static void
  83 pa_volume_s16ne_sse2 (int16_t *samples, int32_t *volumes, unsigned channels, unsigned length)
  84 {
  85     pa_reg_x86 channel, temp;
  86
  87     /* Channels must be at least 8 and always a multiple of the original number.
  88      * This is also the max amount we overread the volume array, which should
  89      * have enough padding. */
  90     if (channels < 8)
  91         channels = channel_overread_table[channels];
  92
  93     __asm__ __volatile__ (
  94         " xor %3, %3                    \n\t"
  95         " sar $1, %2                    \n\t" /* length /= sizeof (int16_t) */
  96
  97         " test $1, %2                   \n\t" /* check for odd samples */
  98         " je 2f                         \n\t"
  99
 100         " movd (%1, %3, 4), %%xmm0      \n\t" /* |  v0h  |  v0l  | */
 101         " movw (%0), %w4                \n\t" /*     ..  |   p0  | */
 102         " movd %4, %%xmm1               \n\t"
 103         VOLUME_32x16 (%%xmm1, %%xmm0)
 104         " movd %%xmm0, %4               \n\t" /*     ..  | p0*v0 | */
 105         " movw %w4, (%0)                \n\t"
 106         " add $2, %0                    \n\t"
 107         MOD_ADD ($1, %5)
 108
 109         "2:                             \n\t"
 110         " sar $1, %2                    \n\t" /* prepare for processing 2 samples at a time */
 111         " test $1, %2                   \n\t"
 112         " je 4f                         \n\t"
 113
 114         "3:                             \n\t" /* do samples in groups of 2 */
 115         " movq (%1, %3, 4), %%xmm0      \n\t" /* |  v1h  |  v1l  |  v0h  |  v0l  | */
 116         " movd (%0), %%xmm1             \n\t" /*              .. |   p1  |  p0   | */
 117         VOLUME_32x16 (%%xmm1, %%xmm0)
 118         " movd %%xmm0, (%0)             \n\t" /*              .. | p1*v1 | p0*v0 | */
 119         " add $4, %0                    \n\t"
 120         MOD_ADD ($2, %5)
 121
 122         "4:                             \n\t"
 123         " sar $1, %2                    \n\t" /* prepare for processing 4 samples at a time */
 124         " test $1, %2                   \n\t"
 125         " je 6f                         \n\t"
 126
 127         /* FIXME, we can do aligned access of the volume values if we can guarantee
 128          * that the array is 16 bytes aligned, we probably have to do the odd values
 129          * after this then. */
 130         "5:                             \n\t" /* do samples in groups of 4 */
 131         " movdqu (%1, %3, 4), %%xmm0    \n\t" /* |  v3h  |  v3l  ..  v0h  |  v0l  | */
 132         " movq (%0), %%xmm1             \n\t" /*              .. |   p3  ..  p0   | */
 133         VOLUME_32x16 (%%xmm1, %%xmm0)
 134         " movq %%xmm0, (%0)             \n\t" /*              .. | p3*v3 .. p0*v0 | */
 135         " add $8, %0                    \n\t"
 136         MOD_ADD ($4, %5)
 137
 138         "6:                             \n\t"
 139         " sar $1, %2                    \n\t" /* prepare for processing 8 samples at a time */
 140         " cmp $0, %2                    \n\t"
 141         " je 8f                         \n\t"
 142
 143         "7:                             \n\t" /* do samples in groups of 8 */
 144         " movdqu (%1, %3, 4), %%xmm0    \n\t" /* |  v3h  |  v3l  ..  v0h  |  v0l  | */
 145         " movdqu 16(%1, %3, 4), %%xmm2  \n\t" /* |  v7h  |  v7l  ..  v4h  |  v4l  | */
 146         " movq (%0), %%xmm1             \n\t" /*              .. |   p3  ..  p0   | */
 147         " movq 8(%0), %%xmm3            \n\t" /*              .. |   p7  ..  p4   | */
 148         VOLUME_32x16 (%%xmm1, %%xmm0)
 149         VOLUME_32x16 (%%xmm3, %%xmm2)
 150         " movq %%xmm0, (%0)             \n\t" /*              .. | p3*v3 .. p0*v0 | */
 151         " movq %%xmm2, 8(%0)            \n\t" /*              .. | p7*v7 .. p4*v4 | */
 152         " add $16, %0                   \n\t"
 153         MOD_ADD ($8, %5)
 154         " dec %2                        \n\t"
 155         " jne 7b                        \n\t"
 156         "8:                             \n\t"
 157
 158         : "+r" (samples), "+r" (volumes), "+r" (length), "=D" (channel), "=&r" (temp)
 159         : "rm" ((pa_reg_x86)channels)
 160         : "cc"
 161     );
 162 }
 163
 164 static void
 165 pa_volume_s16re_sse2 (int16_t *samples, int32_t *volumes, unsigned channels, unsigned length)
 166 {
 167     pa_reg_x86 channel, temp;
 168
 169     /* Channels must be at least 8 and always a multiple of the original number.
 170      * This is also the max amount we overread the volume array, which should
 171      * have enough padding. */
 172     if (channels < 8)
 173         channels = channel_overread_table[channels];
 174
 175     __asm__ __volatile__ (
 176         " xor %3, %3                    \n\t"
 177         " sar $1, %2                    \n\t" /* length /= sizeof (int16_t) */
 178
 179         " test $1, %2                   \n\t" /* check for odd samples */
 180         " je 2f                         \n\t"
 181
 182         " movd (%1, %3, 4), %%xmm0      \n\t" /* |  v0h  |  v0l  | */
 183         " movw (%0), %w4                \n\t" /*     ..  |   p0  | */
 184         " rorw $8, %w4                  \n\t"
 185         " movd %4, %%xmm1               \n\t"
 186         VOLUME_32x16 (%%xmm1, %%xmm0)
 187         " movd %%xmm0, %4               \n\t" /*     ..  | p0*v0 | */
 188         " rorw $8, %w4                  \n\t"
 189         " movw %w4, (%0)                \n\t"
 190         " add $2, %0                    \n\t"
 191         MOD_ADD ($1, %5)
 192
 193         "2:                             \n\t"
 194         " sar $1, %2                    \n\t" /* prepare for processing 2 samples at a time */
 195         " test $1, %2                   \n\t"
 196         " je 4f                         \n\t"
 197
 198         "3:                             \n\t" /* do samples in groups of 2 */
 199         " movq (%1, %3, 4), %%xmm0      \n\t" /* |  v1h  |  v1l  |  v0h  |  v0l  | */
 200         " movd (%0), %%xmm1             \n\t" /*              .. |   p1  |  p0   | */
 201         SWAP_16 (%%xmm1)
 202         VOLUME_32x16 (%%xmm1, %%xmm0)
 203         SWAP_16 (%%xmm0)
 204         " movd %%xmm0, (%0)             \n\t" /*              .. | p1*v1 | p0*v0 | */
 205         " add $4, %0                    \n\t"
 206         MOD_ADD ($2, %5)
 207
 208         "4:                             \n\t"
 209         " sar $1, %2                    \n\t" /* prepare for processing 4 samples at a time */
 210         " test $1, %2                   \n\t"
 211         " je 6f                         \n\t"
 212
 213         /* FIXME, we can do aligned access of the volume values if we can guarantee
 214          * that the array is 16 bytes aligned, we probably have to do the odd values
 215          * after this then. */
 216         "5:                             \n\t" /* do samples in groups of 4 */
 217         " movdqu (%1, %3, 4), %%xmm0    \n\t" /* |  v3h  |  v3l  ..  v0h  |  v0l  | */
 218         " movq (%0), %%xmm1             \n\t" /*              .. |   p3  ..  p0   | */
 219         SWAP_16 (%%xmm1)
 220         VOLUME_32x16 (%%xmm1, %%xmm0)
 221         SWAP_16 (%%xmm0)
 222         " movq %%xmm0, (%0)             \n\t" /*              .. | p3*v3 .. p0*v0 | */
 223         " add $8, %0                    \n\t"
 224         MOD_ADD ($4, %5)
 225
 226         "6:                             \n\t"
 227         " sar $1, %2                    \n\t" /* prepare for processing 8 samples at a time */
 228         " cmp $0, %2                    \n\t"
 229         " je 8f                         \n\t"
 230
 231         "7:                             \n\t" /* do samples in groups of 8 */
 232         " movdqu (%1, %3, 4), %%xmm0    \n\t" /* |  v3h  |  v3l  ..  v0h  |  v0l  | */
 233         " movdqu 16(%1, %3, 4), %%xmm2  \n\t" /* |  v7h  |  v7l  ..  v4h  |  v4l  | */
 234         " movq (%0), %%xmm1             \n\t" /*              .. |   p3  ..  p0   | */
 235         " movq 8(%0), %%xmm3            \n\t" /*              .. |   p7  ..  p4   | */
 236         SWAP_16_2 (%%xmm1, %%xmm3)
 237         VOLUME_32x16 (%%xmm1, %%xmm0)
 238         VOLUME_32x16 (%%xmm3, %%xmm2)
 239         SWAP_16_2 (%%xmm0, %%xmm2)
 240         " movq %%xmm0, (%0)             \n\t" /*              .. | p3*v3 .. p0*v0 | */
 241         " movq %%xmm2, 8(%0)            \n\t" /*              .. | p7*v7 .. p4*v4 | */
 242         " add $16, %0                   \n\t"
 243         MOD_ADD ($8, %5)
 244         " dec %2                        \n\t"
 245         " jne 7b                        \n\t"
 246         "8:                             \n\t"
 247
 248         : "+r" (samples), "+r" (volumes), "+r" (length), "=D" (channel), "=&r" (temp)
 249         : "rm" ((pa_reg_x86)channels)
 250         : "cc"
 251     );
 252 }
 253
 254 #undef RUN_TEST
 255
 256 #ifdef RUN_TEST
 257 #define CHANNELS 2
 258 #define SAMPLES 1021
 259 #define TIMES 1000
 260 #define PADDING 16
 261
 262 static void run_test (void) {
 263     int16_t samples[SAMPLES];
 264     int16_t samples_ref[SAMPLES];
 265     int16_t samples_orig[SAMPLES];
 266     int32_t volumes[CHANNELS + PADDING];
 267     int i, j, padding;
 268     pa_do_volume_func_t func;
 269     pa_usec_t start, stop;
 270
 271     func = pa_get_volume_func (PA_SAMPLE_S16NE);
 272
 273     printf ("checking SSE2 %zd\n", sizeof (samples));
 274
 275     pa_random (samples, sizeof (samples));
 276     memcpy (samples_ref, samples, sizeof (samples));
 277     memcpy (samples_orig, samples, sizeof (samples));
 278
 279     for (i = 0; i < CHANNELS; i++)
 280         volumes[i] = rand() >> 1;
 281     for (padding = 0; padding < PADDING; padding++, i++)
 282         volumes[i] = volumes[padding];
 283
 284     func (samples_ref, volumes, CHANNELS, sizeof (samples));
 285     pa_volume_s16ne_sse2 (samples, volumes, CHANNELS, sizeof (samples));
 286     for (i = 0; i < SAMPLES; i++) {
 287         if (samples[i] != samples_ref[i]) {
 288             printf ("%d: %04x != %04x (%04x * %04x)\n", i, samples[i], samples_ref[i],
 289                       samples_orig[i], volumes[i % CHANNELS]);
 290         }
 291     }
 292
 293     start = pa_rtclock_now();
 294     for (j = 0; j < TIMES; j++) {
 295         memcpy (samples, samples_orig, sizeof (samples));
 296         pa_volume_s16ne_sse2 (samples, volumes, CHANNELS, sizeof (samples));
 297     }
 298     stop = pa_rtclock_now();
 299     pa_log_info("SSE: %llu usec.", (long long unsigned int)(stop - start));
 300
 301     start = pa_rtclock_now();
 302     for (j = 0; j < TIMES; j++) {
 303         memcpy (samples_ref, samples_orig, sizeof (samples));
 304         func (samples_ref, volumes, CHANNELS, sizeof (samples));
 305     }
 306     stop = pa_rtclock_now();
 307     pa_log_info("ref: %llu usec.", (long long unsigned int)(stop - start));
 308
 309     pa_assert_se(memcmp(samples_ref, samples, sizeof(samples)) == 0);
 310 }
 311 #endif
 312 #endif /* defined (__i386__) || defined (__amd64__) */
 313
 314 void pa_volume_func_init_sse (pa_cpu_x86_flag_t flags) {
 315 #if defined (__i386__) || defined (__amd64__)
 316
 317 #ifdef RUN_TEST
 318     run_test ();
 319 #endif
 320
 321     if (flags & PA_CPU_X86_SSE2) {
 322         pa_log_info("Initialising SSE2 optimized functions.");
 323
 324         pa_set_volume_func (PA_SAMPLE_S16NE, (pa_do_volume_func_t) pa_volume_s16ne_sse2);
 325         pa_set_volume_func (PA_SAMPLE_S16RE, (pa_do_volume_func_t) pa_volume_s16re_sse2);
 326     }
 327 #endif /* defined (__i386__) || defined (__amd64__) */
 328 }