src/pulsecore/svolume_mmx.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 /* in s: 2 int16_t samples
  42  * in v: 2 int32_t volumes, fixed point 16:16
  43  * out s: contains scaled and clamped int16_t samples.
  44  *
  45  * We calculate the high 32 bits of a 32x16 multiply which we then
  46  * clamp to 16 bits. The calulcation is:
  47  *
  48  *  vl = (v & 0xffff)
  49  *  vh = (v >> 16)
  50  *  s = ((s * vl) >> 16) + (s * vh);
  51  *
  52  * For the first multiply we have to do a sign correction as we need to
  53  * multiply a signed int with an unsigned int. Hacker's delight 8-3 gives a
  54  * simple formula to correct the sign of the high word after the signed
  55  * multiply.
  56  */
  57 #define VOLUME_32x16(s,v)                  /* .. |   vh  |   vl  | */                   \
  58       " pxor  %%mm4, %%mm4           \n\t" /* .. |    0  |    0  | */                   \
  59       " punpcklwd %%mm4, "#s"        \n\t" /* .. |    0  |   p0  | */                   \
  60       " pcmpgtw "#v", %%mm4          \n\t" /* .. |    0  | s(vl) | */                   \
  61       " pand "#s", %%mm4             \n\t" /* .. |    0  |  (p0) |  (vl >> 15) & p */   \
  62       " movq "#s", %%mm5             \n\t"                                              \
  63       " pmulhw "#v", "#s"            \n\t" /* .. |    0  | vl*p0 | */                   \
  64       " paddw %%mm4, "#s"            \n\t" /* .. |    0  | vl*p0 | + sign correct */    \
  65       " pslld $16, "#s"              \n\t" /* .. | vl*p0 |   0   | */                   \
  66       " psrld $16, "#v"              \n\t" /* .. |    0  |   vh  | */                   \
  67       " psrad $16, "#s"              \n\t" /* .. |     vl*p0     | sign extend */       \
  68       " pmaddwd %%mm5, "#v"          \n\t" /* .. |    p0 * vh    | */                   \
  69       " paddd "#s", "#v"             \n\t" /* .. |    p0 * v0    | */                   \
  70       " packssdw "#v", "#v"          \n\t" /* .. | p1*v1 | p0*v0 | */
  71
  72 /* approximately advances %3 = (%3 + a) % b. This function requires that
  73  * a <= b. */
  74 #define MOD_ADD(a,b) \
  75       " add "#a", %3                 \n\t" \
  76       " mov %3, %4                   \n\t" \
  77       " sub "#b", %4                 \n\t" \
  78       " cmovae %4, %3                \n\t"
  79
  80 /* swap 16 bits */
  81 #define SWAP_16(s) \
  82       " movq "#s", %%mm4             \n\t" /* .. |  h  l |  */ \
  83       " psrlw $8, %%mm4              \n\t" /* .. |  0  h |  */ \
  84       " psllw $8, "#s"               \n\t" /* .. |  l  0 |  */ \
  85       " por %%mm4, "#s"              \n\t" /* .. |  l  h |  */
  86
  87 /* swap 2 registers 16 bits for better pairing */
  88 #define SWAP_16_2(s1,s2) \
  89       " movq "#s1", %%mm4            \n\t" /* .. |  h  l |  */ \
  90       " movq "#s2", %%mm5            \n\t"                     \
  91       " psrlw $8, %%mm4              \n\t" /* .. |  0  h |  */ \
  92       " psrlw $8, %%mm5              \n\t"                     \
  93       " psllw $8, "#s1"              \n\t" /* .. |  l  0 |  */ \
  94       " psllw $8, "#s2"              \n\t"                     \
  95       " por %%mm4, "#s1"             \n\t" /* .. |  l  h |  */ \
  96       " por %%mm5, "#s2"             \n\t"
  97
  98 static void pa_volume_s16ne_mmx(int16_t *samples, int32_t *volumes, unsigned channels, unsigned length) {
  99     pa_reg_x86 channel, temp;
 100
 101     /* Channels must be at least 4, and always a multiple of the original number.
 102      * This is also the max amount we overread the volume array, which should
 103      * have enough padding. */
 104     channels = channels == 3 ? 6 : PA_MAX (4U, channels);
 105
 106     __asm__ __volatile__ (
 107         " xor %3, %3                    \n\t"
 108         " sar $1, %2                    \n\t" /* length /= sizeof (int16_t) */
 109
 110         " test $1, %2                   \n\t" /* check for odd samples */
 111         " je 2f                         \n\t"
 112
 113         " movd (%1, %3, 4), %%mm0       \n\t" /* |  v0h  |  v0l  | */
 114         " movw (%0), %w4                \n\t" /*     ..  |  p0   | */
 115         " movd %4, %%mm1                \n\t"
 116         VOLUME_32x16 (%%mm1, %%mm0)
 117         " movd %%mm0, %4                \n\t" /*     ..  | p0*v0 | */
 118         " movw %w4, (%0)                \n\t"
 119         " add $2, %0                    \n\t"
 120         MOD_ADD ($1, %5)
 121
 122         "2:                             \n\t"
 123         " sar $1, %2                    \n\t" /* prepare for processing 2 samples at a time */
 124         " test $1, %2                   \n\t" /* check for odd samples */
 125         " je 4f                         \n\t"
 126
 127         "3:                             \n\t" /* do samples in groups of 2 */
 128         " movq (%1, %3, 4), %%mm0       \n\t" /* |  v1h  |  v1l  |  v0h  |  v0l  | */
 129         " movd (%0), %%mm1              \n\t" /*              .. |   p1  |  p0   | */
 130         VOLUME_32x16 (%%mm1, %%mm0)
 131         " movd %%mm0, (%0)              \n\t" /*              .. | p1*v1 | p0*v0 | */
 132         " add $4, %0                    \n\t"
 133         MOD_ADD ($2, %5)
 134
 135         "4:                             \n\t"
 136         " sar $1, %2                    \n\t" /* prepare for processing 4 samples at a time */
 137         " cmp $0, %2                    \n\t"
 138         " je 6f                         \n\t"
 139
 140         "5:                             \n\t" /* do samples in groups of 4 */
 141         " movq (%1, %3, 4), %%mm0       \n\t" /* |  v1h  |  v1l  |  v0h  |  v0l  | */
 142         " movq 8(%1, %3, 4), %%mm2      \n\t" /* |  v3h  |  v3l  |  v2h  |  v2l  | */
 143         " movd (%0), %%mm1              \n\t" /*              .. |   p1  |  p0   | */
 144         " movd 4(%0), %%mm3             \n\t" /*              .. |   p3  |  p2   | */
 145         VOLUME_32x16 (%%mm1, %%mm0)
 146         VOLUME_32x16 (%%mm3, %%mm2)
 147         " movd %%mm0, (%0)              \n\t" /*              .. | p1*v1 | p0*v0 | */
 148         " movd %%mm2, 4(%0)             \n\t" /*              .. | p3*v3 | p2*v2 | */
 149         " add $8, %0                    \n\t"
 150         MOD_ADD ($4, %5)
 151         " dec %2                        \n\t"
 152         " jne 5b                        \n\t"
 153
 154         "6:                             \n\t"
 155         " emms                          \n\t"
 156
 157         : "+r" (samples), "+r" (volumes), "+r" (length), "=D" ((pa_reg_x86)channel), "=&r" (temp)
 158         : "rm" ((pa_reg_x86)channels)
 159         : "cc"
 160     );
 161 }
 162
 163 static void pa_volume_s16re_mmx(int16_t *samples, int32_t *volumes, unsigned channels, unsigned length) {
 164     pa_reg_x86 channel, temp;
 165
 166     /* Channels must be at least 4, and always a multiple of the original number.
 167      * This is also the max amount we overread the volume array, which should
 168      * have enough padding. */
 169     channels = channels == 3 ? 6 : PA_MAX (4U, channels);
 170
 171     __asm__ __volatile__ (
 172         " xor %3, %3                    \n\t"
 173         " sar $1, %2                    \n\t" /* length /= sizeof (int16_t) */
 174         " pcmpeqw %%mm6, %%mm6          \n\t" /* .. |  ffff |  ffff | */
 175         " pcmpeqw %%mm7, %%mm7          \n\t" /* .. |  ffff |  ffff | */
 176         " pslld  $16, %%mm6             \n\t" /* .. |  ffff |     0 | */
 177         " psrld  $31, %%mm7             \n\t" /* .. |     0 |     1 | */
 178
 179         " test $1, %2                   \n\t" /* check for odd samples */
 180         " je 2f                         \n\t"
 181
 182         " movd (%1, %3, 4), %%mm0       \n\t" /* |  v0h  |  v0l  | */
 183         " movw (%0), %w4                \n\t" /*     ..  |  p0   | */
 184         " rorw $8, %w4                  \n\t"
 185         " movd %4, %%mm1                \n\t"
 186         VOLUME_32x16 (%%mm1, %%mm0)
 187         " movd %%mm0, %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" /* check for odd samples */
 196         " je 4f                         \n\t"
 197
 198         "3:                             \n\t" /* do samples in groups of 2 */
 199         " movq (%1, %3, 4), %%mm0       \n\t" /* |  v1h  |  v1l  |  v0h  |  v0l  | */
 200         " movd (%0), %%mm1              \n\t" /*              .. |   p1  |  p0   | */
 201         SWAP_16 (%%mm1)
 202         VOLUME_32x16 (%%mm1, %%mm0)
 203         SWAP_16 (%%mm0)
 204         " movd %%mm0, (%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         " cmp $0, %2                    \n\t"
 211         " je 6f                         \n\t"
 212
 213         "5:                             \n\t" /* do samples in groups of 4 */
 214         " movq (%1, %3, 4), %%mm0       \n\t" /* |  v1h  |  v1l  |  v0h  |  v0l  | */
 215         " movq 8(%1, %3, 4), %%mm2      \n\t" /* |  v3h  |  v3l  |  v2h  |  v2l  | */
 216         " movd (%0), %%mm1              \n\t" /*              .. |   p1  |  p0   | */
 217         " movd 4(%0), %%mm3             \n\t" /*              .. |   p3  |  p2   | */
 218         SWAP_16_2 (%%mm1, %%mm3)
 219         VOLUME_32x16 (%%mm1, %%mm0)
 220         VOLUME_32x16 (%%mm3, %%mm2)
 221         SWAP_16_2 (%%mm0, %%mm2)
 222         " movd %%mm0, (%0)              \n\t" /*              .. | p1*v1 | p0*v0 | */
 223         " movd %%mm2, 4(%0)             \n\t" /*              .. | p3*v3 | p2*v2 | */
 224         " add $8, %0                    \n\t"
 225         MOD_ADD ($4, %5)
 226         " dec %2                        \n\t"
 227         " jne 5b                        \n\t"
 228
 229         "6:                             \n\t"
 230         " emms                          \n\t"
 231
 232         : "+r" (samples), "+r" (volumes), "+r" (length), "=D" ((pa_reg_x86)channel), "=&r" (temp)
 233         : "rm" ((pa_reg_x86)channels)
 234         : "cc"
 235     );
 236 }
 237
 238 #undef RUN_TEST
 239
 240 #ifdef RUN_TEST
 241 #define CHANNELS 2
 242 #define SAMPLES 1022
 243 #define TIMES 1000
 244 #define PADDING 16
 245
 246 static void run_test(void) {
 247     int16_t samples[SAMPLES];
 248     int16_t samples_ref[SAMPLES];
 249     int16_t samples_orig[SAMPLES];
 250     int32_t volumes[CHANNELS + PADDING];
 251     int i, j, padding;
 252     pa_do_volume_func_t func;
 253     pa_usec_t start, stop;
 254
 255     func = pa_get_volume_func(PA_SAMPLE_S16NE);
 256
 257     printf("checking MMX %zd\n", sizeof(samples));
 258
 259     pa_random(samples, sizeof(samples));
 260     /* for (i = 0; i < SAMPLES; i++)
 261        samples[i] = -1; */
 262     memcpy(samples_ref, samples, sizeof(samples));
 263     memcpy(samples_orig, samples, sizeof(samples));
 264
 265     for (i = 0; i < CHANNELS; i++)
 266         volumes[i] = PA_CLAMP_VOLUME(rand() >> 1);
 267         /* volumes[i] = 0x0000ffff; */
 268     for (padding = 0; padding < PADDING; padding++, i++)
 269         volumes[i] = volumes[padding];
 270
 271     func(samples_ref, volumes, CHANNELS, sizeof(samples));
 272     pa_volume_s16ne_mmx(samples, volumes, CHANNELS, sizeof(samples));
 273     for (i = 0; i < SAMPLES; i++) {
 274         if (samples[i] != samples_ref[i]) {
 275             printf("%d: %04x != %04x (%04x * %08x)\n", i, samples[i], samples_ref[i],
 276                   samples_orig[i], volumes[i % CHANNELS]);
 277         }
 278     }
 279
 280     start = pa_rtclock_now();
 281     for (j = 0; j < TIMES; j++) {
 282         memcpy(samples, samples_orig, sizeof(samples));
 283         pa_volume_s16ne_mmx(samples, volumes, CHANNELS, sizeof(samples));
 284     }
 285     stop = pa_rtclock_now();
 286     pa_log_info("MMX: %llu usec.", (long long unsigned int)(stop - start));
 287
 288     start = pa_rtclock_now();
 289     for (j = 0; j < TIMES; j++) {
 290         memcpy(samples_ref, samples_orig, sizeof(samples));
 291         func(samples_ref, volumes, CHANNELS, sizeof(samples));
 292     }
 293     stop = pa_rtclock_now();
 294     pa_log_info("ref: %llu usec.", (long long unsigned int)(stop - start));
 295
 296     pa_assert_se(memcmp(samples_ref, samples, sizeof(samples)) == 0);
 297 }
 298 #endif
 299
 300 #endif /* defined (__i386__) || defined (__amd64__) */
 301
 302
 303 void pa_volume_func_init_mmx(pa_cpu_x86_flag_t flags) {
 304 #if defined (__i386__) || defined (__amd64__)
 305
 306 #ifdef RUN_TEST
 307     run_test();
 308 #endif
 309
 310     if ((flags & PA_CPU_X86_MMX) && (flags & PA_CPU_X86_CMOV)) {
 311         pa_log_info("Initialising MMX optimized functions.");
 312
 313         pa_set_volume_func(PA_SAMPLE_S16NE, (pa_do_volume_func_t) pa_volume_s16ne_mmx);
 314         pa_set_volume_func(PA_SAMPLE_S16RE, (pa_do_volume_func_t) pa_volume_s16re_mmx);
 315     }
 316 #endif /* defined (__i386__) || defined (__amd64__) */
 317 }