src/pulsecore/sample-util.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
   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 <stdio.h>
  28 #include <string.h>
  29 #include <stdlib.h>
  30 #include <stdio.h>
  31 #include <errno.h>
  32
  33 #include <liboil/liboilfuncs.h>
  34 #include <liboil/liboil.h>
  35
  36 #include <pulse/timeval.h>
  37
  38 #include <pulsecore/log.h>
  39 #include <pulsecore/core-error.h>
  40 #include <pulsecore/macro.h>
  41 #include <pulsecore/g711.h>
  42 #include <pulsecore/core-util.h>
  43
  44 #include "sample-util.h"
  45 #include "endianmacros.h"
  46
  47 #define PA_SILENCE_MAX (PA_PAGE_SIZE*16)
  48
  49 pa_memblock *pa_silence_memblock(pa_memblock* b, const pa_sample_spec *spec) {
  50     void *data;
  51
  52     pa_assert(b);
  53     pa_assert(spec);
  54
  55     data = pa_memblock_acquire(b);
  56     pa_silence_memory(data, pa_memblock_get_length(b), spec);
  57     pa_memblock_release(b);
  58
  59     return b;
  60 }
  61
  62 pa_memchunk* pa_silence_memchunk(pa_memchunk *c, const pa_sample_spec *spec) {
  63     void *data;
  64
  65     pa_assert(c);
  66     pa_assert(c->memblock);
  67     pa_assert(spec);
  68
  69     data = pa_memblock_acquire(c->memblock);
  70     pa_silence_memory((uint8_t*) data+c->index, c->length, spec);
  71     pa_memblock_release(c->memblock);
  72
  73     return c;
  74 }
  75
  76 static uint8_t silence_byte(pa_sample_format_t format) {
  77     switch (format) {
  78         case PA_SAMPLE_U8:
  79             return 0x80;
  80         case PA_SAMPLE_S16LE:
  81         case PA_SAMPLE_S16BE:
  82         case PA_SAMPLE_S32LE:
  83         case PA_SAMPLE_S32BE:
  84         case PA_SAMPLE_FLOAT32LE:
  85         case PA_SAMPLE_FLOAT32BE:
  86         case PA_SAMPLE_S24LE:
  87         case PA_SAMPLE_S24BE:
  88         case PA_SAMPLE_S24_32LE:
  89         case PA_SAMPLE_S24_32BE:
  90             return 0;
  91         case PA_SAMPLE_ALAW:
  92             return 0xd5;
  93         case PA_SAMPLE_ULAW:
  94             return 0xff;
  95         default:
  96             pa_assert_not_reached();
  97     }
  98 }
  99
 100 void* pa_silence_memory(void *p, size_t length, const pa_sample_spec *spec) {
 101     pa_assert(p);
 102     pa_assert(length > 0);
 103     pa_assert(spec);
 104
 105     memset(p, silence_byte(spec->format), length);
 106     return p;
 107 }
 108
 109 static void calc_linear_integer_volume(int32_t linear[], const pa_cvolume *volume) {
 110     unsigned channel;
 111
 112     pa_assert(linear);
 113     pa_assert(volume);
 114
 115     for (channel = 0; channel < volume->channels; channel++)
 116         linear[channel] = (int32_t) lrint(pa_sw_volume_to_linear(volume->values[channel]) * 0x10000);
 117 }
 118
 119 static void calc_linear_float_volume(float linear[], const pa_cvolume *volume) {
 120     unsigned channel;
 121
 122     pa_assert(linear);
 123     pa_assert(volume);
 124
 125     for (channel = 0; channel < volume->channels; channel++)
 126         linear[channel] = (float) pa_sw_volume_to_linear(volume->values[channel]);
 127 }
 128
 129 static void calc_linear_integer_stream_volumes(pa_mix_info streams[], unsigned nstreams, const pa_cvolume *volume, const pa_sample_spec *spec) {
 130     unsigned k, channel;
 131     float linear[PA_CHANNELS_MAX];
 132
 133     pa_assert(streams);
 134     pa_assert(spec);
 135     pa_assert(volume);
 136
 137     calc_linear_float_volume(linear, volume);
 138
 139     for (k = 0; k < nstreams; k++) {
 140
 141         for (channel = 0; channel < spec->channels; channel++) {
 142             pa_mix_info *m = streams + k;
 143             m->linear[channel].i = (int32_t) lrint(pa_sw_volume_to_linear(m->volume.values[channel]) * linear[channel] * 0x10000);
 144         }
 145     }
 146 }
 147
 148 static void calc_linear_float_stream_volumes(pa_mix_info streams[], unsigned nstreams, const pa_cvolume *volume, const pa_sample_spec *spec) {
 149     unsigned k, channel;
 150     float linear[PA_CHANNELS_MAX];
 151
 152     pa_assert(streams);
 153     pa_assert(spec);
 154     pa_assert(volume);
 155
 156     calc_linear_float_volume(linear, volume);
 157
 158     for (k = 0; k < nstreams; k++) {
 159
 160         for (channel = 0; channel < spec->channels; channel++) {
 161             pa_mix_info *m = streams + k;
 162             m->linear[channel].f = (float) (pa_sw_volume_to_linear(m->volume.values[channel]) * linear[channel]);
 163         }
 164     }
 165 }
 166
 167 size_t pa_mix(
 168         pa_mix_info streams[],
 169         unsigned nstreams,
 170         void *data,
 171         size_t length,
 172         const pa_sample_spec *spec,
 173         const pa_cvolume *volume,
 174         pa_bool_t mute) {
 175
 176     pa_cvolume full_volume;
 177     unsigned k;
 178     unsigned z;
 179     void *end;
 180
 181     pa_assert(streams);
 182     pa_assert(data);
 183     pa_assert(length);
 184     pa_assert(spec);
 185
 186     if (!volume)
 187         volume = pa_cvolume_reset(&full_volume, spec->channels);
 188
 189     if (mute || pa_cvolume_is_muted(volume) || nstreams <= 0) {
 190         pa_silence_memory(data, length, spec);
 191         return length;
 192     }
 193
 194     for (k = 0; k < nstreams; k++)
 195         streams[k].ptr = (uint8_t*) pa_memblock_acquire(streams[k].chunk.memblock) + streams[k].chunk.index;
 196
 197     for (z = 0; z < nstreams; z++)
 198         if (length > streams[z].chunk.length)
 199             length = streams[z].chunk.length;
 200
 201     end = (uint8_t*) data + length;
 202
 203     switch (spec->format) {
 204
 205         case PA_SAMPLE_S16NE:{
 206             unsigned channel = 0;
 207
 208             calc_linear_integer_stream_volumes(streams, nstreams, volume, spec);
 209
 210             while (data < end) {
 211                 int32_t sum = 0;
 212                 unsigned i;
 213
 214                 for (i = 0; i < nstreams; i++) {
 215                     pa_mix_info *m = streams + i;
 216                     int32_t v, lo, hi, cv = m->linear[channel].i;
 217
 218                     if (PA_UNLIKELY(cv <= 0))
 219                         continue;
 220
 221                     /* Multiplying the 32bit volume factor with the
 222                      * 16bit sample might result in an 48bit value. We
 223                      * want to do without 64 bit integers and hence do
 224                      * the multiplication independantly for the HI and
 225                      * LO part of the volume. */
 226
 227                     hi = cv >> 16;
 228                     lo = cv & 0xFFFF;
 229
 230                     v = *((int16_t*) m->ptr);
 231                     v = ((v * lo) >> 16) + (v * hi);
 232                     sum += v;
 233
 234                     m->ptr = (uint8_t*) m->ptr + sizeof(int16_t);
 235                 }
 236
 237                 sum = PA_CLAMP_UNLIKELY(sum, -0x8000, 0x7FFF);
 238                 *((int16_t*) data) = (int16_t) sum;
 239
 240                 data = (uint8_t*) data + sizeof(int16_t);
 241
 242                 if (PA_UNLIKELY(++channel >= spec->channels))
 243                     channel = 0;
 244             }
 245
 246             break;
 247         }
 248
 249         case PA_SAMPLE_S16RE:{
 250             unsigned channel = 0;
 251
 252             calc_linear_integer_stream_volumes(streams, nstreams, volume, spec);
 253
 254             while (data < end) {
 255                 int32_t sum = 0;
 256                 unsigned i;
 257
 258                 for (i = 0; i < nstreams; i++) {
 259                     pa_mix_info *m = streams + i;
 260                     int32_t v, lo, hi, cv = m->linear[channel].i;
 261
 262                     if (PA_UNLIKELY(cv <= 0))
 263                         continue;
 264
 265                     hi = cv >> 16;
 266                     lo = cv & 0xFFFF;
 267
 268                     v = PA_INT16_SWAP(*((int16_t*) m->ptr));
 269                     v = ((v * lo) >> 16) + (v * hi);
 270                     sum += v;
 271
 272                     m->ptr = (uint8_t*) m->ptr + sizeof(int16_t);
 273                 }
 274
 275                 sum = PA_CLAMP_UNLIKELY(sum, -0x8000, 0x7FFF);
 276                 *((int16_t*) data) = PA_INT16_SWAP((int16_t) sum);
 277
 278                 data = (uint8_t*) data + sizeof(int16_t);
 279
 280                 if (PA_UNLIKELY(++channel >= spec->channels))
 281                     channel = 0;
 282             }
 283
 284             break;
 285         }
 286
 287         case PA_SAMPLE_S32NE:{
 288             unsigned channel = 0;
 289
 290             calc_linear_integer_stream_volumes(streams, nstreams, volume, spec);
 291
 292             while (data < end) {
 293                 int64_t sum = 0;
 294                 unsigned i;
 295
 296                 for (i = 0; i < nstreams; i++) {
 297                     pa_mix_info *m = streams + i;
 298                     int32_t cv = m->linear[channel].i;
 299                     int64_t v;
 300
 301                     if (PA_UNLIKELY(cv <= 0))
 302                         continue;
 303
 304                     v = *((int32_t*) m->ptr);
 305                     v = (v * cv) >> 16;
 306                     sum += v;
 307
 308                     m->ptr = (uint8_t*) m->ptr + sizeof(int32_t);
 309                 }
 310
 311                 sum = PA_CLAMP_UNLIKELY(sum, -0x80000000LL, 0x7FFFFFFFLL);
 312                 *((int32_t*) data) = (int32_t) sum;
 313
 314                 data = (uint8_t*) data + sizeof(int32_t);
 315
 316                 if (PA_UNLIKELY(++channel >= spec->channels))
 317                     channel = 0;
 318             }
 319
 320             break;
 321         }
 322
 323         case PA_SAMPLE_S32RE:{
 324             unsigned channel = 0;
 325
 326             calc_linear_integer_stream_volumes(streams, nstreams, volume, spec);
 327
 328             while (data < end) {
 329                 int64_t sum = 0;
 330                 unsigned i;
 331
 332                 for (i = 0; i < nstreams; i++) {
 333                     pa_mix_info *m = streams + i;
 334                     int32_t cv = m->linear[channel].i;
 335                     int64_t v;
 336
 337                     if (PA_UNLIKELY(cv <= 0))
 338                         continue;
 339
 340                     v = PA_INT32_SWAP(*((int32_t*) m->ptr));
 341                     v = (v * cv) >> 16;
 342                     sum += v;
 343
 344                     m->ptr = (uint8_t*) m->ptr + sizeof(int32_t);
 345                 }
 346
 347                 sum = PA_CLAMP_UNLIKELY(sum, -0x80000000LL, 0x7FFFFFFFLL);
 348                 *((int32_t*) data) = PA_INT32_SWAP((int32_t) sum);
 349
 350                 data = (uint8_t*) data + sizeof(int32_t);
 351
 352                 if (PA_UNLIKELY(++channel >= spec->channels))
 353                     channel = 0;
 354             }
 355
 356             break;
 357         }
 358
 359         case PA_SAMPLE_S24NE: {
 360             unsigned channel = 0;
 361
 362             calc_linear_integer_stream_volumes(streams, nstreams, volume, spec);
 363
 364             while (data < end) {
 365                 int64_t sum = 0;
 366                 unsigned i;
 367
 368                 for (i = 0; i < nstreams; i++) {
 369                     pa_mix_info *m = streams + i;
 370                     int32_t cv = m->linear[channel].i;
 371                     int64_t v;
 372
 373                     if (PA_UNLIKELY(cv <= 0))
 374                         continue;
 375
 376                     v = (int32_t) (PA_READ24NE(m->ptr) << 8);
 377                     v = (v * cv) >> 16;
 378                     sum += v;
 379
 380                     m->ptr = (uint8_t*) m->ptr + 3;
 381                 }
 382
 383                 sum = PA_CLAMP_UNLIKELY(sum, -0x80000000LL, 0x7FFFFFFFLL);
 384                 PA_WRITE24NE(data, ((uint32_t) sum) >> 8);
 385
 386                 data = (uint8_t*) data + 3;
 387
 388                 if (PA_UNLIKELY(++channel >= spec->channels))
 389                     channel = 0;
 390             }
 391
 392             break;
 393         }
 394
 395         case PA_SAMPLE_S24RE: {
 396             unsigned channel = 0;
 397
 398             calc_linear_integer_stream_volumes(streams, nstreams, volume, spec);
 399
 400             while (data < end) {
 401                 int64_t sum = 0;
 402                 unsigned i;
 403
 404                 for (i = 0; i < nstreams; i++) {
 405                     pa_mix_info *m = streams + i;
 406                     int32_t cv = m->linear[channel].i;
 407                     int64_t v;
 408
 409                     if (PA_UNLIKELY(cv <= 0))
 410                         continue;
 411
 412                     v = (int32_t) (PA_READ24RE(m->ptr) << 8);
 413                     v = (v * cv) >> 16;
 414                     sum += v;
 415
 416                     m->ptr = (uint8_t*) m->ptr + 3;
 417                 }
 418
 419                 sum = PA_CLAMP_UNLIKELY(sum, -0x80000000LL, 0x7FFFFFFFLL);
 420                 PA_WRITE24RE(data, ((uint32_t) sum) >> 8);
 421
 422                 data = (uint8_t*) data + 3;
 423
 424                 if (PA_UNLIKELY(++channel >= spec->channels))
 425                     channel = 0;
 426             }
 427
 428             break;
 429         }
 430
 431         case PA_SAMPLE_S24_32NE: {
 432             unsigned channel = 0;
 433
 434             calc_linear_integer_stream_volumes(streams, nstreams, volume, spec);
 435
 436             while (data < end) {
 437                 int64_t sum = 0;
 438                 unsigned i;
 439
 440                 for (i = 0; i < nstreams; i++) {
 441                     pa_mix_info *m = streams + i;
 442                     int32_t cv = m->linear[channel].i;
 443                     int64_t v;
 444
 445                     if (PA_UNLIKELY(cv <= 0))
 446                         continue;
 447
 448                     v = (int32_t) (*((uint32_t*)m->ptr) << 8);
 449                     v = (v * cv) >> 16;
 450                     sum += v;
 451
 452                     m->ptr = (uint8_t*) m->ptr + sizeof(int32_t);
 453                 }
 454
 455                 sum = PA_CLAMP_UNLIKELY(sum, -0x80000000LL, 0x7FFFFFFFLL);
 456                 *((uint32_t*) data) = ((uint32_t) (int32_t) sum) >> 8;
 457
 458                 data = (uint8_t*) data + sizeof(uint32_t);
 459
 460                 if (PA_UNLIKELY(++channel >= spec->channels))
 461                     channel = 0;
 462             }
 463
 464             break;
 465         }
 466
 467         case PA_SAMPLE_S24_32RE: {
 468             unsigned channel = 0;
 469
 470             calc_linear_integer_stream_volumes(streams, nstreams, volume, spec);
 471
 472             while (data < end) {
 473                 int64_t sum = 0;
 474                 unsigned i;
 475
 476                 for (i = 0; i < nstreams; i++) {
 477                     pa_mix_info *m = streams + i;
 478                     int32_t cv = m->linear[channel].i;
 479                     int64_t v;
 480
 481                     if (PA_UNLIKELY(cv <= 0))
 482                         continue;
 483
 484                     v = (int32_t) (PA_UINT32_SWAP(*((uint32_t*) m->ptr)) << 8);
 485                     v = (v * cv) >> 16;
 486                     sum += v;
 487
 488                     m->ptr = (uint8_t*) m->ptr + 3;
 489                 }
 490
 491                 sum = PA_CLAMP_UNLIKELY(sum, -0x80000000LL, 0x7FFFFFFFLL);
 492                 *((uint32_t*) data) = PA_INT32_SWAP(((uint32_t) (int32_t) sum) >> 8);
 493
 494                 data = (uint8_t*) data + sizeof(uint32_t);
 495
 496                 if (PA_UNLIKELY(++channel >= spec->channels))
 497                     channel = 0;
 498             }
 499
 500             break;
 501         }
 502
 503         case PA_SAMPLE_U8: {
 504             unsigned channel = 0;
 505
 506             calc_linear_integer_stream_volumes(streams, nstreams, volume, spec);
 507
 508             while (data < end) {
 509                 int32_t sum = 0;
 510                 unsigned i;
 511
 512                 for (i = 0; i < nstreams; i++) {
 513                     pa_mix_info *m = streams + i;
 514                     int32_t v, cv = m->linear[channel].i;
 515
 516                     if (PA_UNLIKELY(cv <= 0))
 517                         continue;
 518
 519                     v = (int32_t) *((uint8_t*) m->ptr) - 0x80;
 520                     v = (v * cv) >> 16;
 521                     sum += v;
 522
 523                     m->ptr = (uint8_t*) m->ptr + 1;
 524                 }
 525
 526                 sum = PA_CLAMP_UNLIKELY(sum, -0x80, 0x7F);
 527                 *((uint8_t*) data) = (uint8_t) (sum + 0x80);
 528
 529                 data = (uint8_t*) data + 1;
 530
 531                 if (PA_UNLIKELY(++channel >= spec->channels))
 532                     channel = 0;
 533             }
 534
 535             break;
 536         }
 537
 538         case PA_SAMPLE_ULAW: {
 539             unsigned channel = 0;
 540
 541             calc_linear_integer_stream_volumes(streams, nstreams, volume, spec);
 542
 543             while (data < end) {
 544                 int32_t sum = 0;
 545                 unsigned i;
 546
 547                 for (i = 0; i < nstreams; i++) {
 548                     pa_mix_info *m = streams + i;
 549                     int32_t v, hi, lo, cv = m->linear[channel].i;
 550
 551                     if (PA_UNLIKELY(cv <= 0))
 552                         continue;
 553
 554                     hi = cv >> 16;
 555                     lo = cv & 0xFFFF;
 556
 557                     v = (int32_t) st_ulaw2linear16(*((uint8_t*) m->ptr));
 558                     v = ((v * lo) >> 16) + (v * hi);
 559                     sum += v;
 560
 561                     m->ptr = (uint8_t*) m->ptr + 1;
 562                 }
 563
 564                 sum = PA_CLAMP_UNLIKELY(sum, -0x8000, 0x7FFF);
 565                 *((uint8_t*) data) = (uint8_t) st_14linear2ulaw((int16_t) sum >> 2);
 566
 567                 data = (uint8_t*) data + 1;
 568
 569                 if (PA_UNLIKELY(++channel >= spec->channels))
 570                     channel = 0;
 571             }
 572
 573             break;
 574         }
 575
 576         case PA_SAMPLE_ALAW: {
 577             unsigned channel = 0;
 578
 579             calc_linear_integer_stream_volumes(streams, nstreams, volume, spec);
 580
 581             while (data < end) {
 582                 int32_t sum = 0;
 583                 unsigned i;
 584
 585                 for (i = 0; i < nstreams; i++) {
 586                     pa_mix_info *m = streams + i;
 587                     int32_t v, hi, lo, cv = m->linear[channel].i;
 588
 589                     if (PA_UNLIKELY(cv <= 0))
 590                         continue;
 591
 592                     hi = cv >> 16;
 593                     lo = cv & 0xFFFF;
 594
 595                     v = (int32_t) st_alaw2linear16(*((uint8_t*) m->ptr));
 596                     v = ((v * lo) >> 16) + (v * hi);
 597                     sum += v;
 598
 599                     m->ptr = (uint8_t*) m->ptr + 1;
 600                 }
 601
 602                 sum = PA_CLAMP_UNLIKELY(sum, -0x8000, 0x7FFF);
 603                 *((uint8_t*) data) = (uint8_t) st_13linear2alaw((int16_t) sum >> 3);
 604
 605                 data = (uint8_t*) data + 1;
 606
 607                 if (PA_UNLIKELY(++channel >= spec->channels))
 608                     channel = 0;
 609             }
 610
 611             break;
 612         }
 613
 614         case PA_SAMPLE_FLOAT32NE: {
 615             unsigned channel = 0;
 616
 617             calc_linear_float_stream_volumes(streams, nstreams, volume, spec);
 618
 619             while (data < end) {
 620                 float sum = 0;
 621                 unsigned i;
 622
 623                 for (i = 0; i < nstreams; i++) {
 624                     pa_mix_info *m = streams + i;
 625                     float v, cv = m->linear[channel].f;
 626
 627                     if (PA_UNLIKELY(cv <= 0))
 628                         continue;
 629
 630                     v = *((float*) m->ptr);
 631                     v *= cv;
 632                     sum += v;
 633
 634                     m->ptr = (uint8_t*) m->ptr + sizeof(float);
 635                 }
 636
 637                 *((float*) data) = sum;
 638
 639                 data = (uint8_t*) data + sizeof(float);
 640
 641                 if (PA_UNLIKELY(++channel >= spec->channels))
 642                     channel = 0;
 643             }
 644
 645             break;
 646         }
 647
 648         case PA_SAMPLE_FLOAT32RE: {
 649             unsigned channel = 0;
 650
 651             calc_linear_float_stream_volumes(streams, nstreams, volume, spec);
 652
 653             while (data < end) {
 654                 float sum = 0;
 655                 unsigned i;
 656
 657                 for (i = 0; i < nstreams; i++) {
 658                     pa_mix_info *m = streams + i;
 659                     float v, cv = m->linear[channel].f;
 660
 661                     if (PA_UNLIKELY(cv <= 0))
 662                         continue;
 663
 664                     v = PA_FLOAT32_SWAP(*(float*) m->ptr);
 665                     v *= cv;
 666                     sum += v;
 667
 668                     m->ptr = (uint8_t*) m->ptr + sizeof(float);
 669                 }
 670
 671                 *((float*) data) = PA_FLOAT32_SWAP(sum);
 672
 673                 data = (uint8_t*) data + sizeof(float);
 674
 675                 if (PA_UNLIKELY(++channel >= spec->channels))
 676                     channel = 0;
 677             }
 678
 679             break;
 680         }
 681
 682         default:
 683             pa_log_error("Unable to mix audio data of format %s.", pa_sample_format_to_string(spec->format));
 684             pa_assert_not_reached();
 685     }
 686
 687     for (k = 0; k < nstreams; k++)
 688         pa_memblock_release(streams[k].chunk.memblock);
 689
 690     return length;
 691 }
 692
 693
 694 void pa_volume_memchunk(
 695         pa_memchunk*c,
 696         const pa_sample_spec *spec,
 697         const pa_cvolume *volume) {
 698
 699     void *ptr;
 700
 701     pa_assert(c);
 702     pa_assert(spec);
 703     pa_assert(c->length % pa_frame_size(spec) == 0);
 704     pa_assert(volume);
 705
 706     if (pa_memblock_is_silence(c->memblock))
 707         return;
 708
 709     if (pa_cvolume_channels_equal_to(volume, PA_VOLUME_NORM))
 710         return;
 711
 712     if (pa_cvolume_channels_equal_to(volume, PA_VOLUME_MUTED)) {
 713         pa_silence_memchunk(c, spec);
 714         return;
 715     }
 716
 717     ptr = (uint8_t*) pa_memblock_acquire(c->memblock) + c->index;
 718
 719     switch (spec->format) {
 720
 721         case PA_SAMPLE_S16NE: {
 722             int16_t *d, *e;
 723             unsigned channel;
 724             int32_t linear[PA_CHANNELS_MAX];
 725
 726             calc_linear_integer_volume(linear, volume);
 727
 728             e = (int16_t*) ptr + c->length/sizeof(int16_t);
 729
 730             for (channel = 0, d = ptr; d < e; d++) {
 731                 int32_t t, hi, lo;
 732
 733                 /* Multiplying the 32bit volume factor with the 16bit
 734                  * sample might result in an 48bit value. We want to
 735                  * do without 64 bit integers and hence do the
 736                  * multiplication independantly for the HI and LO part
 737                  * of the volume. */
 738
 739                 hi = linear[channel] >> 16;
 740                 lo = linear[channel] & 0xFFFF;
 741
 742                 t = (int32_t)(*d);
 743                 t = ((t * lo) >> 16) + (t * hi);
 744                 t = PA_CLAMP_UNLIKELY(t, -0x8000, 0x7FFF);
 745                 *d = (int16_t) t;
 746
 747                 if (PA_UNLIKELY(++channel >= spec->channels))
 748                     channel = 0;
 749             }
 750
 751             break;
 752         }
 753
 754         case PA_SAMPLE_S16RE: {
 755             int16_t *d, *e;
 756             unsigned channel;
 757             int32_t linear[PA_CHANNELS_MAX];
 758
 759             calc_linear_integer_volume(linear, volume);
 760
 761             e = (int16_t*) ptr + c->length/sizeof(int16_t);
 762
 763             for (channel = 0, d = ptr; d < e; d++) {
 764                 int32_t t, hi, lo;
 765
 766                 hi = linear[channel] >> 16;
 767                 lo = linear[channel] & 0xFFFF;
 768
 769                 t = (int32_t) PA_INT16_SWAP(*d);
 770                 t = ((t * lo) >> 16) + (t * hi);
 771                 t = PA_CLAMP_UNLIKELY(t, -0x8000, 0x7FFF);
 772                 *d = PA_INT16_SWAP((int16_t) t);
 773
 774                 if (PA_UNLIKELY(++channel >= spec->channels))
 775                     channel = 0;
 776             }
 777
 778             break;
 779         }
 780
 781         case PA_SAMPLE_S32NE: {
 782             int32_t *d, *e;
 783             unsigned channel;
 784             int32_t linear[PA_CHANNELS_MAX];
 785
 786             calc_linear_integer_volume(linear, volume);
 787
 788             e = (int32_t*) ptr + c->length/sizeof(int32_t);
 789
 790             for (channel = 0, d = ptr; d < e; d++) {
 791                 int64_t t;
 792
 793                 t = (int64_t)(*d);
 794                 t = (t * linear[channel]) >> 16;
 795                 t = PA_CLAMP_UNLIKELY(t, -0x80000000LL, 0x7FFFFFFFLL);
 796                 *d = (int32_t) t;
 797
 798                 if (PA_UNLIKELY(++channel >= spec->channels))
 799                     channel = 0;
 800             }
 801             break;
 802         }
 803
 804         case PA_SAMPLE_S32RE: {
 805             int32_t *d, *e;
 806             unsigned channel;
 807             int32_t linear[PA_CHANNELS_MAX];
 808
 809             calc_linear_integer_volume(linear, volume);
 810
 811             e = (int32_t*) ptr + c->length/sizeof(int32_t);
 812
 813             for (channel = 0, d = ptr; d < e; d++) {
 814                 int64_t t;
 815
 816                 t = (int64_t) PA_INT32_SWAP(*d);
 817                 t = (t * linear[channel]) >> 16;
 818                 t = PA_CLAMP_UNLIKELY(t, -0x80000000LL, 0x7FFFFFFFLL);
 819                 *d = PA_INT32_SWAP((int32_t) t);
 820
 821                 if (PA_UNLIKELY(++channel >= spec->channels))
 822                     channel = 0;
 823             }
 824             break;
 825         }
 826
 827         case PA_SAMPLE_S24NE: {
 828             uint8_t *d, *e;
 829             unsigned channel;
 830             int32_t linear[PA_CHANNELS_MAX];
 831
 832             calc_linear_integer_volume(linear, volume);
 833
 834             e = (uint8_t*) ptr + c->length;
 835
 836             for (channel = 0, d = ptr; d < e; d += 3) {
 837                 int64_t t;
 838
 839                 t = (int64_t)((int32_t) (PA_READ24NE(d) << 8));
 840                 t = (t * linear[channel]) >> 16;
 841                 t = PA_CLAMP_UNLIKELY(t, -0x80000000LL, 0x7FFFFFFFLL);
 842                 PA_WRITE24NE(d, ((uint32_t) (int32_t) t) >> 8);
 843
 844                 if (PA_UNLIKELY(++channel >= spec->channels))
 845                     channel = 0;
 846             }
 847             break;
 848         }
 849
 850         case PA_SAMPLE_S24RE: {
 851             uint8_t *d, *e;
 852             unsigned channel;
 853             int32_t linear[PA_CHANNELS_MAX];
 854
 855             calc_linear_integer_volume(linear, volume);
 856
 857             e = (uint8_t*) ptr + c->length;
 858
 859             for (channel = 0, d = ptr; d < e; d += 3) {
 860                 int64_t t;
 861
 862                 t = (int64_t)((int32_t) (PA_READ24RE(d) << 8));
 863                 t = (t * linear[channel]) >> 16;
 864                 t = PA_CLAMP_UNLIKELY(t, -0x80000000LL, 0x7FFFFFFFLL);
 865                 PA_WRITE24RE(d, ((uint32_t) (int32_t) t) >> 8);
 866
 867                 if (PA_UNLIKELY(++channel >= spec->channels))
 868                     channel = 0;
 869             }
 870             break;
 871         }
 872
 873         case PA_SAMPLE_S24_32NE: {
 874             uint32_t *d, *e;
 875             unsigned channel;
 876             int32_t linear[PA_CHANNELS_MAX];
 877
 878             calc_linear_integer_volume(linear, volume);
 879
 880             e = (uint32_t*) ptr + c->length/sizeof(uint32_t);
 881
 882             for (channel = 0, d = ptr; d < e; d++) {
 883                 int64_t t;
 884
 885                 t = (int64_t) ((int32_t) (*d << 8));
 886                 t = (t * linear[channel]) >> 16;
 887                 t = PA_CLAMP_UNLIKELY(t, -0x80000000LL, 0x7FFFFFFFLL);
 888                 *d = ((uint32_t) ((int32_t) t)) >> 8;
 889
 890                 if (PA_UNLIKELY(++channel >= spec->channels))
 891                     channel = 0;
 892             }
 893             break;
 894         }
 895
 896         case PA_SAMPLE_S24_32RE: {
 897             uint32_t *d, *e;
 898             unsigned channel;
 899             int32_t linear[PA_CHANNELS_MAX];
 900
 901             calc_linear_integer_volume(linear, volume);
 902
 903             e = (uint32_t*) ptr + c->length/sizeof(uint32_t);
 904
 905             for (channel = 0, d = ptr; d < e; d++) {
 906                 int64_t t;
 907
 908                 t = (int64_t) ((int32_t) (PA_UINT32_SWAP(*d) << 8));
 909                 t = (t * linear[channel]) >> 16;
 910                 t = PA_CLAMP_UNLIKELY(t, -0x80000000LL, 0x7FFFFFFFLL);
 911                 *d = PA_UINT32_SWAP(((uint32_t) ((int32_t) t)) >> 8);
 912
 913                 if (PA_UNLIKELY(++channel >= spec->channels))
 914                     channel = 0;
 915             }
 916             break;
 917         }
 918
 919         case PA_SAMPLE_U8: {
 920             uint8_t *d, *e;
 921             unsigned channel;
 922             int32_t linear[PA_CHANNELS_MAX];
 923
 924             calc_linear_integer_volume(linear, volume);
 925
 926             e = (uint8_t*) ptr + c->length;
 927
 928             for (channel = 0, d = ptr; d < e; d++) {
 929                 int32_t t, hi, lo;
 930
 931                 hi = linear[channel] >> 16;
 932                 lo = linear[channel] & 0xFFFF;
 933
 934                 t = (int32_t) *d - 0x80;
 935                 t = ((t * lo) >> 16) + (t * hi);
 936                 t = PA_CLAMP_UNLIKELY(t, -0x80, 0x7F);
 937                 *d = (uint8_t) (t + 0x80);
 938
 939                 if (PA_UNLIKELY(++channel >= spec->channels))
 940                     channel = 0;
 941             }
 942             break;
 943         }
 944
 945         case PA_SAMPLE_ULAW: {
 946             uint8_t *d, *e;
 947             unsigned channel;
 948             int32_t linear[PA_CHANNELS_MAX];
 949
 950             calc_linear_integer_volume(linear, volume);
 951
 952             e = (uint8_t*) ptr + c->length;
 953
 954             for (channel = 0, d = ptr; d < e; d++) {
 955                 int32_t t, hi, lo;
 956
 957                 hi = linear[channel] >> 16;
 958                 lo = linear[channel] & 0xFFFF;
 959
 960                 t = (int32_t) st_ulaw2linear16(*d);
 961                 t = ((t * lo) >> 16) + (t * hi);
 962                 t = PA_CLAMP_UNLIKELY(t, -0x8000, 0x7FFF);
 963                 *d = (uint8_t) st_14linear2ulaw((int16_t) t >> 2);
 964
 965                 if (PA_UNLIKELY(++channel >= spec->channels))
 966                     channel = 0;
 967             }
 968             break;
 969         }
 970
 971         case PA_SAMPLE_ALAW: {
 972             uint8_t *d, *e;
 973             unsigned channel;
 974             int32_t linear[PA_CHANNELS_MAX];
 975
 976             calc_linear_integer_volume(linear, volume);
 977
 978             e = (uint8_t*) ptr + c->length;
 979
 980             for (channel = 0, d = ptr; d < e; d++) {
 981                 int32_t t, hi, lo;
 982
 983                 hi = linear[channel] >> 16;
 984                 lo = linear[channel] & 0xFFFF;
 985
 986                 t = (int32_t) st_alaw2linear16(*d);
 987                 t = ((t * lo) >> 16) + (t * hi);
 988                 t = PA_CLAMP_UNLIKELY(t, -0x8000, 0x7FFF);
 989                 *d = (uint8_t) st_13linear2alaw((int16_t) t >> 3);
 990
 991                 if (PA_UNLIKELY(++channel >= spec->channels))
 992                     channel = 0;
 993             }
 994             break;
 995         }
 996
 997         case PA_SAMPLE_FLOAT32NE: {
 998             float *d;
 999             int skip;
1000             unsigned n;
1001             unsigned channel;
1002
1003             d = ptr;
1004             skip = (int) (spec->channels * sizeof(float));
1005             n = (unsigned) (c->length/sizeof(float)/spec->channels);
1006
1007             for (channel = 0; channel < spec->channels; channel ++) {
1008                 float v, *t;
1009
1010                 if (PA_UNLIKELY(volume->values[channel] == PA_VOLUME_NORM))
1011                     continue;
1012
1013                 v = (float) pa_sw_volume_to_linear(volume->values[channel]);
1014                 t = d + channel;
1015                 oil_scalarmult_f32(t, skip, t, skip, &v, (int) n);
1016             }
1017             break;
1018         }
1019
1020         case PA_SAMPLE_FLOAT32RE: {
1021             float *d, *e;
1022             unsigned channel;
1023             float linear[PA_CHANNELS_MAX];
1024
1025             calc_linear_float_volume(linear, volume);
1026
1027             e = (float*) ptr + c->length/sizeof(float);
1028
1029             for (channel = 0, d = ptr; d < e; d++) {
1030                 float t;
1031
1032                 t = PA_FLOAT32_SWAP(*d);
1033                 t *= linear[channel];
1034                 *d = PA_FLOAT32_SWAP(t);
1035
1036                 if (PA_UNLIKELY(++channel >= spec->channels))
1037                     channel = 0;
1038             }
1039
1040             break;
1041         }
1042
1043
1044         default:
1045             pa_log_warn(" Unable to change volume of format %s.", pa_sample_format_to_string(spec->format));
1046             /* If we cannot change the volume, we just don't do it */
1047     }
1048
1049     pa_memblock_release(c->memblock);
1050 }
1051
1052 size_t pa_frame_align(size_t l, const pa_sample_spec *ss) {
1053     size_t fs;
1054
1055     pa_assert(ss);
1056
1057     fs = pa_frame_size(ss);
1058
1059     return (l/fs) * fs;
1060 }
1061
1062 pa_bool_t pa_frame_aligned(size_t l, const pa_sample_spec *ss) {
1063     size_t fs;
1064
1065     pa_assert(ss);
1066
1067     fs = pa_frame_size(ss);
1068
1069     return l % fs == 0;
1070 }
1071
1072 void pa_interleave(const void *src[], unsigned channels, void *dst, size_t ss, unsigned n) {
1073     unsigned c;
1074     size_t fs;
1075
1076     pa_assert(src);
1077     pa_assert(channels > 0);
1078     pa_assert(dst);
1079     pa_assert(ss > 0);
1080     pa_assert(n > 0);
1081
1082     fs = ss * channels;
1083
1084     for (c = 0; c < channels; c++) {
1085         unsigned j;
1086         void *d;
1087         const void *s;
1088
1089         s = src[c];
1090         d = (uint8_t*) dst + c * ss;
1091
1092         for (j = 0; j < n; j ++) {
1093             oil_memcpy(d, s, (int) ss);
1094             s = (uint8_t*) s + ss;
1095             d = (uint8_t*) d + fs;
1096         }
1097     }
1098 }
1099
1100 void pa_deinterleave(const void *src, void *dst[], unsigned channels, size_t ss, unsigned n) {
1101     size_t fs;
1102     unsigned c;
1103
1104     pa_assert(src);
1105     pa_assert(dst);
1106     pa_assert(channels > 0);
1107     pa_assert(ss > 0);
1108     pa_assert(n > 0);
1109
1110     fs = ss * channels;
1111
1112     for (c = 0; c < channels; c++) {
1113         unsigned j;
1114         const void *s;
1115         void *d;
1116
1117         s = (uint8_t*) src + c * ss;
1118         d = dst[c];
1119
1120         for (j = 0; j < n; j ++) {
1121             oil_memcpy(d, s, (int) ss);
1122             s = (uint8_t*) s + fs;
1123             d = (uint8_t*) d + ss;
1124         }
1125     }
1126 }
1127
1128 static pa_memblock *silence_memblock_new(pa_mempool *pool, uint8_t c) {
1129     pa_memblock *b;
1130     size_t length;
1131     void *data;
1132
1133     pa_assert(pool);
1134
1135     length = PA_MIN(pa_mempool_block_size_max(pool), PA_SILENCE_MAX);
1136
1137     b = pa_memblock_new(pool, length);
1138
1139     data = pa_memblock_acquire(b);
1140     memset(data, c, length);
1141     pa_memblock_release(b);
1142
1143     pa_memblock_set_is_silence(b, TRUE);
1144
1145     return b;
1146 }
1147
1148 void pa_silence_cache_init(pa_silence_cache *cache) {
1149     pa_assert(cache);
1150
1151     memset(cache, 0, sizeof(pa_silence_cache));
1152 }
1153
1154 void pa_silence_cache_done(pa_silence_cache *cache) {
1155     pa_sample_format_t f;
1156     pa_assert(cache);
1157
1158     for (f = 0; f < PA_SAMPLE_MAX; f++)
1159         if (cache->blocks[f])
1160             pa_memblock_unref(cache->blocks[f]);
1161
1162     memset(cache, 0, sizeof(pa_silence_cache));
1163 }
1164
1165 pa_memchunk* pa_silence_memchunk_get(pa_silence_cache *cache, pa_mempool *pool, pa_memchunk* ret, const pa_sample_spec *spec, size_t length) {
1166     pa_memblock *b;
1167     size_t l;
1168
1169     pa_assert(cache);
1170     pa_assert(pa_sample_spec_valid(spec));
1171
1172     if (!(b = cache->blocks[spec->format]))
1173
1174         switch (spec->format) {
1175             case PA_SAMPLE_U8:
1176                 cache->blocks[PA_SAMPLE_U8] = b = silence_memblock_new(pool, 0x80);
1177                 break;
1178             case PA_SAMPLE_S16LE:
1179             case PA_SAMPLE_S16BE:
1180             case PA_SAMPLE_S32LE:
1181             case PA_SAMPLE_S32BE:
1182             case PA_SAMPLE_S24LE:
1183             case PA_SAMPLE_S24BE:
1184             case PA_SAMPLE_S24_32LE:
1185             case PA_SAMPLE_S24_32BE:
1186             case PA_SAMPLE_FLOAT32LE:
1187             case PA_SAMPLE_FLOAT32BE:
1188                 cache->blocks[PA_SAMPLE_S16LE] = b = silence_memblock_new(pool, 0);
1189                 cache->blocks[PA_SAMPLE_S16BE] = pa_memblock_ref(b);
1190                 cache->blocks[PA_SAMPLE_S32LE] = pa_memblock_ref(b);
1191                 cache->blocks[PA_SAMPLE_S32BE] = pa_memblock_ref(b);
1192                 cache->blocks[PA_SAMPLE_S24LE] = pa_memblock_ref(b);
1193                 cache->blocks[PA_SAMPLE_S24BE] = pa_memblock_ref(b);
1194                 cache->blocks[PA_SAMPLE_S24_32LE] = pa_memblock_ref(b);
1195                 cache->blocks[PA_SAMPLE_S24_32BE] = pa_memblock_ref(b);
1196                 cache->blocks[PA_SAMPLE_FLOAT32LE] = pa_memblock_ref(b);
1197                 cache->blocks[PA_SAMPLE_FLOAT32BE] = pa_memblock_ref(b);
1198                 break;
1199             case PA_SAMPLE_ALAW:
1200                 cache->blocks[PA_SAMPLE_ALAW] = b = silence_memblock_new(pool, 0xd5);
1201                 break;
1202             case PA_SAMPLE_ULAW:
1203                 cache->blocks[PA_SAMPLE_ULAW] = b = silence_memblock_new(pool, 0xff);
1204                 break;
1205             default:
1206                 pa_assert_not_reached();
1207     }
1208
1209     pa_assert(b);
1210
1211     ret->memblock = pa_memblock_ref(b);
1212
1213     l = pa_memblock_get_length(b);
1214     if (length > l || length == 0)
1215         length = l;
1216
1217     ret->length = pa_frame_align(length, spec);
1218     ret->index = 0;
1219
1220     return ret;
1221 }
1222
1223 void pa_sample_clamp(pa_sample_format_t format, void *dst, size_t dstr, const void *src, size_t sstr, unsigned n) {
1224     const float *s;
1225     float *d;
1226
1227     s = src; d = dst;
1228
1229     if (format == PA_SAMPLE_FLOAT32NE) {
1230
1231         float minus_one = -1.0, plus_one = 1.0;
1232         oil_clip_f32(d, (int) dstr, s, (int) sstr, (int) n, &minus_one, &plus_one);
1233
1234     } else {
1235         pa_assert(format == PA_SAMPLE_FLOAT32RE);
1236
1237         for (; n > 0; n--) {
1238             float f;
1239
1240             f = PA_FLOAT32_SWAP(*s);
1241             f = PA_CLAMP_UNLIKELY(f, -1.0f, 1.0f);
1242             *d = PA_FLOAT32_SWAP(f);
1243
1244             s = (const float*) ((const uint8_t*) s + sstr);
1245             d = (float*) ((uint8_t*) d + dstr);
1246         }
1247     }
1248 }
1249
1250 /* Similar to pa_bytes_to_usec() but rounds up, not down */
1251
1252 pa_usec_t pa_bytes_to_usec_round_up(uint64_t length, const pa_sample_spec *spec) {
1253     size_t fs;
1254     pa_usec_t usec;
1255
1256     pa_assert(spec);
1257
1258     fs = pa_frame_size(spec);
1259     length = (length + fs - 1) / fs;
1260
1261     usec = (pa_usec_t) length * PA_USEC_PER_SEC;
1262
1263     return (usec + spec->rate - 1) / spec->rate;
1264 }
1265
1266 /* Similar to pa_usec_to_bytes() but rounds up, not down */
1267
1268 size_t pa_usec_to_bytes_round_up(pa_usec_t t, const pa_sample_spec *spec) {
1269     uint64_t u;
1270     pa_assert(spec);
1271
1272     u = (uint64_t) t * (uint64_t) spec->rate;
1273
1274     u = (u + PA_USEC_PER_SEC - 1) / PA_USEC_PER_SEC;
1275
1276     u *= pa_frame_size(spec);
1277
1278     return (size_t) u;
1279 }
1280
1281 void pa_memchunk_dump_to_file(pa_memchunk *c, const char *fn) {
1282     FILE *f;
1283     void *p;
1284
1285     pa_assert(c);
1286     pa_assert(fn);
1287
1288     /* Only for debugging purposes */
1289
1290     f = fopen(fn, "a");
1291
1292     if (!f) {
1293         pa_log_warn("Failed to open '%s': %s", fn, pa_cstrerror(errno));
1294         return;
1295     }
1296
1297     p = pa_memblock_acquire(c->memblock);
1298
1299     if (fwrite((uint8_t*) p + c->index, 1, c->length, f) != c->length)
1300         pa_log_warn("Failed to write to '%s': %s", fn, pa_cstrerror(errno));
1301
1302     pa_memblock_release(c->memblock);
1303
1304     fclose(f);
1305 }
1306
1307 static void calc_sine(float *f, size_t l, double freq) {
1308     size_t i;
1309
1310     l /= sizeof(float);
1311
1312     for (i = 0; i < l; i++)
1313         *(f++) = (float) 0.5f * sin((double) i*M_PI*2*freq / (double) l);
1314 }
1315
1316 void pa_memchunk_sine(pa_memchunk *c, pa_mempool *pool, unsigned rate, unsigned freq) {
1317     size_t l;
1318     unsigned gcd, n;
1319     void *p;
1320
1321     pa_memchunk_reset(c);
1322
1323     gcd = pa_gcd(rate, freq);
1324     n = rate / gcd;
1325
1326     l = pa_mempool_block_size_max(pool) / sizeof(float);
1327
1328     l /= n;
1329     if (l <= 0) l = 1;
1330     l *= n;
1331
1332     c->length = l * sizeof(float);
1333     c->memblock = pa_memblock_new(pool, c->length);
1334
1335     p = pa_memblock_acquire(c->memblock);
1336     calc_sine(p, c->length, freq * l / rate);
1337     pa_memblock_release(c->memblock);
1338 }