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