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 <pulsecore/random.h>
  29 #include <pulsecore/macro.h>
  30 #include <pulsecore/g711.h>
  31 #include <pulsecore/core-util.h>
  32
  33 #include "cpu-x86.h"
  34
  35 #include "sample-util.h"
  36 #include "endianmacros.h"
  37
  38 #if 0
  39 static void
  40 pa_volume_u8_sse (uint8_t *samples, int32_t *volumes, unsigned channels, unsigned length)
  41 {
  42   unsigned channel;
  43
  44   for (channel = 0; length; length--) {
  45     int32_t t, hi, lo;
  46
  47     hi = volumes[channel] >> 16;
  48     lo = volumes[channel] & 0xFFFF;
  49
  50     t = (int32_t) *samples - 0x80;
  51     t = ((t * lo) >> 16) + (t * hi);
  52     t = PA_CLAMP_UNLIKELY(t, -0x80, 0x7F);
  53     *samples++ = (uint8_t) (t + 0x80);
  54
  55     if (PA_UNLIKELY(++channel >= channels))
  56       channel = 0;
  57   }
  58 }
  59
  60 static void
  61 pa_volume_alaw_sse (uint8_t *samples, int32_t *volumes, unsigned channels, unsigned length)
  62 {
  63   unsigned channel;
  64
  65   for (channel = 0; length; length--) {
  66     int32_t t, hi, lo;
  67
  68     hi = volumes[channel] >> 16;
  69     lo = volumes[channel] & 0xFFFF;
  70
  71     t = (int32_t) st_alaw2linear16(*samples);
  72     t = ((t * lo) >> 16) + (t * hi);
  73     t = PA_CLAMP_UNLIKELY(t, -0x8000, 0x7FFF);
  74     *samples++ = (uint8_t) st_13linear2alaw((int16_t) t >> 3);
  75
  76     if (PA_UNLIKELY(++channel >= channels))
  77       channel = 0;
  78   }
  79 }
  80
  81 static void
  82 pa_volume_ulaw_sse (uint8_t *samples, int32_t *volumes, unsigned channels, unsigned length)
  83 {
  84   unsigned channel;
  85
  86   for (channel = 0; length; length--) {
  87     int32_t t, hi, lo;
  88
  89     hi = volumes[channel] >> 16;
  90     lo = volumes[channel] & 0xFFFF;
  91
  92     t = (int32_t) st_ulaw2linear16(*samples);
  93     t = ((t * lo) >> 16) + (t * hi);
  94     t = PA_CLAMP_UNLIKELY(t, -0x8000, 0x7FFF);
  95     *samples++ = (uint8_t) st_14linear2ulaw((int16_t) t >> 2);
  96
  97     if (PA_UNLIKELY(++channel >= channels))
  98       channel = 0;
  99   }
 100 }
 101 #endif
 102
 103 #define VOLUME_32x16(s,v)                  /* .. |   vh  |   vl  | */                   \
 104       " pxor %%xmm4, %%xmm4          \n\t" /* .. |    0  |    0  | */                   \
 105       " punpcklwd %%xmm4, "#s"       \n\t" /* .. |    0  |   p0  | */                   \
 106       " pcmpgtw "#s", %%xmm4         \n\t" /* .. |    0  | s(p0) | */                   \
 107       " pand "#v", %%xmm4            \n\t" /* .. |    0  |  (vl) | */                   \
 108       " movdqa "#s", %%xmm5          \n\t"                                              \
 109       " pmulhuw "#v", "#s"           \n\t" /* .. |    0  | vl*p0 | */                   \
 110       " psubd %%xmm4, "#s"           \n\t" /* .. |    0  | vl*p0 | + sign correct */    \
 111       " psrld $16, "#v"              \n\t" /* .. |   p0  |    0  | */                   \
 112       " pmaddwd %%xmm5, "#v"         \n\t" /* .. |    p0 * vh    | */                   \
 113       " paddd "#s", "#v"             \n\t" /* .. |    p0 * v0    | */                   \
 114       " packssdw "#v", "#v"          \n\t" /* .. | p1*v1 | p0*v0 | */
 115
 116 #define MOD_ADD(a,b) \
 117       " add "#a", %3                 \n\t" /* channel += inc           */ \
 118       " mov %3, %4                   \n\t"                                \
 119       " sub "#b", %4                 \n\t" /* tmp = channel - channels */ \
 120       " cmp "#b", %3                 \n\t" /* if (channel >= channels) */ \
 121       " cmovae %4, %3                \n\t" /*   channel = tmp          */
 122
 123 /* swap 16 bits */
 124 #define SWAP_16(s) \
 125       " movdqa "#s", %%xmm4          \n\t" /* .. |  h  l |  */ \
 126       " psrlw $8, %%xmm4             \n\t" /* .. |  0  h |  */ \
 127       " psllw $8, "#s"               \n\t" /* .. |  l  0 |  */ \
 128       " por %%xmm4, "#s"             \n\t" /* .. |  l  h |  */
 129
 130 /* swap 2 registers 16 bits for better pairing */
 131 #define SWAP_16_2(s1,s2) \
 132       " movdqa "#s1", %%xmm4         \n\t" /* .. |  h  l |  */ \
 133       " movdqa "#s2", %%xmm5         \n\t"                     \
 134       " psrlw $8, %%xmm4             \n\t" /* .. |  0  h |  */ \
 135       " psrlw $8, %%xmm5             \n\t"                     \
 136       " psllw $8, "#s1"              \n\t" /* .. |  l  0 |  */ \
 137       " psllw $8, "#s2"              \n\t"                     \
 138       " por %%xmm4, "#s1"            \n\t" /* .. |  l  h |  */ \
 139       " por %%xmm5, "#s2"            \n\t"
 140
 141 static void
 142 pa_volume_s16ne_sse (int16_t *samples, int32_t *volumes, unsigned channels, unsigned length)
 143 {
 144   pa_reg_x86 channel, temp;
 145
 146   /* the max number of samples we process at a time, this is also the max amount
 147    * we overread the volume array, which should have enough padding. */
 148   channels = MAX (8, channels);
 149
 150   __asm__ __volatile__ (
 151     " xor %3, %3                    \n\t"
 152     " sar $1, %2                    \n\t" /* length /= sizeof (int16_t) */
 153
 154     " test $1, %2                   \n\t" /* check for odd samples */
 155     " je 2f                         \n\t"
 156
 157     " movd (%1, %3, 4), %%xmm0      \n\t" /* |  v0h  |  v0l  | */
 158     " movw (%0), %4                 \n\t" /*     ..  |   p0  | */
 159     " movd %4, %%xmm1               \n\t"
 160     VOLUME_32x16 (%%xmm1, %%xmm0)
 161     " movd %%xmm0, %4               \n\t" /*     ..  | p0*v0 | */
 162     " movw %4, (%0)                 \n\t"
 163     " add $2, %0                    \n\t"
 164     MOD_ADD ($1, %5)
 165
 166     "2:                             \n\t"
 167     " sar $1, %2                    \n\t" /* prepare for processing 2 samples at a time */
 168     " test $1, %2                   \n\t"
 169     " je 4f                         \n\t"
 170
 171     "3:                             \n\t" /* do samples in groups of 2 */
 172     " movq (%1, %3, 4), %%xmm0      \n\t" /* |  v1h  |  v1l  |  v0h  |  v0l  | */
 173     " movd (%0), %%xmm1             \n\t" /*              .. |   p1  |  p0   | */
 174     VOLUME_32x16 (%%xmm1, %%xmm0)
 175     " movd %%xmm0, (%0)             \n\t" /*              .. | p1*v1 | p0*v0 | */
 176     " add $4, %0                    \n\t"
 177     MOD_ADD ($2, %5)
 178
 179     "4:                             \n\t"
 180     " sar $1, %2                    \n\t" /* prepare for processing 4 samples at a time */
 181     " test $1, %2                   \n\t"
 182     " je 6f                         \n\t"
 183
 184     "5:                             \n\t" /* do samples in groups of 4 */
 185     " movdqu (%1, %3, 4), %%xmm0    \n\t" /* |  v3h  |  v3l  ..  v0h  |  v0l  | */
 186     " movq (%0), %%xmm1             \n\t" /*              .. |   p3  ..  p0   | */
 187     VOLUME_32x16 (%%xmm1, %%xmm0)
 188     " movq %%xmm0, (%0)             \n\t" /*              .. | p3*v3 .. p0*v0 | */
 189     " add $8, %0                    \n\t"
 190     MOD_ADD ($4, %5)
 191
 192     "6:                             \n\t"
 193     " sar $1, %2                    \n\t" /* prepare for processing 8 samples at a time */
 194     " cmp $0, %2                    \n\t"
 195     " je 8f                         \n\t"
 196
 197     "7:                             \n\t" /* do samples in groups of 8 */
 198     " movdqu (%1, %3, 4), %%xmm0    \n\t" /* |  v3h  |  v3l  ..  v0h  |  v0l  | */
 199     " movdqu 16(%1, %3, 4), %%xmm2  \n\t" /* |  v7h  |  v7l  ..  v4h  |  v4l  | */
 200     " movq (%0), %%xmm1             \n\t" /*              .. |   p3  ..  p0   | */
 201     " movq 8(%0), %%xmm3            \n\t" /*              .. |   p7  ..  p4   | */
 202     VOLUME_32x16 (%%xmm1, %%xmm0)
 203     VOLUME_32x16 (%%xmm3, %%xmm2)
 204     " movq %%xmm0, (%0)             \n\t" /*              .. | p3*v3 .. p0*v0 | */
 205     " movq %%xmm2, 8(%0)            \n\t" /*              .. | p7*v7 .. p4*v4 | */
 206     " add $16, %0                   \n\t"
 207     MOD_ADD ($8, %5)
 208     " dec %2                        \n\t"
 209     " jne 7b                        \n\t"
 210     "8:                             \n\t"
 211
 212     : "+r" (samples), "+r" (volumes), "+r" (length), "=D" (channel), "=&r" (temp)
 213     : "r" ((pa_reg_x86)channels)
 214     : "cc"
 215   );
 216 }
 217
 218 static void
 219 pa_volume_s16re_sse (int16_t *samples, int32_t *volumes, unsigned channels, unsigned length)
 220 {
 221   pa_reg_x86 channel, temp;
 222
 223   /* the max number of samples we process at a time, this is also the max amount
 224    * we overread the volume array, which should have enough padding. */
 225   channels = MAX (8, channels);
 226
 227   __asm__ __volatile__ (
 228     " xor %3, %3                    \n\t"
 229     " sar $1, %2                    \n\t" /* length /= sizeof (int16_t) */
 230
 231     " test $1, %2                   \n\t" /* check for odd samples */
 232     " je 2f                         \n\t"
 233
 234     " movd (%1, %3, 4), %%xmm0      \n\t" /* do odd sample */
 235     " movw (%0), %4                 \n\t"
 236     " rorw $8, %4                   \n\t"
 237     " movd %4, %%xmm1               \n\t"
 238     VOLUME_32x16 (%%xmm1, %%xmm0)
 239     " movd %%xmm0, %4               \n\t"
 240     " rorw $8, %4                   \n\t"
 241     " movw %4, (%0)                 \n\t"
 242     " add $2, %0                    \n\t"
 243     MOD_ADD ($1, %5)
 244
 245     "2:                             \n\t"
 246     " sar $1, %2                    \n\t" /* prepare for processing 2 samples at a time */
 247     " test $1, %2                   \n\t" /* check for odd samples */
 248     " je 4f                         \n\t"
 249
 250     "3:                             \n\t" /* do samples in pairs of 2 */
 251     " movq (%1, %3, 4), %%xmm0      \n\t" /* v1_h  | v1_l  | v0_h  | v0_l      */
 252     " movd (%0), %%xmm1             \n\t" /*  X    |  X    |  p1   |  p0       */
 253     SWAP_16 (%%xmm1)
 254     VOLUME_32x16 (%%xmm1, %%xmm0)
 255     SWAP_16 (%%xmm0)
 256     " movd %%xmm0, (%0)             \n\t"
 257     " add $4, %0                    \n\t"
 258     MOD_ADD ($2, %5)
 259
 260     "4:                             \n\t"
 261     " sar $1, %2                    \n\t" /* prepare for processing 4 samples at a time */
 262     " test $1, %2                   \n\t" /* check for odd samples */
 263     " je 6f                         \n\t"
 264
 265     "5:                             \n\t" /* do samples in pairs of 4 */
 266     " movdqu (%1, %3, 4), %%xmm0    \n\t" /* v1_h  | v1_l  | v0_h  | v0_l      */
 267     " movq (%0), %%xmm1             \n\t" /*  X    |  X    |  p1   |  p0       */
 268     SWAP_16 (%%xmm1)
 269     VOLUME_32x16 (%%xmm1, %%xmm0)
 270     SWAP_16 (%%xmm0)
 271     " movq %%xmm0, (%0)             \n\t"
 272     " add $8, %0                    \n\t"
 273     MOD_ADD ($4, %5)
 274
 275     "6:                             \n\t"
 276     " sar $1, %2                    \n\t" /* prepare for processing 8 samples at a time */
 277     " cmp $0, %2                    \n\t"
 278     " je 8f                         \n\t"
 279
 280     "7:                             \n\t" /* do samples in pairs of 8 */
 281     " movdqu (%1, %3, 4), %%xmm0    \n\t" /* v1_h  | v1_l  | v0_h  | v0_l      */
 282     " movdqu 16(%1, %3, 4), %%xmm2  \n\t" /* v3_h  | v3_l  | v2_h  | v2_l      */
 283     " movq (%0), %%xmm1             \n\t" /*  X    |  X    |  p1   |  p0       */
 284     " movq 8(%0), %%xmm3            \n\t" /*  X    |  X    |  p3   |  p2       */
 285     SWAP_16_2 (%%xmm1, %%xmm3)
 286     VOLUME_32x16 (%%xmm1, %%xmm0)
 287     VOLUME_32x16 (%%xmm3, %%xmm2)
 288     SWAP_16_2 (%%xmm0, %%xmm2)
 289     " movq %%xmm0, (%0)             \n\t"
 290     " movq %%xmm2, 8(%0)            \n\t"
 291     " add $16, %0                   \n\t"
 292     MOD_ADD ($8, %5)
 293     " dec %2                        \n\t"
 294     " jne 7b                        \n\t"
 295     "8:                             \n\t"
 296
 297     : "+r" (samples), "+r" (volumes), "+r" (length), "=D" (channel), "=&r" (temp)
 298     : "r" ((pa_reg_x86)channels)
 299     : "cc"
 300   );
 301 }
 302
 303 #if 0
 304 static void
 305 pa_volume_float32ne_sse (float *samples, float *volumes, unsigned channels, unsigned length)
 306 {
 307   unsigned channel;
 308
 309   length /= sizeof (float);
 310
 311   for (channel = 0; length; length--) {
 312     *samples++ *= volumes[channel];
 313
 314     if (PA_UNLIKELY(++channel >= channels))
 315       channel = 0;
 316   }
 317 }
 318
 319 static void
 320 pa_volume_float32re_sse (float *samples, float *volumes, unsigned channels, unsigned length)
 321 {
 322   unsigned channel;
 323
 324   length /= sizeof (float);
 325
 326   for (channel = 0; length; length--) {
 327     float t;
 328
 329     t = PA_FLOAT32_SWAP(*samples);
 330     t *= volumes[channel];
 331     *samples++ = PA_FLOAT32_SWAP(t);
 332
 333     if (PA_UNLIKELY(++channel >= channels))
 334       channel = 0;
 335   }
 336 }
 337
 338 static void
 339 pa_volume_s32ne_sse (int32_t *samples, int32_t *volumes, unsigned channels, unsigned length)
 340 {
 341   unsigned channel;
 342
 343   length /= sizeof (int32_t);
 344
 345   for (channel = 0; length; length--) {
 346     int64_t t;
 347
 348     t = (int64_t)(*samples);
 349     t = (t * volumes[channel]) >> 16;
 350     t = PA_CLAMP_UNLIKELY(t, -0x80000000LL, 0x7FFFFFFFLL);
 351     *samples++ = (int32_t) t;
 352
 353     if (PA_UNLIKELY(++channel >= channels))
 354       channel = 0;
 355   }
 356 }
 357
 358 static void
 359 pa_volume_s32re_sse (int32_t *samples, int32_t *volumes, unsigned channels, unsigned length)
 360 {
 361   unsigned channel;
 362
 363   length /= sizeof (int32_t);
 364
 365   for (channel = 0; length; length--) {
 366     int64_t t;
 367
 368     t = (int64_t) PA_INT32_SWAP(*samples);
 369     t = (t * volumes[channel]) >> 16;
 370     t = PA_CLAMP_UNLIKELY(t, -0x80000000LL, 0x7FFFFFFFLL);
 371     *samples++ = PA_INT32_SWAP((int32_t) t);
 372
 373     if (PA_UNLIKELY(++channel >= channels))
 374       channel = 0;
 375   }
 376 }
 377
 378 static void
 379 pa_volume_s24ne_sse (uint8_t *samples, int32_t *volumes, unsigned channels, unsigned length)
 380 {
 381   unsigned channel;
 382   uint8_t *e;
 383
 384   e = samples + length;
 385
 386   for (channel = 0; samples < e; samples += 3) {
 387     int64_t t;
 388
 389     t = (int64_t)((int32_t) (PA_READ24NE(samples) << 8));
 390     t = (t * volumes[channel]) >> 16;
 391     t = PA_CLAMP_UNLIKELY(t, -0x80000000LL, 0x7FFFFFFFLL);
 392     PA_WRITE24NE(samples, ((uint32_t) (int32_t) t) >> 8);
 393
 394     if (PA_UNLIKELY(++channel >= channels))
 395       channel = 0;
 396   }
 397 }
 398
 399 static void
 400 pa_volume_s24re_sse (uint8_t *samples, int32_t *volumes, unsigned channels, unsigned length)
 401 {
 402   unsigned channel;
 403   uint8_t *e;
 404
 405   e = samples + length;
 406
 407   for (channel = 0; samples < e; samples += 3) {
 408     int64_t t;
 409
 410     t = (int64_t)((int32_t) (PA_READ24RE(samples) << 8));
 411     t = (t * volumes[channel]) >> 16;
 412     t = PA_CLAMP_UNLIKELY(t, -0x80000000LL, 0x7FFFFFFFLL);
 413     PA_WRITE24RE(samples, ((uint32_t) (int32_t) t) >> 8);
 414
 415     if (PA_UNLIKELY(++channel >= channels))
 416       channel = 0;
 417   }
 418 }
 419
 420 static void
 421 pa_volume_s24_32ne_sse (uint32_t *samples, int32_t *volumes, unsigned channels, unsigned length)
 422 {
 423   unsigned channel;
 424
 425   length /= sizeof (uint32_t);
 426
 427   for (channel = 0; length; length--) {
 428     int64_t t;
 429
 430     t = (int64_t) ((int32_t) (*samples << 8));
 431     t = (t * volumes[channel]) >> 16;
 432     t = PA_CLAMP_UNLIKELY(t, -0x80000000LL, 0x7FFFFFFFLL);
 433     *samples++ = ((uint32_t) ((int32_t) t)) >> 8;
 434
 435     if (PA_UNLIKELY(++channel >= channels))
 436       channel = 0;
 437   }
 438 }
 439
 440 static void
 441 pa_volume_s24_32re_sse (uint32_t *samples, int32_t *volumes, unsigned channels, unsigned length)
 442 {
 443   unsigned channel;
 444
 445   length /= sizeof (uint32_t);
 446
 447   for (channel = 0; length; length--) {
 448     int64_t t;
 449
 450     t = (int64_t) ((int32_t) (PA_UINT32_SWAP(*samples) << 8));
 451     t = (t * volumes[channel]) >> 16;
 452     t = PA_CLAMP_UNLIKELY(t, -0x80000000LL, 0x7FFFFFFFLL);
 453     *samples++ = PA_UINT32_SWAP(((uint32_t) ((int32_t) t)) >> 8);
 454
 455     if (PA_UNLIKELY(++channel >= channels))
 456       channel = 0;
 457   }
 458 }
 459 #endif
 460
 461 #define RUN_TEST
 462
 463 #ifdef RUN_TEST
 464 #define CHANNELS 2
 465 #define SAMPLES 1021
 466 #define TIMES 1000
 467 #define PADDING 16
 468
 469 static void run_test (void) {
 470   int16_t samples[SAMPLES];
 471   int16_t samples_ref[SAMPLES];
 472   int16_t samples_orig[SAMPLES];
 473   int32_t volumes[CHANNELS + PADDING];
 474   int i, j, padding;
 475   pa_do_volume_func_t func;
 476   struct timeval start, stop;
 477
 478   func = pa_get_volume_func (PA_SAMPLE_S16NE);
 479
 480   printf ("checking SSE %zd\n", sizeof (samples));
 481
 482   pa_random (samples, sizeof (samples));
 483   memcpy (samples_ref, samples, sizeof (samples));
 484   memcpy (samples_orig, samples, sizeof (samples));
 485
 486   for (i = 0; i < CHANNELS; i++)
 487     volumes[i] = rand() >> 1;
 488   for (padding = 0; padding < PADDING; padding++, i++)
 489     volumes[i] = volumes[padding];
 490
 491   func (samples_ref, volumes, CHANNELS, sizeof (samples));
 492   pa_volume_s16ne_sse (samples, volumes, CHANNELS, sizeof (samples));
 493   for (i = 0; i < SAMPLES; i++) {
 494     if (samples[i] != samples_ref[i]) {
 495       printf ("%d: %04x != %04x (%04x * %04x)\n", i, samples[i], samples_ref[i],
 496               samples_orig[i], volumes[i % CHANNELS]);
 497     }
 498   }
 499
 500   pa_gettimeofday(&start);
 501   for (j = 0; j < TIMES; j++) {
 502     memcpy (samples, samples_orig, sizeof (samples));
 503     pa_volume_s16ne_sse (samples, volumes, CHANNELS, sizeof (samples));
 504   }
 505   pa_gettimeofday(&stop);
 506   pa_log_info("SSE: %llu usec.", (long long unsigned int)pa_timeval_diff (&stop, &start));
 507
 508   pa_gettimeofday(&start);
 509   for (j = 0; j < TIMES; j++) {
 510     memcpy (samples_ref, samples_orig, sizeof (samples));
 511     func (samples_ref, volumes, CHANNELS, sizeof (samples));
 512   }
 513   pa_gettimeofday(&stop);
 514   pa_log_info("ref: %llu usec.", (long long unsigned int)pa_timeval_diff (&stop, &start));
 515 }
 516 #endif
 517
 518 void pa_volume_func_init_sse (pa_cpu_x86_flag_t flags) {
 519   pa_log_info("Initialising SSE optimized functions.");
 520
 521 #ifdef RUN_TEST
 522   run_test ();
 523 #endif
 524
 525   pa_set_volume_func (PA_SAMPLE_S16NE,     (pa_do_volume_func_t) pa_volume_s16ne_sse);
 526   pa_set_volume_func (PA_SAMPLE_S16RE,     (pa_do_volume_func_t) pa_volume_s16re_sse);
 527 }