src/pulsecore/mix.c

   1 /***
   2   This file is part of PulseAudio.
   3
   4   Copyright 2004-2006 Lennart Poettering
   5   Copyright 2006 Pierre Ossman <ossman@cendio.se> for Cendio AB
   6   Copyright 2013 Peter Meerwald <pmeerw@pmeerw.net>
   7
   8   PulseAudio is free software; you can redistribute it and/or modify
   9   it under the terms of the GNU Lesser General Public License as published
  10   by the Free Software Foundation; either version 2.1 of the License,
  11   or (at your option) any later version.
  12
  13   PulseAudio is distributed in the hope that it will be useful, but
  14   WITHOUT ANY WARRANTY; without even the implied warranty of
  15   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  16   General Public License for more details.
  17
  18   You should have received a copy of the GNU Lesser General Public License
  19   along with PulseAudio; if not, write to the Free Software
  20   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
  21   USA.
  22 ***/
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include <config.h>
  26 #endif
  27
  28 #include <math.h>
  29
  30 #include <pulsecore/sample-util.h>
  31 #include <pulsecore/macro.h>
  32 #include <pulsecore/g711.h>
  33 #include <pulsecore/endianmacros.h>
  34
  35 #include "mix.h"
  36
  37 #define VOLUME_PADDING 32
  38
  39 static void calc_linear_integer_volume(int32_t linear[], const pa_cvolume *volume) {
  40     unsigned channel, nchannels, padding;
  41
  42     pa_assert(linear);
  43     pa_assert(volume);
  44
  45     nchannels = volume->channels;
  46
  47     for (channel = 0; channel < nchannels; channel++)
  48         linear[channel] = (int32_t) lrint(pa_sw_volume_to_linear(volume->values[channel]) * 0x10000);
  49
  50     for (padding = 0; padding < VOLUME_PADDING; padding++, channel++)
  51         linear[channel] = linear[padding];
  52 }
  53
  54 static void calc_linear_float_volume(float linear[], const pa_cvolume *volume) {
  55     unsigned channel, nchannels, padding;
  56
  57     pa_assert(linear);
  58     pa_assert(volume);
  59
  60     nchannels = volume->channels;
  61
  62     for (channel = 0; channel < nchannels; channel++)
  63         linear[channel] = (float) pa_sw_volume_to_linear(volume->values[channel]);
  64
  65     for (padding = 0; padding < VOLUME_PADDING; padding++, channel++)
  66         linear[channel] = linear[padding];
  67 }
  68
  69 static void calc_linear_integer_stream_volumes(pa_mix_info streams[], unsigned nstreams, const pa_cvolume *volume, const pa_sample_spec *spec) {
  70     unsigned k, channel;
  71     float linear[PA_CHANNELS_MAX + VOLUME_PADDING];
  72
  73     pa_assert(streams);
  74     pa_assert(spec);
  75     pa_assert(volume);
  76
  77     calc_linear_float_volume(linear, volume);
  78
  79     for (k = 0; k < nstreams; k++) {
  80
  81         for (channel = 0; channel < spec->channels; channel++) {
  82             pa_mix_info *m = streams + k;
  83             m->linear[channel].i = (int32_t) lrint(pa_sw_volume_to_linear(m->volume.values[channel]) * linear[channel] * 0x10000);
  84         }
  85     }
  86 }
  87
  88 static void calc_linear_float_stream_volumes(pa_mix_info streams[], unsigned nstreams, const pa_cvolume *volume, const pa_sample_spec *spec) {
  89     unsigned k, channel;
  90     float linear[PA_CHANNELS_MAX + VOLUME_PADDING];
  91
  92     pa_assert(streams);
  93     pa_assert(spec);
  94     pa_assert(volume);
  95
  96     calc_linear_float_volume(linear, volume);
  97
  98     for (k = 0; k < nstreams; k++) {
  99
 100         for (channel = 0; channel < spec->channels; channel++) {
 101             pa_mix_info *m = streams + k;
 102             m->linear[channel].f = (float) (pa_sw_volume_to_linear(m->volume.values[channel]) * linear[channel]);
 103         }
 104     }
 105 }
 106
 107 typedef void (*pa_calc_stream_volumes_func_t) (pa_mix_info streams[], unsigned nstreams, const pa_cvolume *volume, const pa_sample_spec *spec);
 108
 109 static const pa_calc_stream_volumes_func_t calc_stream_volumes_table[] = {
 110   [PA_SAMPLE_U8]        = (pa_calc_stream_volumes_func_t) calc_linear_integer_stream_volumes,
 111   [PA_SAMPLE_ALAW]      = (pa_calc_stream_volumes_func_t) calc_linear_integer_stream_volumes,
 112   [PA_SAMPLE_ULAW]      = (pa_calc_stream_volumes_func_t) calc_linear_integer_stream_volumes,
 113   [PA_SAMPLE_S16LE]     = (pa_calc_stream_volumes_func_t) calc_linear_integer_stream_volumes,
 114   [PA_SAMPLE_S16BE]     = (pa_calc_stream_volumes_func_t) calc_linear_integer_stream_volumes,
 115   [PA_SAMPLE_FLOAT32LE] = (pa_calc_stream_volumes_func_t) calc_linear_float_stream_volumes,
 116   [PA_SAMPLE_FLOAT32BE] = (pa_calc_stream_volumes_func_t) calc_linear_float_stream_volumes,
 117   [PA_SAMPLE_S32LE]     = (pa_calc_stream_volumes_func_t) calc_linear_integer_stream_volumes,
 118   [PA_SAMPLE_S32BE]     = (pa_calc_stream_volumes_func_t) calc_linear_integer_stream_volumes,
 119   [PA_SAMPLE_S24LE]     = (pa_calc_stream_volumes_func_t) calc_linear_integer_stream_volumes,
 120   [PA_SAMPLE_S24BE]     = (pa_calc_stream_volumes_func_t) calc_linear_integer_stream_volumes,
 121   [PA_SAMPLE_S24_32LE]  = (pa_calc_stream_volumes_func_t) calc_linear_integer_stream_volumes,
 122   [PA_SAMPLE_S24_32BE]  = (pa_calc_stream_volumes_func_t) calc_linear_integer_stream_volumes
 123 };
 124
 125 /* special case: mix 2 s16ne streams, 1 channel each */
 126 static void pa_mix2_ch1_s16ne(pa_mix_info streams[], int16_t *data, unsigned length) {
 127     const int16_t *ptr0 = streams[0].ptr;
 128     const int16_t *ptr1 = streams[1].ptr;
 129
 130     const int32_t cv0 = streams[0].linear[0].i;
 131     const int32_t cv1 = streams[1].linear[0].i;
 132
 133     length /= sizeof(int16_t);
 134
 135     for (; length > 0; length--) {
 136         int32_t sum;
 137
 138         sum = pa_mult_s16_volume(*ptr0++, cv0);
 139         sum += pa_mult_s16_volume(*ptr1++, cv1);
 140
 141         sum = PA_CLAMP_UNLIKELY(sum, -0x8000, 0x7FFF);
 142         *data++ = sum;
 143     }
 144 }
 145
 146 /* special case: mix 2 s16ne streams, 2 channels each */
 147 static void pa_mix2_ch2_s16ne(pa_mix_info streams[], int16_t *data, unsigned length) {
 148     const int16_t *ptr0 = streams[0].ptr;
 149     const int16_t *ptr1 = streams[1].ptr;
 150
 151     length /= sizeof(int16_t) * 2;
 152
 153     for (; length > 0; length--) {
 154         int32_t sum;
 155
 156         sum = pa_mult_s16_volume(*ptr0++, streams[0].linear[0].i);
 157         sum += pa_mult_s16_volume(*ptr1++, streams[1].linear[0].i);
 158
 159         sum = PA_CLAMP_UNLIKELY(sum, -0x8000, 0x7FFF);
 160         *data++ = sum;
 161
 162         sum = pa_mult_s16_volume(*ptr0++, streams[0].linear[1].i);
 163         sum += pa_mult_s16_volume(*ptr1++, streams[1].linear[1].i);
 164
 165         sum = PA_CLAMP_UNLIKELY(sum, -0x8000, 0x7FFF);
 166         *data++ = sum;
 167     }
 168 }
 169
 170 /* special case: mix 2 s16ne streams */
 171 static void pa_mix2_s16ne(pa_mix_info streams[], unsigned channels, int16_t *data, unsigned length) {
 172     const int16_t *ptr0 = streams[0].ptr;
 173     const int16_t *ptr1 = streams[1].ptr;
 174     unsigned channel = 0;
 175
 176     length /= sizeof(int16_t);
 177
 178     for (; length > 0; length--) {
 179         int32_t sum;
 180
 181         sum = pa_mult_s16_volume(*ptr0++, streams[0].linear[channel].i);
 182         sum += pa_mult_s16_volume(*ptr1++, streams[1].linear[channel].i);
 183
 184         sum = PA_CLAMP_UNLIKELY(sum, -0x8000, 0x7FFF);
 185         *data++ = sum;
 186
 187         if (PA_UNLIKELY(++channel >= channels))
 188             channel = 0;
 189     }
 190 }
 191
 192 /* special case: mix s16ne streams, 2 channels each */
 193 static void pa_mix_ch2_s16ne(pa_mix_info streams[], unsigned nstreams, int16_t *data, unsigned length) {
 194
 195     length /= sizeof(int16_t) * 2;
 196
 197     for (; length > 0; length--) {
 198         int32_t sum0 = 0, sum1 = 0;
 199         unsigned i;
 200
 201         for (i = 0; i < nstreams; i++) {
 202             pa_mix_info *m = streams + i;
 203             int32_t cv0 = m->linear[0].i;
 204             int32_t cv1 = m->linear[1].i;
 205
 206             sum0 += pa_mult_s16_volume(*((int16_t*) m->ptr), cv0);
 207             m->ptr = (uint8_t*) m->ptr + sizeof(int16_t);
 208
 209             sum1 += pa_mult_s16_volume(*((int16_t*) m->ptr), cv1);
 210             m->ptr = (uint8_t*) m->ptr + sizeof(int16_t);
 211         }
 212
 213         *data++ = PA_CLAMP_UNLIKELY(sum0, -0x8000, 0x7FFF);
 214         *data++ = PA_CLAMP_UNLIKELY(sum1, -0x8000, 0x7FFF);
 215     }
 216 }
 217
 218 static void pa_mix_generic_s16ne(pa_mix_info streams[], unsigned nstreams, unsigned channels, int16_t *data, unsigned length) {
 219     unsigned channel = 0;
 220
 221     length /= sizeof(int16_t);
 222
 223     for (; length > 0; length--) {
 224         int32_t sum = 0;
 225         unsigned i;
 226
 227         for (i = 0; i < nstreams; i++) {
 228             pa_mix_info *m = streams + i;
 229             int32_t cv = m->linear[channel].i;
 230
 231             if (PA_LIKELY(cv > 0))
 232                 sum += pa_mult_s16_volume(*((int16_t*) m->ptr), cv);
 233             m->ptr = (uint8_t*) m->ptr + sizeof(int16_t);
 234         }
 235
 236         sum = PA_CLAMP_UNLIKELY(sum, -0x8000, 0x7FFF);
 237         *data++ = sum;
 238
 239         if (PA_UNLIKELY(++channel >= channels))
 240             channel = 0;
 241     }
 242 }
 243
 244 static void pa_mix_s16ne_c(pa_mix_info streams[], unsigned nstreams, unsigned channels, int16_t *data, unsigned length) {
 245     if (nstreams == 2 && channels == 1)
 246         pa_mix2_ch1_s16ne(streams, data, length);
 247     else if (nstreams == 2 && channels == 2)
 248         pa_mix2_ch2_s16ne(streams, data, length);
 249     else if (nstreams == 2)
 250         pa_mix2_s16ne(streams, channels, data, length);
 251     else if (channels == 2)
 252         pa_mix_ch2_s16ne(streams, nstreams, data, length);
 253     else
 254         pa_mix_generic_s16ne(streams, nstreams, channels, data, length);
 255 }
 256
 257 static void pa_mix_s16re_c(pa_mix_info streams[], unsigned nstreams, unsigned channels, int16_t *data, unsigned length) {
 258     unsigned channel = 0;
 259
 260     length /= sizeof(int16_t);
 261
 262     for (; length > 0; length--, data++) {
 263         int32_t sum = 0;
 264         unsigned i;
 265
 266         for (i = 0; i < nstreams; i++) {
 267             pa_mix_info *m = streams + i;
 268             int32_t cv = m->linear[channel].i;
 269
 270             if (PA_LIKELY(cv > 0))
 271                 sum += pa_mult_s16_volume(PA_INT16_SWAP(*((int16_t*) m->ptr)), cv);
 272             m->ptr = (uint8_t*) m->ptr + sizeof(int16_t);
 273         }
 274
 275         sum = PA_CLAMP_UNLIKELY(sum, -0x8000, 0x7FFF);
 276         *data = PA_INT16_SWAP((int16_t) sum);
 277
 278         if (PA_UNLIKELY(++channel >= channels))
 279             channel = 0;
 280     }
 281 }
 282
 283 static void pa_mix_s32ne_c(pa_mix_info streams[], unsigned nstreams, unsigned channels, int32_t *data, unsigned length) {
 284     unsigned channel = 0;
 285
 286     length /= sizeof(int32_t);
 287
 288     for (; length > 0; length--, data++) {
 289         int64_t sum = 0;
 290         unsigned i;
 291
 292         for (i = 0; i < nstreams; i++) {
 293             pa_mix_info *m = streams + i;
 294             int32_t cv = m->linear[channel].i;
 295             int64_t v;
 296
 297             if (PA_LIKELY(cv > 0)) {
 298                 v = *((int32_t*) m->ptr);
 299                 v = (v * cv) >> 16;
 300                 sum += v;
 301             }
 302             m->ptr = (uint8_t*) m->ptr + sizeof(int32_t);
 303         }
 304
 305         sum = PA_CLAMP_UNLIKELY(sum, -0x80000000LL, 0x7FFFFFFFLL);
 306         *data = (int32_t) sum;
 307
 308         if (PA_UNLIKELY(++channel >= channels))
 309             channel = 0;
 310     }
 311 }
 312
 313 static void pa_mix_s32re_c(pa_mix_info streams[], unsigned nstreams, unsigned channels, int32_t *data, unsigned length) {
 314     unsigned channel = 0;
 315
 316     length /= sizeof(int32_t);
 317
 318     for (; length > 0; length--, data++) {
 319         int64_t sum = 0;
 320         unsigned i;
 321
 322         for (i = 0; i < nstreams; i++) {
 323             pa_mix_info *m = streams + i;
 324             int32_t cv = m->linear[channel].i;
 325             int64_t v;
 326
 327             if (PA_LIKELY(cv > 0)) {
 328                 v = PA_INT32_SWAP(*((int32_t*) m->ptr));
 329                 v = (v * cv) >> 16;
 330                 sum += v;
 331             }
 332             m->ptr = (uint8_t*) m->ptr + sizeof(int32_t);
 333         }
 334
 335         sum = PA_CLAMP_UNLIKELY(sum, -0x80000000LL, 0x7FFFFFFFLL);
 336         *data = PA_INT32_SWAP((int32_t) sum);
 337
 338         if (PA_UNLIKELY(++channel >= channels))
 339             channel = 0;
 340     }
 341 }
 342
 343 static void pa_mix_s24ne_c(pa_mix_info streams[], unsigned nstreams, unsigned channels, uint8_t *data, unsigned length) {
 344     unsigned channel = 0;
 345
 346     for (; length > 0; length -= 3, data += 3) {
 347         int64_t sum = 0;
 348         unsigned i;
 349
 350         for (i = 0; i < nstreams; i++) {
 351             pa_mix_info *m = streams + i;
 352             int32_t cv = m->linear[channel].i;
 353             int64_t v;
 354
 355             if (PA_LIKELY(cv > 0)) {
 356                 v = (int32_t) (PA_READ24NE(m->ptr) << 8);
 357                 v = (v * cv) >> 16;
 358                 sum += v;
 359             }
 360             m->ptr = (uint8_t*) m->ptr + 3;
 361         }
 362
 363         sum = PA_CLAMP_UNLIKELY(sum, -0x80000000LL, 0x7FFFFFFFLL);
 364         PA_WRITE24NE(data, ((uint32_t) sum) >> 8);
 365
 366         if (PA_UNLIKELY(++channel >= channels))
 367             channel = 0;
 368     }
 369 }
 370
 371 static void pa_mix_s24re_c(pa_mix_info streams[], unsigned nstreams, unsigned channels, uint8_t *data, unsigned length) {
 372     unsigned channel = 0;
 373
 374     for (; length > 0; length -= 3, data += 3) {
 375         int64_t sum = 0;
 376         unsigned i;
 377
 378         for (i = 0; i < nstreams; i++) {
 379             pa_mix_info *m = streams + i;
 380             int32_t cv = m->linear[channel].i;
 381             int64_t v;
 382
 383             if (PA_LIKELY(cv > 0)) {
 384                 v = (int32_t) (PA_READ24RE(m->ptr) << 8);
 385                 v = (v * cv) >> 16;
 386                 sum += v;
 387             }
 388             m->ptr = (uint8_t*) m->ptr + 3;
 389         }
 390
 391         sum = PA_CLAMP_UNLIKELY(sum, -0x80000000LL, 0x7FFFFFFFLL);
 392         PA_WRITE24RE(data, ((uint32_t) sum) >> 8);
 393
 394         if (PA_UNLIKELY(++channel >= channels))
 395             channel = 0;
 396     }
 397 }
 398
 399 static void pa_mix_s24_32ne_c(pa_mix_info streams[], unsigned nstreams, unsigned channels, uint32_t *data, unsigned length) {
 400     unsigned channel = 0;
 401
 402     length /= sizeof(uint32_t);
 403
 404     for (; length > 0; length--, data++) {
 405         int64_t sum = 0;
 406         unsigned i;
 407
 408         for (i = 0; i < nstreams; i++) {
 409             pa_mix_info *m = streams + i;
 410             int32_t cv = m->linear[channel].i;
 411             int64_t v;
 412
 413             if (PA_LIKELY(cv > 0)) {
 414                 v = (int32_t) (*((uint32_t*)m->ptr) << 8);
 415                 v = (v * cv) >> 16;
 416                 sum += v;
 417             }
 418             m->ptr = (uint8_t*) m->ptr + sizeof(int32_t);
 419         }
 420
 421         sum = PA_CLAMP_UNLIKELY(sum, -0x80000000LL, 0x7FFFFFFFLL);
 422         *data = ((uint32_t) (int32_t) sum) >> 8;
 423
 424         if (PA_UNLIKELY(++channel >= channels))
 425             channel = 0;
 426     }
 427 }
 428
 429 static void pa_mix_s24_32re_c(pa_mix_info streams[], unsigned nstreams, unsigned channels, uint32_t *data, unsigned length) {
 430     unsigned channel = 0;
 431
 432     length /= sizeof(uint32_t);
 433
 434     for (; length > 0; length--, data++) {
 435         int64_t sum = 0;
 436         unsigned i;
 437
 438         for (i = 0; i < nstreams; i++) {
 439             pa_mix_info *m = streams + i;
 440             int32_t cv = m->linear[channel].i;
 441             int64_t v;
 442
 443             if (PA_LIKELY(cv > 0)) {
 444                 v = (int32_t) (PA_UINT32_SWAP(*((uint32_t*) m->ptr)) << 8);
 445                 v = (v * cv) >> 16;
 446                 sum += v;
 447             }
 448             m->ptr = (uint8_t*) m->ptr + 3;
 449         }
 450
 451         sum = PA_CLAMP_UNLIKELY(sum, -0x80000000LL, 0x7FFFFFFFLL);
 452         *data = PA_INT32_SWAP(((uint32_t) (int32_t) sum) >> 8);
 453
 454         if (PA_UNLIKELY(++channel >= channels))
 455             channel = 0;
 456     }
 457 }
 458
 459 static void pa_mix_u8_c(pa_mix_info streams[], unsigned nstreams, unsigned channels, uint8_t *data, unsigned length) {
 460     unsigned channel = 0;
 461
 462     length /= sizeof(uint8_t);
 463
 464     for (; length > 0; length--, data++) {
 465         int32_t sum = 0;
 466         unsigned i;
 467
 468         for (i = 0; i < nstreams; i++) {
 469             pa_mix_info *m = streams + i;
 470             int32_t v, cv = m->linear[channel].i;
 471
 472             if (PA_LIKELY(cv > 0)) {
 473                 v = (int32_t) *((uint8_t*) m->ptr) - 0x80;
 474                 v = (v * cv) >> 16;
 475                 sum += v;
 476             }
 477             m->ptr = (uint8_t*) m->ptr + 1;
 478         }
 479
 480         sum = PA_CLAMP_UNLIKELY(sum, -0x80, 0x7F);
 481         *data = (uint8_t) (sum + 0x80);
 482
 483         if (PA_UNLIKELY(++channel >= channels))
 484             channel = 0;
 485     }
 486 }
 487
 488 static void pa_mix_ulaw_c(pa_mix_info streams[], unsigned nstreams, unsigned channels, uint8_t *data, unsigned length) {
 489     unsigned channel = 0;
 490
 491     length /= sizeof(uint8_t);
 492
 493     for (; length > 0; length--, data++) {
 494         int32_t sum = 0;
 495         unsigned i;
 496
 497         for (i = 0; i < nstreams; i++) {
 498             pa_mix_info *m = streams + i;
 499             int32_t cv = m->linear[channel].i;
 500
 501             if (PA_LIKELY(cv > 0))
 502                 sum += pa_mult_s16_volume(st_ulaw2linear16(*((uint8_t*) m->ptr)), cv);
 503             m->ptr = (uint8_t*) m->ptr + 1;
 504         }
 505
 506         sum = PA_CLAMP_UNLIKELY(sum, -0x8000, 0x7FFF);
 507         *data = (uint8_t) st_14linear2ulaw((int16_t) sum >> 2);
 508
 509         if (PA_UNLIKELY(++channel >= channels))
 510             channel = 0;
 511     }
 512 }
 513
 514 static void pa_mix_alaw_c(pa_mix_info streams[], unsigned nstreams, unsigned channels, uint8_t *data, unsigned length) {
 515     unsigned channel = 0;
 516
 517     length /= sizeof(uint8_t);
 518
 519     for (; length > 0; length--, data++) {
 520         int32_t sum = 0;
 521         unsigned i;
 522
 523         for (i = 0; i < nstreams; i++) {
 524             pa_mix_info *m = streams + i;
 525             int32_t cv = m->linear[channel].i;
 526
 527             if (PA_LIKELY(cv > 0))
 528                 sum += pa_mult_s16_volume(st_alaw2linear16(*((uint8_t*) m->ptr)), cv);
 529             m->ptr = (uint8_t*) m->ptr + 1;
 530         }
 531
 532         sum = PA_CLAMP_UNLIKELY(sum, -0x8000, 0x7FFF);
 533         *data = (uint8_t) st_13linear2alaw((int16_t) sum >> 3);
 534
 535         if (PA_UNLIKELY(++channel >= channels))
 536             channel = 0;
 537     }
 538 }
 539
 540 static void pa_mix_float32ne_c(pa_mix_info streams[], unsigned nstreams, unsigned channels, float *data, unsigned length) {
 541     unsigned channel = 0;
 542
 543     length /= sizeof(float);
 544
 545     for (; length > 0; length--, data++) {
 546         float sum = 0;
 547         unsigned i;
 548
 549         for (i = 0; i < nstreams; i++) {
 550             pa_mix_info *m = streams + i;
 551             float v, cv = m->linear[channel].f;
 552
 553             if (PA_LIKELY(cv > 0)) {
 554                 v = *((float*) m->ptr);
 555                 v *= cv;
 556                 sum += v;
 557             }
 558             m->ptr = (uint8_t*) m->ptr + sizeof(float);
 559         }
 560
 561         *data = sum;
 562
 563         if (PA_UNLIKELY(++channel >= channels))
 564             channel = 0;
 565     }
 566 }
 567
 568 static void pa_mix_float32re_c(pa_mix_info streams[], unsigned nstreams, unsigned channels, float *data, unsigned length) {
 569     unsigned channel = 0;
 570
 571     length /= sizeof(float);
 572
 573     for (; length > 0; length--, data++) {
 574         float sum = 0;
 575         unsigned i;
 576
 577         for (i = 0; i < nstreams; i++) {
 578             pa_mix_info *m = streams + i;
 579             float v, cv = m->linear[channel].f;
 580
 581             if (PA_LIKELY(cv > 0)) {
 582                 v = PA_FLOAT32_SWAP(*(float*) m->ptr);
 583                 v *= cv;
 584                 sum += v;
 585             }
 586             m->ptr = (uint8_t*) m->ptr + sizeof(float);
 587         }
 588
 589         *data = PA_FLOAT32_SWAP(sum);
 590
 591         if (PA_UNLIKELY(++channel >= channels))
 592             channel = 0;
 593     }
 594 }
 595
 596 static pa_do_mix_func_t do_mix_table[] = {
 597     [PA_SAMPLE_U8]        = (pa_do_mix_func_t) pa_mix_u8_c,
 598     [PA_SAMPLE_ALAW]      = (pa_do_mix_func_t) pa_mix_alaw_c,
 599     [PA_SAMPLE_ULAW]      = (pa_do_mix_func_t) pa_mix_ulaw_c,
 600     [PA_SAMPLE_S16NE]     = (pa_do_mix_func_t) pa_mix_s16ne_c,
 601     [PA_SAMPLE_S16RE]     = (pa_do_mix_func_t) pa_mix_s16re_c,
 602     [PA_SAMPLE_FLOAT32NE] = (pa_do_mix_func_t) pa_mix_float32ne_c,
 603     [PA_SAMPLE_FLOAT32RE] = (pa_do_mix_func_t) pa_mix_float32re_c,
 604     [PA_SAMPLE_S32NE]     = (pa_do_mix_func_t) pa_mix_s32ne_c,
 605     [PA_SAMPLE_S32RE]     = (pa_do_mix_func_t) pa_mix_s32re_c,
 606     [PA_SAMPLE_S24NE]     = (pa_do_mix_func_t) pa_mix_s24ne_c,
 607     [PA_SAMPLE_S24RE]     = (pa_do_mix_func_t) pa_mix_s24re_c,
 608     [PA_SAMPLE_S24_32NE]  = (pa_do_mix_func_t) pa_mix_s24_32ne_c,
 609     [PA_SAMPLE_S24_32RE]  = (pa_do_mix_func_t) pa_mix_s24_32re_c
 610 };
 611
 612 size_t pa_mix(
 613         pa_mix_info streams[],
 614         unsigned nstreams,
 615         void *data,
 616         size_t length,
 617         const pa_sample_spec *spec,
 618         const pa_cvolume *volume,
 619         bool mute) {
 620
 621     pa_cvolume full_volume;
 622     unsigned k;
 623
 624     pa_assert(streams);
 625     pa_assert(data);
 626     pa_assert(length);
 627     pa_assert(spec);
 628
 629     if (!volume)
 630         volume = pa_cvolume_reset(&full_volume, spec->channels);
 631
 632     if (mute || pa_cvolume_is_muted(volume) || nstreams <= 0) {
 633         pa_silence_memory(data, length, spec);
 634         return length;
 635     }
 636
 637     for (k = 0; k < nstreams; k++) {
 638         streams[k].ptr = pa_memblock_acquire_chunk(&streams[k].chunk);
 639         if (length > streams[k].chunk.length)
 640             length = streams[k].chunk.length;
 641     }
 642
 643     calc_stream_volumes_table[spec->format](streams, nstreams, volume, spec);
 644     do_mix_table[spec->format](streams, nstreams, spec->channels, data, length);
 645
 646     for (k = 0; k < nstreams; k++)
 647         pa_memblock_release(streams[k].chunk.memblock);
 648
 649     return length;
 650 }
 651
 652 pa_do_mix_func_t pa_get_mix_func(pa_sample_format_t f) {
 653     pa_assert(pa_sample_format_valid(f));
 654
 655     return do_mix_table[f];
 656 }
 657
 658 void pa_set_mix_func(pa_sample_format_t f, pa_do_mix_func_t func) {
 659     pa_assert(pa_sample_format_valid(f));
 660
 661     do_mix_table[f] = func;
 662 }
 663
 664 typedef union {
 665   float f;
 666   uint32_t i;
 667 } volume_val;
 668
 669 typedef void (*pa_calc_volume_func_t) (void *volumes, const pa_cvolume *volume);
 670
 671 static const pa_calc_volume_func_t calc_volume_table[] = {
 672   [PA_SAMPLE_U8]        = (pa_calc_volume_func_t) calc_linear_integer_volume,
 673   [PA_SAMPLE_ALAW]      = (pa_calc_volume_func_t) calc_linear_integer_volume,
 674   [PA_SAMPLE_ULAW]      = (pa_calc_volume_func_t) calc_linear_integer_volume,
 675   [PA_SAMPLE_S16LE]     = (pa_calc_volume_func_t) calc_linear_integer_volume,
 676   [PA_SAMPLE_S16BE]     = (pa_calc_volume_func_t) calc_linear_integer_volume,
 677   [PA_SAMPLE_FLOAT32LE] = (pa_calc_volume_func_t) calc_linear_float_volume,
 678   [PA_SAMPLE_FLOAT32BE] = (pa_calc_volume_func_t) calc_linear_float_volume,
 679   [PA_SAMPLE_S32LE]     = (pa_calc_volume_func_t) calc_linear_integer_volume,
 680   [PA_SAMPLE_S32BE]     = (pa_calc_volume_func_t) calc_linear_integer_volume,
 681   [PA_SAMPLE_S24LE]     = (pa_calc_volume_func_t) calc_linear_integer_volume,
 682   [PA_SAMPLE_S24BE]     = (pa_calc_volume_func_t) calc_linear_integer_volume,
 683   [PA_SAMPLE_S24_32LE]  = (pa_calc_volume_func_t) calc_linear_integer_volume,
 684   [PA_SAMPLE_S24_32BE]  = (pa_calc_volume_func_t) calc_linear_integer_volume
 685 };
 686
 687 void pa_volume_memchunk(
 688         pa_memchunk*c,
 689         const pa_sample_spec *spec,
 690         const pa_cvolume *volume) {
 691
 692     void *ptr;
 693     volume_val linear[PA_CHANNELS_MAX + VOLUME_PADDING];
 694     pa_do_volume_func_t do_volume;
 695
 696     pa_assert(c);
 697     pa_assert(spec);
 698     pa_assert(pa_sample_spec_valid(spec));
 699     pa_assert(pa_frame_aligned(c->length, spec));
 700     pa_assert(volume);
 701
 702     if (pa_memblock_is_silence(c->memblock))
 703         return;
 704
 705     if (pa_cvolume_channels_equal_to(volume, PA_VOLUME_NORM))
 706         return;
 707
 708     if (pa_cvolume_channels_equal_to(volume, PA_VOLUME_MUTED)) {
 709         pa_silence_memchunk(c, spec);
 710         return;
 711     }
 712
 713     do_volume = pa_get_volume_func(spec->format);
 714     pa_assert(do_volume);
 715
 716     calc_volume_table[spec->format] ((void *)linear, volume);
 717
 718     ptr = pa_memblock_acquire_chunk(c);
 719
 720     do_volume(ptr, (void *)linear, spec->channels, c->length);
 721
 722     pa_memblock_release(c->memblock);
 723 }
 724
 725 static void calc_linear_integer_volume_no_mapping(int32_t linear[], float volume[], unsigned nchannels) {
 726     unsigned channel, padding;
 727
 728     pa_assert(linear);
 729     pa_assert(volume);
 730
 731     for (channel = 0; channel < nchannels; channel++)
 732         linear[channel] = (int32_t) lrint(volume[channel] * 0x10000U);
 733
 734     for (padding = 0; padding < VOLUME_PADDING; padding++, channel++)
 735         linear[channel] = linear[padding];
 736 }
 737
 738 static void calc_linear_float_volume_no_mapping(float linear[], float volume[], unsigned nchannels) {
 739     unsigned channel, padding;
 740
 741     pa_assert(linear);
 742     pa_assert(volume);
 743
 744     for (channel = 0; channel < nchannels; channel++)
 745         linear[channel] = volume[channel];
 746
 747     for (padding = 0; padding < VOLUME_PADDING; padding++, channel++)
 748         linear[channel] = linear[padding];
 749 }
 750
 751 typedef void (*pa_calc_volume_no_mapping_func_t) (void *volumes, float *volume, int channels);
 752
 753 static const pa_calc_volume_no_mapping_func_t calc_volume_table_no_mapping[] = {
 754   [PA_SAMPLE_U8]        = (pa_calc_volume_no_mapping_func_t) calc_linear_integer_volume_no_mapping,
 755   [PA_SAMPLE_ALAW]      = (pa_calc_volume_no_mapping_func_t) calc_linear_integer_volume_no_mapping,
 756   [PA_SAMPLE_ULAW]      = (pa_calc_volume_no_mapping_func_t) calc_linear_integer_volume_no_mapping,
 757   [PA_SAMPLE_S16LE]     = (pa_calc_volume_no_mapping_func_t) calc_linear_integer_volume_no_mapping,
 758   [PA_SAMPLE_S16BE]     = (pa_calc_volume_no_mapping_func_t) calc_linear_integer_volume_no_mapping,
 759   [PA_SAMPLE_FLOAT32LE] = (pa_calc_volume_no_mapping_func_t) calc_linear_float_volume_no_mapping,
 760   [PA_SAMPLE_FLOAT32BE] = (pa_calc_volume_no_mapping_func_t) calc_linear_float_volume_no_mapping,
 761   [PA_SAMPLE_S32LE]     = (pa_calc_volume_no_mapping_func_t) calc_linear_integer_volume_no_mapping,
 762   [PA_SAMPLE_S32BE]     = (pa_calc_volume_no_mapping_func_t) calc_linear_integer_volume_no_mapping,
 763   [PA_SAMPLE_S24LE]     = (pa_calc_volume_no_mapping_func_t) calc_linear_integer_volume_no_mapping,
 764   [PA_SAMPLE_S24BE]     = (pa_calc_volume_no_mapping_func_t) calc_linear_integer_volume_no_mapping,
 765   [PA_SAMPLE_S24_32LE]  = (pa_calc_volume_no_mapping_func_t) calc_linear_integer_volume_no_mapping,
 766   [PA_SAMPLE_S24_32BE]  = (pa_calc_volume_no_mapping_func_t) calc_linear_integer_volume_no_mapping
 767 };
 768
 769 static const unsigned format_sample_size_table[] = {
 770   [PA_SAMPLE_U8]        = 1,
 771   [PA_SAMPLE_ALAW]      = 1,
 772   [PA_SAMPLE_ULAW]      = 1,
 773   [PA_SAMPLE_S16LE]     = 2,
 774   [PA_SAMPLE_S16BE]     = 2,
 775   [PA_SAMPLE_FLOAT32LE] = 4,
 776   [PA_SAMPLE_FLOAT32BE] = 4,
 777   [PA_SAMPLE_S32LE]     = 4,
 778   [PA_SAMPLE_S32BE]     = 4,
 779   [PA_SAMPLE_S24LE]     = 3,
 780   [PA_SAMPLE_S24BE]     = 3,
 781   [PA_SAMPLE_S24_32LE]  = 4,
 782   [PA_SAMPLE_S24_32BE]  = 4
 783 };
 784
 785 static float calc_volume_ramp_linear(pa_volume_ramp_int_t *ramp) {
 786     pa_assert(ramp);
 787     pa_assert(ramp->length > 0);
 788
 789     /* basic linear interpolation */
 790     return ramp->start + (ramp->length - ramp->left) * (ramp->end - ramp->start) / (float) ramp->length;
 791 }
 792
 793 static float calc_volume_ramp_logarithmic(pa_volume_ramp_int_t *ramp) {
 794     float x_val, s, e;
 795     long temp;
 796
 797     pa_assert(ramp);
 798     pa_assert(ramp->length > 0);
 799
 800     if (ramp->end > ramp->start) {
 801         temp = ramp->left;
 802         s = ramp->end;
 803         e = ramp->start;
 804     } else {
 805         temp = ramp->length - ramp->left;
 806         s = ramp->start;
 807         e = ramp->end;
 808     }
 809
 810     x_val = temp == 0 ? 0.0 : powf(temp, 10);
 811
 812     /* base 10 logarithmic interpolation */
 813     return s + x_val * (e - s) / powf(ramp->length, 10);
 814 }
 815
 816 static float calc_volume_ramp_cubic(pa_volume_ramp_int_t *ramp) {
 817     float x_val, s, e;
 818     long temp;
 819
 820     pa_assert(ramp);
 821     pa_assert(ramp->length > 0);
 822
 823     if (ramp->end > ramp->start) {
 824         temp = ramp->left;
 825         s = ramp->end;
 826         e = ramp->start;
 827     } else {
 828         temp = ramp->length - ramp->left;
 829         s = ramp->start;
 830         e = ramp->end;
 831     }
 832
 833     x_val = temp == 0 ? 0.0 : cbrtf(temp);
 834
 835     /* cubic interpolation */
 836     return s + x_val * (e - s) / cbrtf(ramp->length);
 837 }
 838
 839 typedef float (*pa_calc_volume_ramp_func_t) (pa_volume_ramp_int_t *);
 840
 841 static const pa_calc_volume_ramp_func_t calc_volume_ramp_table[] = {
 842     [PA_VOLUME_RAMP_TYPE_LINEAR] = (pa_calc_volume_ramp_func_t) calc_volume_ramp_linear,
 843     [PA_VOLUME_RAMP_TYPE_LOGARITHMIC] = (pa_calc_volume_ramp_func_t) calc_volume_ramp_logarithmic,
 844     [PA_VOLUME_RAMP_TYPE_CUBIC] = (pa_calc_volume_ramp_func_t) calc_volume_ramp_cubic
 845 };
 846
 847 static void calc_volume_ramps(pa_cvolume_ramp_int *ram, float *vol)
 848 {
 849     int i;
 850
 851     for (i = 0; i < ram->channels; i++) {
 852         if (ram->ramps[i].left <= 0) {
 853             if (ram->ramps[i].target == PA_VOLUME_NORM) {
 854                 vol[i] = 1.0;
 855             }
 856         } else {
 857             vol[i] = ram->ramps[i].curr = calc_volume_ramp_table[ram->ramps[i].type] (&ram->ramps[i]);
 858             ram->ramps[i].left--;
 859         }
 860     }
 861 }
 862
 863 void pa_volume_ramp_memchunk(
 864         pa_memchunk *c,
 865         const pa_sample_spec *spec,
 866         pa_cvolume_ramp_int *ramp) {
 867
 868     void *ptr;
 869     volume_val linear[PA_CHANNELS_MAX + VOLUME_PADDING];
 870     float vol[PA_CHANNELS_MAX + VOLUME_PADDING];
 871     pa_do_volume_func_t do_volume;
 872     long length_in_frames;
 873     int i;
 874
 875     pa_assert(c);
 876     pa_assert(spec);
 877     pa_assert(pa_frame_aligned(c->length, spec));
 878     pa_assert(ramp);
 879
 880     length_in_frames = c->length / format_sample_size_table[spec->format] / spec->channels;
 881
 882     if (pa_memblock_is_silence(c->memblock)) {
 883         for (i = 0; i < ramp->channels; i++) {
 884             if (ramp->ramps[i].length > 0)
 885                 ramp->ramps[i].length -= length_in_frames;
 886         }
 887         return;
 888     }
 889
 890     if (spec->format < 0 || spec->format >= PA_SAMPLE_MAX) {
 891       pa_log_warn("Unable to change volume of format");
 892       return;
 893     }
 894
 895     do_volume = pa_get_volume_func(spec->format);
 896     pa_assert(do_volume);
 897
 898     ptr = (uint8_t*) pa_memblock_acquire(c->memblock) + c->index;
 899
 900     for (i = 0; i < length_in_frames; i++) {
 901         calc_volume_ramps(ramp, vol);
 902         calc_volume_table_no_mapping[spec->format] ((void *)linear, vol, spec->channels);
 903
 904         /* we only process one frame per iteration */
 905         do_volume (ptr, (void *)linear, spec->channels, format_sample_size_table[spec->format] * spec->channels);
 906
 907         /* pa_log_debug("1: %d  2: %d", linear[0], linear[1]); */
 908
 909         ptr = (uint8_t*)ptr + format_sample_size_table[spec->format] * spec->channels;
 910     }
 911
 912     pa_memblock_release(c->memblock);
 913 }
 914
 915 pa_cvolume_ramp_int* pa_cvolume_ramp_convert(const pa_cvolume_ramp *src, pa_cvolume_ramp_int *dst, int sample_rate) {
 916
 917     int i, j, channels, remaining_channels;
 918     float temp;
 919
 920     if (dst->channels < src->channels) {
 921         channels = dst->channels;
 922         remaining_channels = 0;
 923     }
 924     else {
 925         channels = src->channels;
 926         remaining_channels = dst->channels;
 927     }
 928
 929     for (i = 0; i < channels; i++) {
 930         dst->ramps[i].type = src->ramps[i].type;
 931         /* ms to samples */
 932         dst->ramps[i].length = src->ramps[i].length * sample_rate / 1000;
 933         dst->ramps[i].left = dst->ramps[i].length;
 934         dst->ramps[i].start = dst->ramps[i].end;
 935         dst->ramps[i].target = src->ramps[i].target;
 936         /* scale to pulse internal mapping so that when ramp is over there's no glitch in volume */
 937         temp = src->ramps[i].target / (float)0x10000U;
 938         dst->ramps[i].end = temp * temp * temp;
 939     }
 940
 941     j = i;
 942
 943     for (i--; j < remaining_channels; j++) {
 944         dst->ramps[j].type = dst->ramps[i].type;
 945         dst->ramps[j].length = dst->ramps[i].length;
 946         dst->ramps[j].left = dst->ramps[i].left;
 947         dst->ramps[j].start = dst->ramps[i].start;
 948         dst->ramps[j].target = dst->ramps[i].target;
 949         dst->ramps[j].end = dst->ramps[i].end;
 950     }
 951
 952     return dst;
 953 }
 954
 955 bool pa_cvolume_ramp_active(pa_cvolume_ramp_int *ramp) {
 956     int i;
 957
 958     for (i = 0; i < ramp->channels; i++) {
 959         if (ramp->ramps[i].left > 0)
 960             return true;
 961     }
 962
 963     return false;
 964 }
 965
 966 bool pa_cvolume_ramp_target_active(pa_cvolume_ramp_int *ramp) {
 967     int i;
 968
 969     for (i = 0; i < ramp->channels; i++) {
 970         if (ramp->ramps[i].target != PA_VOLUME_NORM)
 971             return true;
 972     }
 973
 974     return false;
 975 }
 976
 977 pa_cvolume * pa_cvolume_ramp_get_targets(pa_cvolume_ramp_int *ramp, pa_cvolume *volume) {
 978     int i = 0;
 979
 980     volume->channels = ramp->channels;
 981
 982     for (i = 0; i < ramp->channels; i++)
 983         volume->values[i] = ramp->ramps[i].target;
 984
 985     return volume;
 986 }
 987
 988 pa_cvolume_ramp_int* pa_cvolume_ramp_start_from(pa_cvolume_ramp_int *src, pa_cvolume_ramp_int *dst) {
 989     int i;
 990
 991     for (i = 0; i < src->channels; i++) {
 992         /* if new vols are invalid, copy old ramp i.e. no effect */
 993         if (dst->ramps[i].target == PA_VOLUME_INVALID)
 994             dst->ramps[i] = src->ramps[i];
 995         /* if there's some old ramp still left */
 996         else if (src->ramps[i].left > 0)
 997             dst->ramps[i].start = src->ramps[i].curr;
 998     }
 999
1000     return dst;
1001 }
1002
1003 pa_cvolume_ramp_int* pa_cvolume_ramp_int_init(pa_cvolume_ramp_int *src, pa_volume_t vol, int channels) {
1004     int i;
1005     float temp;
1006
1007     src->channels = channels;
1008
1009     for (i = 0; i < channels; i++) {
1010         src->ramps[i].type = PA_VOLUME_RAMP_TYPE_LINEAR;
1011         src->ramps[i].length = 0;
1012         src->ramps[i].left = 0;
1013         if (vol == PA_VOLUME_NORM) {
1014             src->ramps[i].start = 1.0;
1015             src->ramps[i].end = 1.0;
1016             src->ramps[i].curr = 1.0;
1017         }
1018         else if (vol == PA_VOLUME_MUTED) {
1019             src->ramps[i].start = 0.0;
1020             src->ramps[i].end = 0.0;
1021             src->ramps[i].curr = 0.0;
1022         }
1023         else {
1024             temp = vol / (float)0x10000U;
1025             src->ramps[i].start = temp * temp * temp;
1026             src->ramps[i].end = src->ramps[i].start;
1027             src->ramps[i].curr = src->ramps[i].start;
1028         }
1029         src->ramps[i].target = vol;
1030     }
1031
1032     return src;
1033 }