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