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
  99 pa_volume_s16ne_mmx (int16_t *samples, int32_t *volumes, unsigned channels, unsigned length)
 100 {
 101     pa_reg_x86 channel, temp;
 102
 103     /* Channels must be at least 4, and always a multiple of the original number.
 104      * This is also the max amount we overread the volume array, which should
 105      * have enough padding. */
 106     channels = channels == 3 ? 6 : PA_MAX (4U, channels);
 107
 108     __asm__ __volatile__ (
 109         " xor %3, %3                    \n\t"
 110         " sar $1, %2                    \n\t" /* length /= sizeof (int16_t) */
 111
 112         " test $1, %2                   \n\t" /* check for odd samples */
 113         " je 2f                         \n\t"
 114
 115         " movd (%1, %3, 4), %%mm0       \n\t" /* |  v0h  |  v0l  | */
 116         " movw (%0), %w4                \n\t" /*     ..  |  p0   | */
 117         " movd %4, %%mm1                \n\t"
 118         VOLUME_32x16 (%%mm1, %%mm0)
 119         " movd %%mm0, %4                \n\t" /*     ..  | p0*v0 | */
 120         " movw %w4, (%0)                \n\t"
 121         " add $2, %0                    \n\t"
 122         MOD_ADD ($1, %5)
 123
 124         "2:                             \n\t"
 125         " sar $1, %2                    \n\t" /* prepare for processing 2 samples at a time */
 126         " test $1, %2                   \n\t" /* check for odd samples */
 127         " je 4f                         \n\t"
 128
 129         "3:                             \n\t" /* do samples in groups of 2 */
 130         " movq (%1, %3, 4), %%mm0       \n\t" /* |  v1h  |  v1l  |  v0h  |  v0l  | */
 131         " movd (%0), %%mm1              \n\t" /*              .. |   p1  |  p0   | */
 132         VOLUME_32x16 (%%mm1, %%mm0)
 133         " movd %%mm0, (%0)              \n\t" /*              .. | p1*v1 | p0*v0 | */
 134         " add $4, %0                    \n\t"
 135         MOD_ADD ($2, %5)
 136
 137         "4:                             \n\t"
 138         " sar $1, %2                    \n\t" /* prepare for processing 4 samples at a time */
 139         " cmp $0, %2                    \n\t"
 140         " je 6f                         \n\t"
 141
 142         "5:                             \n\t" /* do samples in groups of 4 */
 143         " movq (%1, %3, 4), %%mm0       \n\t" /* |  v1h  |  v1l  |  v0h  |  v0l  | */
 144         " movq 8(%1, %3, 4), %%mm2      \n\t" /* |  v3h  |  v3l  |  v2h  |  v2l  | */
 145         " movd (%0), %%mm1              \n\t" /*              .. |   p1  |  p0   | */
 146         " movd 4(%0), %%mm3             \n\t" /*              .. |   p3  |  p2   | */
 147         VOLUME_32x16 (%%mm1, %%mm0)
 148         VOLUME_32x16 (%%mm3, %%mm2)
 149         " movd %%mm0, (%0)              \n\t" /*              .. | p1*v1 | p0*v0 | */
 150         " movd %%mm2, 4(%0)             \n\t" /*              .. | p3*v3 | p2*v2 | */
 151         " add $8, %0                    \n\t"
 152         MOD_ADD ($4, %5)
 153         " dec %2                        \n\t"
 154         " jne 5b                        \n\t"
 155
 156         "6:                             \n\t"
 157         " emms                          \n\t"
 158
 159         : "+r" (samples), "+r" (volumes), "+r" (length), "=D" ((pa_reg_x86)channel), "=&r" (temp)
 160         : "rm" ((pa_reg_x86)channels)
 161         : "cc"
 162     );
 163 }
 164
 165 static void
 166 pa_volume_s16re_mmx (int16_t *samples, int32_t *volumes, unsigned channels, unsigned length)
 167 {
 168     pa_reg_x86 channel, temp;
 169
 170     /* Channels must be at least 4, and always a multiple of the original number.
 171      * This is also the max amount we overread the volume array, which should
 172      * have enough padding. */
 173     channels = channels == 3 ? 6 : PA_MAX (4U, channels);
 174
 175     __asm__ __volatile__ (
 176         " xor %3, %3                    \n\t"
 177         " sar $1, %2                    \n\t" /* length /= sizeof (int16_t) */
 178         " pcmpeqw %%mm6, %%mm6          \n\t" /* .. |  ffff |  ffff | */
 179         " pcmpeqw %%mm7, %%mm7          \n\t" /* .. |  ffff |  ffff | */
 180         " pslld  $16, %%mm6             \n\t" /* .. |  ffff |     0 | */
 181         " psrld  $31, %%mm7             \n\t" /* .. |     0 |     1 | */
 182
 183         " test $1, %2                   \n\t" /* check for odd samples */
 184         " je 2f                         \n\t"
 185
 186         " movd (%1, %3, 4), %%mm0       \n\t" /* |  v0h  |  v0l  | */
 187         " movw (%0), %w4                \n\t" /*     ..  |  p0   | */
 188         " rorw $8, %w4                  \n\t"
 189         " movd %4, %%mm1                \n\t"
 190         VOLUME_32x16 (%%mm1, %%mm0)
 191         " movd %%mm0, %4                \n\t" /*     ..  | p0*v0 | */
 192         " rorw $8, %w4                  \n\t"
 193         " movw %w4, (%0)                \n\t"
 194         " add $2, %0                    \n\t"
 195         MOD_ADD ($1, %5)
 196
 197         "2:                             \n\t"
 198         " sar $1, %2                    \n\t" /* prepare for processing 2 samples at a time */
 199         " test $1, %2                   \n\t" /* check for odd samples */
 200         " je 4f                         \n\t"
 201
 202         "3:                             \n\t" /* do samples in groups of 2 */
 203         " movq (%1, %3, 4), %%mm0       \n\t" /* |  v1h  |  v1l  |  v0h  |  v0l  | */
 204         " movd (%0), %%mm1              \n\t" /*              .. |   p1  |  p0   | */
 205         SWAP_16 (%%mm1)
 206         VOLUME_32x16 (%%mm1, %%mm0)
 207         SWAP_16 (%%mm0)
 208         " movd %%mm0, (%0)              \n\t" /*              .. | p1*v1 | p0*v0 | */
 209         " add $4, %0                    \n\t"
 210         MOD_ADD ($2, %5)
 211
 212         "4:                             \n\t"
 213         " sar $1, %2                    \n\t" /* prepare for processing 4 samples at a time */
 214         " cmp $0, %2                    \n\t"
 215         " je 6f                         \n\t"
 216
 217         "5:                             \n\t" /* do samples in groups of 4 */
 218         " movq (%1, %3, 4), %%mm0       \n\t" /* |  v1h  |  v1l  |  v0h  |  v0l  | */
 219         " movq 8(%1, %3, 4), %%mm2      \n\t" /* |  v3h  |  v3l  |  v2h  |  v2l  | */
 220         " movd (%0), %%mm1              \n\t" /*              .. |   p1  |  p0   | */
 221         " movd 4(%0), %%mm3             \n\t" /*              .. |   p3  |  p2   | */
 222         SWAP_16_2 (%%mm1, %%mm3)
 223         VOLUME_32x16 (%%mm1, %%mm0)
 224         VOLUME_32x16 (%%mm3, %%mm2)
 225         SWAP_16_2 (%%mm0, %%mm2)
 226         " movd %%mm0, (%0)              \n\t" /*              .. | p1*v1 | p0*v0 | */
 227         " movd %%mm2, 4(%0)             \n\t" /*              .. | p3*v3 | p2*v2 | */
 228         " add $8, %0                    \n\t"
 229         MOD_ADD ($4, %5)
 230         " dec %2                        \n\t"
 231         " jne 5b                        \n\t"
 232
 233         "6:                             \n\t"
 234         " emms                          \n\t"
 235
 236         : "+r" (samples), "+r" (volumes), "+r" (length), "=D" ((pa_reg_x86)channel), "=&r" (temp)
 237         : "rm" ((pa_reg_x86)channels)
 238         : "cc"
 239     );
 240 }
 241
 242 #undef RUN_TEST
 243
 244 #ifdef RUN_TEST
 245 #define CHANNELS 2
 246 #define SAMPLES 1022
 247 #define TIMES 1000
 248 #define PADDING 16
 249
 250 static void run_test (void) {
 251     int16_t samples[SAMPLES];
 252     int16_t samples_ref[SAMPLES];
 253     int16_t samples_orig[SAMPLES];
 254     int32_t volumes[CHANNELS + PADDING];
 255     int i, j, padding;
 256     pa_do_volume_func_t func;
 257     pa_usec_t start, stop;
 258
 259     func = pa_get_volume_func (PA_SAMPLE_S16NE);
 260
 261     printf ("checking MMX %zd\n", sizeof (samples));
 262
 263     pa_random (samples, sizeof (samples));
 264     /* for (i = 0; i < SAMPLES; i++)
 265        samples[i] = -1; */
 266     memcpy (samples_ref, samples, sizeof (samples));
 267     memcpy (samples_orig, samples, sizeof (samples));
 268
 269     for (i = 0; i < CHANNELS; i++)
 270         volumes[i] = PA_CLAMP_VOLUME(rand() >> 1);
 271         /* volumes[i] = 0x0000ffff; */
 272     for (padding = 0; padding < PADDING; padding++, i++)
 273         volumes[i] = volumes[padding];
 274
 275     func (samples_ref, volumes, CHANNELS, sizeof (samples));
 276     pa_volume_s16ne_mmx (samples, volumes, CHANNELS, sizeof (samples));
 277     for (i = 0; i < SAMPLES; i++) {
 278         if (samples[i] != samples_ref[i]) {
 279             printf ("%d: %04x != %04x (%04x * %08x)\n", i, samples[i], samples_ref[i],
 280                   samples_orig[i], volumes[i % CHANNELS]);
 281         }
 282     }
 283
 284     start = pa_rtclock_now();
 285     for (j = 0; j < TIMES; j++) {
 286         memcpy (samples, samples_orig, sizeof (samples));
 287         pa_volume_s16ne_mmx (samples, volumes, CHANNELS, sizeof (samples));
 288     }
 289     stop = pa_rtclock_now();
 290     pa_log_info("MMX: %llu usec.", (long long unsigned int)(stop - start));
 291
 292     start = pa_rtclock_now();
 293     for (j = 0; j < TIMES; j++) {
 294         memcpy (samples_ref, samples_orig, sizeof (samples));
 295         func (samples_ref, volumes, CHANNELS, sizeof (samples));
 296     }
 297     stop = pa_rtclock_now();
 298     pa_log_info("ref: %llu usec.", (long long unsigned int)(stop - start));
 299
 300     pa_assert_se(memcmp(samples_ref, samples, sizeof(samples)) == 0);
 301 }
 302 #endif
 303
 304 #endif /* defined (__i386__) || defined (__amd64__) */
 305
 306
 307 void pa_volume_func_init_mmx (pa_cpu_x86_flag_t flags) {
 308 #if defined (__i386__) || defined (__amd64__)
 309
 310 #ifdef RUN_TEST
 311     run_test ();
 312 #endif
 313
 314     if ((flags & PA_CPU_X86_MMX) && (flags & PA_CPU_X86_CMOV)) {
 315         pa_log_info("Initialising MMX optimized functions.");
 316
 317         pa_set_volume_func (PA_SAMPLE_S16NE, (pa_do_volume_func_t) pa_volume_s16ne_mmx);
 318         pa_set_volume_func (PA_SAMPLE_S16RE, (pa_do_volume_func_t) pa_volume_s16re_mmx);
 319     }
 320 #endif /* defined (__i386__) || defined (__amd64__) */
 321 }