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