Merge commit 'origin/master-tx'
[profile/ivi/pulseaudio-panda.git] / src / pulsecore / envelope.c
1 /***
2   This file is part of PulseAudio.
3
4   Copyright 2007 Lennart Poettering
5
6   PulseAudio is free software; you can redistribute it and/or modify
7   it under the terms of the GNU Lesser General Public License as
8   published by the Free Software Foundation; either version 2.1 of the
9   License, or (at your option) any later version.
10
11   PulseAudio is distributed in the hope that it will be useful, but
12   WITHOUT ANY WARRANTY; without even the implied warranty of
13   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14   Lesser General Public License for more details.
15
16   You should have received a copy of the GNU Lesser General Public
17   License along with PulseAudio; if not, write to the Free Software
18   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
19   USA.
20 ***/
21
22 #ifdef HAVE_CONFIG_H
23 #include <config.h>
24 #endif
25
26 #include <stdio.h>
27
28 #include <pulse/sample.h>
29 #include <pulse/xmalloc.h>
30
31 #include <pulsecore/endianmacros.h>
32 #include <pulsecore/memchunk.h>
33 #include <pulsecore/macro.h>
34 #include <pulsecore/flist.h>
35 #include <pulsecore/semaphore.h>
36 #include <pulsecore/g711.h>
37
38 #include "envelope.h"
39
40 /*
41     Envelope subsystem for applying linear interpolated volume
42     envelopes on audio data. If multiple enevelopes shall be applied
43     at the same time, the "minimum" envelope is determined and
44     applied.
45
46     Envelopes are defined in a statically allocated constant structure
47     pa_envelope_def. It may be activated using pa_envelope_add(). And
48     already active envelope may be replaced with pa_envelope_replace()
49     and removed with pa_envelope_remove().The combined "minimum"
50     envelope can be applied to audio data with pa_envelope_apply().
51
52     _apply() on one hand and _add()/_replace()/_remove() on the other
53     can be executed in seperate threads, in which case no locking is
54     used.
55 */
56
57 PA_STATIC_FLIST_DECLARE(items, 0, pa_xfree);
58
59 struct pa_envelope_item {
60     PA_LLIST_FIELDS(pa_envelope_item);
61     const pa_envelope_def *def;
62     pa_usec_t start_x;
63     union {
64         int32_t i;
65         float f;
66     } start_y;
67     unsigned j;
68 };
69
70 enum envelope_state {
71     STATE_VALID0,
72     STATE_VALID1,
73     STATE_READ0,
74     STATE_READ1,
75     STATE_WAIT0,
76     STATE_WAIT1,
77     STATE_WRITE0,
78     STATE_WRITE1
79 };
80
81 struct pa_envelope {
82     pa_sample_spec sample_spec;
83
84     PA_LLIST_HEAD(pa_envelope_item, items);
85
86     pa_atomic_t state;
87
88     size_t x;
89
90     struct {
91         unsigned n_points, n_allocated, n_current;
92
93         size_t *x;
94         union {
95             int32_t *i;
96             float *f;
97         } y;
98
99         size_t cached_dx;
100         int32_t cached_dy_i;
101         float cached_dy_dx;
102         pa_bool_t cached_valid;
103     } points[2];
104
105     pa_bool_t is_float;
106
107     pa_semaphore *semaphore;
108 };
109
110 pa_envelope *pa_envelope_new(const pa_sample_spec *ss) {
111     pa_envelope *e;
112     pa_assert(ss);
113
114     e = pa_xnew(pa_envelope, 1);
115
116     e->sample_spec = *ss;
117     PA_LLIST_HEAD_INIT(pa_envelope_item, e->items);
118
119     e->x = 0;
120
121     e->points[0].n_points = e->points[1].n_points = 0;
122     e->points[0].n_allocated = e->points[1].n_allocated = 0;
123     e->points[0].n_current = e->points[1].n_current = 0;
124     e->points[0].x = e->points[1].x = NULL;
125     e->points[0].y.i = e->points[1].y.i = NULL;
126     e->points[0].cached_valid = e->points[1].cached_valid = FALSE;
127
128     pa_atomic_store(&e->state, STATE_VALID0);
129
130     e->is_float =
131         ss->format == PA_SAMPLE_FLOAT32LE ||
132         ss->format == PA_SAMPLE_FLOAT32BE;
133
134     e->semaphore = pa_semaphore_new(0);
135
136     return e;
137 }
138
139 void pa_envelope_free(pa_envelope *e) {
140     pa_assert(e);
141
142     while (e->items)
143         pa_envelope_remove(e, e->items);
144
145     pa_xfree(e->points[0].x);
146     pa_xfree(e->points[1].x);
147     pa_xfree(e->points[0].y.i);
148     pa_xfree(e->points[1].y.i);
149
150     pa_semaphore_free(e->semaphore);
151
152     pa_xfree(e);
153 }
154
155 static int32_t linear_interpolate_int(pa_usec_t x1, int32_t _y1, pa_usec_t x2, int32_t y2, pa_usec_t x3) {
156     return (int32_t) ((double) _y1 + (double) (x3 - x1) * (double) (y2 - _y1) / (double) (x2 - x1));
157 }
158
159 static float linear_interpolate_float(pa_usec_t x1, float _y1, pa_usec_t x2, float y2, pa_usec_t x3) {
160     return _y1 + ((float) x3 - (float) x1) * (y2 - _y1) / ((float) x2 - (float) x1);
161 }
162
163 static int32_t item_get_int(pa_envelope_item *i, pa_usec_t x) {
164     pa_assert(i);
165
166     if (x <= i->start_x)
167         return i->start_y.i;
168
169     x -= i->start_x;
170
171     if (x <= i->def->points_x[0])
172         return linear_interpolate_int(0, i->start_y.i,
173                                       i->def->points_x[0], i->def->points_y.i[0], x);
174
175     if (x >= i->def->points_x[i->def->n_points-1])
176         return i->def->points_y.i[i->def->n_points-1];
177
178     pa_assert(i->j > 0);
179     pa_assert(i->def->points_x[i->j-1] <= x);
180     pa_assert(x < i->def->points_x[i->j]);
181
182     return linear_interpolate_int(i->def->points_x[i->j-1], i->def->points_y.i[i->j-1],
183                                   i->def->points_x[i->j], i->def->points_y.i[i->j], x);
184 }
185
186 static float item_get_float(pa_envelope_item *i, pa_usec_t x) {
187     pa_assert(i);
188
189     if (x <= i->start_x)
190         return i->start_y.f;
191
192     x -= i->start_x;
193
194     if (x <= i->def->points_x[0])
195         return linear_interpolate_float(0, i->start_y.f,
196                                         i->def->points_x[0], i->def->points_y.f[0], x);
197
198     if (x >= i->def->points_x[i->def->n_points-1])
199         return i->def->points_y.f[i->def->n_points-1];
200
201     pa_assert(i->j > 0);
202     pa_assert(i->def->points_x[i->j-1] <= x);
203     pa_assert(x < i->def->points_x[i->j]);
204
205     return linear_interpolate_float(i->def->points_x[i->j-1], i->def->points_y.f[i->j-1],
206                                     i->def->points_x[i->j], i->def->points_y.f[i->j], x);
207 }
208
209 static void envelope_begin_write(pa_envelope *e, int *v) {
210     enum envelope_state new_state, old_state;
211     pa_bool_t wait_sem;
212
213     pa_assert(e);
214     pa_assert(v);
215
216     for (;;) {
217         do {
218             wait_sem = FALSE;
219             old_state = pa_atomic_load(&e->state);
220
221             switch (old_state) {
222                 case STATE_VALID0:
223                     *v = 1;
224                     new_state = STATE_WRITE0;
225                     break;
226                 case STATE_VALID1:
227                     *v = 0;
228                     new_state = STATE_WRITE1;
229                     break;
230                 case STATE_READ0:
231                     new_state = STATE_WAIT0;
232                     wait_sem = TRUE;
233                     break;
234                 case STATE_READ1:
235                     new_state = STATE_WAIT1;
236                     wait_sem = TRUE;
237                     break;
238                 default:
239                     pa_assert_not_reached();
240             }
241         } while (!pa_atomic_cmpxchg(&e->state, old_state, new_state));
242
243         if (!wait_sem)
244             break;
245
246         pa_semaphore_wait(e->semaphore);
247     }
248 }
249
250 static pa_bool_t envelope_commit_write(pa_envelope *e, int v) {
251     enum envelope_state new_state, old_state;
252
253     pa_assert(e);
254
255     do {
256         old_state = pa_atomic_load(&e->state);
257
258         switch (old_state) {
259             case STATE_WRITE0:
260                 pa_assert(v == 1);
261                 new_state = STATE_VALID1;
262                 break;
263             case STATE_WRITE1:
264                 pa_assert(v == 0);
265                 new_state = STATE_VALID0;
266                 break;
267             case STATE_VALID0:
268             case STATE_VALID1:
269             case STATE_READ0:
270             case STATE_READ1:
271                 return FALSE;
272             default:
273                 pa_assert_not_reached();
274         }
275     } while (!pa_atomic_cmpxchg(&e->state, old_state, new_state));
276
277     return TRUE;
278 }
279
280 static void envelope_begin_read(pa_envelope *e, int *v) {
281     enum envelope_state new_state, old_state;
282     pa_assert(e);
283     pa_assert(v);
284
285     do {
286         old_state = pa_atomic_load(&e->state);
287
288         switch (old_state) {
289             case STATE_VALID0:
290             case STATE_WRITE0:
291                 *v = 0;
292                 new_state = STATE_READ0;
293                 break;
294             case STATE_VALID1:
295             case STATE_WRITE1:
296                 *v = 1;
297                 new_state = STATE_READ1;
298                 break;
299             default:
300                 pa_assert_not_reached();
301         }
302     } while (!pa_atomic_cmpxchg(&e->state, old_state, new_state));
303 }
304
305 static void envelope_commit_read(pa_envelope *e, int v) {
306     enum envelope_state new_state, old_state;
307     pa_bool_t post_sem;
308
309     pa_assert(e);
310
311     do {
312         post_sem = FALSE;
313         old_state = pa_atomic_load(&e->state);
314
315         switch (old_state) {
316             case STATE_READ0:
317                 pa_assert(v == 0);
318                 new_state = STATE_VALID0;
319                 break;
320             case STATE_READ1:
321                 pa_assert(v == 1);
322                 new_state = STATE_VALID1;
323                 break;
324             case STATE_WAIT0:
325                 pa_assert(v == 0);
326                 new_state = STATE_VALID0;
327                 post_sem = TRUE;
328                 break;
329             case STATE_WAIT1:
330                 pa_assert(v == 1);
331                 new_state = STATE_VALID1;
332                 post_sem = TRUE;
333                 break;
334             default:
335                 pa_assert_not_reached();
336         }
337     } while (!pa_atomic_cmpxchg(&e->state, old_state, new_state));
338
339     if (post_sem)
340         pa_semaphore_post(e->semaphore);
341 }
342
343 static void envelope_merge(pa_envelope *e, int v) {
344
345     e->points[v].n_points = 0;
346
347     if (e->items) {
348         pa_envelope_item *i;
349         pa_usec_t x = (pa_usec_t) -1;
350
351         for (i = e->items; i; i = i->next)
352             i->j = 0;
353
354         for (;;) {
355             pa_bool_t min_is_set;
356             pa_envelope_item *s = NULL;
357
358             /* Let's find the next spot on the X axis to analyze */
359             for (i = e->items; i; i = i->next) {
360
361                 for (;;) {
362
363                     if (i->j >= i->def->n_points)
364                         break;
365
366                     if ((x != (pa_usec_t) -1) && i->start_x + i->def->points_x[i->j] <= x) {
367                         i->j++;
368                         continue;
369                     }
370
371                     if (!s || (i->start_x + i->def->points_x[i->j] < s->start_x + s->def->points_x[s->j]))
372                         s = i;
373
374                     break;
375                 }
376             }
377
378             if (!s)
379                 break;
380
381             if (e->points[v].n_points >= e->points[v].n_allocated) {
382                 e->points[v].n_allocated = PA_MAX(e->points[v].n_points*2, PA_ENVELOPE_POINTS_MAX);
383
384                 e->points[v].x = pa_xrealloc(e->points[v].x, sizeof(size_t) * e->points[v].n_allocated);
385                 e->points[v].y.i = pa_xrealloc(e->points[v].y.i, sizeof(int32_t) * e->points[v].n_allocated);
386             }
387
388             x = s->start_x + s->def->points_x[s->j];
389             e->points[v].x[e->points[v].n_points] = pa_usec_to_bytes(x, &e->sample_spec);
390
391             min_is_set = FALSE;
392
393             /* Now let's find the lowest value */
394             if (e->is_float) {
395                 float min_f;
396
397                 for (i = e->items; i; i = i->next) {
398                     float f = item_get_float(i, x);
399                     if (!min_is_set || f < min_f) {
400                         min_f = f;
401                         min_is_set = TRUE;
402                     }
403                 }
404
405                 e->points[v].y.f[e->points[v].n_points] = min_f;
406             } else {
407                 int32_t min_k;
408
409                 for (i = e->items; i; i = i->next) {
410                     int32_t k = item_get_int(i, x);
411                     if (!min_is_set || k < min_k) {
412                         min_k = k;
413                         min_is_set = TRUE;
414                     }
415                 }
416
417                 e->points[v].y.i[e->points[v].n_points] = min_k;
418             }
419
420             pa_assert_se(min_is_set);
421             e->points[v].n_points++;
422         }
423     }
424
425     e->points[v].n_current = 0;
426     e->points[v].cached_valid = FALSE;
427 }
428
429 pa_envelope_item *pa_envelope_add(pa_envelope *e, const pa_envelope_def *def) {
430     pa_envelope_item *i;
431     int v;
432
433     pa_assert(e);
434     pa_assert(def);
435     pa_assert(def->n_points > 0);
436
437     if (!(i = pa_flist_pop(PA_STATIC_FLIST_GET(items))))
438         i = pa_xnew(pa_envelope_item, 1);
439
440     i->def = def;
441
442     if (e->is_float)
443         i->start_y.f = def->points_y.f[0];
444     else
445         i->start_y.i = def->points_y.i[0];
446
447     PA_LLIST_PREPEND(pa_envelope_item, e->items, i);
448
449     envelope_begin_write(e, &v);
450
451     do {
452
453         i->start_x = pa_bytes_to_usec(e->x, &e->sample_spec);
454         envelope_merge(e, v);
455
456     } while (!envelope_commit_write(e, v));
457
458     return i;
459 }
460
461 pa_envelope_item *pa_envelope_replace(pa_envelope *e, pa_envelope_item *i, const pa_envelope_def *def) {
462     pa_usec_t x;
463     int v;
464
465     pa_assert(e);
466     pa_assert(i);
467     pa_assert(def->n_points > 0);
468
469     envelope_begin_write(e, &v);
470
471     for (;;) {
472         float saved_f;
473         int32_t saved_i;
474         uint64_t saved_start_x;
475         const pa_envelope_def *saved_def;
476
477         x = pa_bytes_to_usec(e->x, &e->sample_spec);
478
479         if (e->is_float) {
480             saved_f = i->start_y.f;
481             i->start_y.f = item_get_float(i, x);
482         } else {
483             saved_i = i->start_y.i;
484             i->start_y.i = item_get_int(i, x);
485         }
486
487         saved_start_x = i->start_x;
488         saved_def = i->def;
489
490         i->start_x = x;
491         i->def = def;
492
493         envelope_merge(e, v);
494
495         if (envelope_commit_write(e, v))
496             break;
497
498         i->start_x = saved_start_x;
499         i->def = saved_def;
500
501         if (e->is_float)
502             i->start_y.f = saved_f;
503         else
504             i->start_y.i = saved_i;
505     }
506
507     return i;
508 }
509
510 void pa_envelope_remove(pa_envelope *e, pa_envelope_item *i) {
511     int v;
512
513     pa_assert(e);
514     pa_assert(i);
515
516     PA_LLIST_REMOVE(pa_envelope_item, e->items, i);
517
518     if (pa_flist_push(PA_STATIC_FLIST_GET(items), i) < 0)
519         pa_xfree(i);
520
521     envelope_begin_write(e, &v);
522     do {
523         envelope_merge(e, v);
524     } while (!envelope_commit_write(e, v));
525 }
526
527 static int32_t linear_get_int(pa_envelope *e, int v) {
528     pa_assert(e);
529
530     /* The repeated division could be replaced by Bresenham, as an
531      * optimization */
532
533     if (e->x < e->points[v].x[0])
534         return e->points[v].y.i[0];
535
536     for (;;) {
537         if (e->points[v].n_current+1 >= e->points[v].n_points)
538             return e->points[v].y.i[e->points[v].n_points-1];
539
540         if (e->x < e->points[v].x[e->points[v].n_current+1])
541             break;
542
543         e->points[v].n_current++;
544         e->points[v].cached_valid = FALSE;
545     }
546
547     if (!e->points[v].cached_valid) {
548         e->points[v].cached_dx = e->points[v].x[e->points[v].n_current+1] - e->points[v].x[e->points[v].n_current];
549         e->points[v].cached_dy_i = e->points[v].y.i[e->points[v].n_current+1] - e->points[v].y.i[e->points[v].n_current];
550         e->points[v].cached_valid = TRUE;
551     }
552
553     return e->points[v].y.i[e->points[v].n_current] + (e->points[v].cached_dy_i * (int32_t) (e->x - e->points[v].x[e->points[v].n_current])) / (int32_t) e->points[v].cached_dx;
554 }
555
556 static float linear_get_float(pa_envelope *e, int v) {
557     pa_assert(e);
558
559     if (e->x < e->points[v].x[0])
560         return e->points[v].y.f[0];
561
562     for (;;) {
563         if (e->points[v].n_current+1 >= e->points[v].n_points)
564             return e->points[v].y.f[e->points[v].n_points-1];
565
566         if (e->x < e->points[v].x[e->points[v].n_current+1])
567             break;
568
569         e->points[v].n_current++;
570         e->points[v].cached_valid = FALSE;
571     }
572
573     if (!e->points[v].cached_valid) {
574         e->points[v].cached_dy_dx =
575             (e->points[v].y.f[e->points[v].n_current+1] - e->points[v].y.f[e->points[v].n_current]) /
576             ((float) e->points[v].x[e->points[v].n_current+1] - (float) e->points[v].x[e->points[v].n_current]);
577         e->points[v].cached_valid = TRUE;
578     }
579
580     return e->points[v].y.f[e->points[v].n_current] + (float) (e->x - e->points[v].x[e->points[v].n_current]) * e->points[v].cached_dy_dx;
581 }
582
583 void pa_envelope_apply(pa_envelope *e, pa_memchunk *chunk) {
584     int v;
585
586     pa_assert(e);
587     pa_assert(chunk);
588
589     envelope_begin_read(e, &v);
590
591     if (e->points[v].n_points > 0) {
592         void *p;
593         size_t fs, n;
594
595         pa_memchunk_make_writable(chunk, 0);
596         p = (uint8_t*) pa_memblock_acquire(chunk->memblock) + chunk->index;
597         fs = pa_frame_size(&e->sample_spec);
598         n = chunk->length;
599
600         switch (e->sample_spec.format) {
601
602             case PA_SAMPLE_U8: {
603                 uint8_t *t;
604
605                 for (t = p; n > 0; n -= fs) {
606                     int32_t factor = linear_get_int(e, v);
607                     unsigned c;
608                     e->x += fs;
609
610                     for (c = 0; c < e->sample_spec.channels; c++, t++)
611                         *t = (uint8_t) (((factor * ((int16_t) *t - 0x80)) / 0x10000) + 0x80);
612                 }
613
614                 break;
615             }
616
617             case PA_SAMPLE_ULAW: {
618                 uint8_t *t;
619
620                 for (t = p; n > 0; n -= fs) {
621                     int32_t factor = linear_get_int(e, v);
622                     unsigned c;
623                     e->x += fs;
624
625                     for (c = 0; c < e->sample_spec.channels; c++, t++) {
626                         int16_t k = st_ulaw2linear16(*t);
627                         *t = (uint8_t) st_14linear2ulaw((int16_t) (((factor * k) / 0x10000) >> 2));
628                     }
629                 }
630
631                 break;
632             }
633
634             case PA_SAMPLE_ALAW: {
635                 uint8_t *t;
636
637                 for (t = p; n > 0; n -= fs) {
638                     int32_t factor = linear_get_int(e, v);
639                     unsigned c;
640                     e->x += fs;
641
642                     for (c = 0; c < e->sample_spec.channels; c++, t++) {
643                         int16_t k = st_alaw2linear16(*t);
644                         *t = (uint8_t) st_13linear2alaw((int16_t) (((factor * k) / 0x10000) >> 3));
645                     }
646                 }
647
648                 break;
649             }
650
651             case PA_SAMPLE_S16NE: {
652                 int16_t *t;
653
654                 for (t = p; n > 0; n -= fs) {
655                     int32_t factor = linear_get_int(e, v);
656                     unsigned c;
657                     e->x += fs;
658
659                     for (c = 0; c < e->sample_spec.channels; c++, t++)
660                         *t = (int16_t) ((factor * *t) / 0x10000);
661                 }
662
663                 break;
664             }
665
666             case PA_SAMPLE_S16RE: {
667                 int16_t *t;
668
669                 for (t = p; n > 0; n -= fs) {
670                     int32_t factor = linear_get_int(e, v);
671                     unsigned c;
672                     e->x += fs;
673
674                     for (c = 0; c < e->sample_spec.channels; c++, t++) {
675                         int16_t r = (int16_t) ((factor * PA_INT16_SWAP(*t)) / 0x10000);
676                         *t = PA_INT16_SWAP(r);
677                     }
678                 }
679
680                 break;
681             }
682
683             case PA_SAMPLE_S32NE: {
684                 int32_t *t;
685
686                 for (t = p; n > 0; n -= fs) {
687                     int32_t factor = linear_get_int(e, v);
688                     unsigned c;
689                     e->x += fs;
690
691                     for (c = 0; c < e->sample_spec.channels; c++, t++)
692                         *t = (int32_t) (((int64_t) factor * (int64_t) *t) / 0x10000);
693                 }
694
695                 break;
696             }
697
698             case PA_SAMPLE_S32RE: {
699                 int32_t *t;
700
701                 for (t = p; n > 0; n -= fs) {
702                     int32_t factor = linear_get_int(e, v);
703                     unsigned c;
704                     e->x += fs;
705
706                     for (c = 0; c < e->sample_spec.channels; c++, t++) {
707                         int32_t r = (int32_t) (((int64_t) factor * (int64_t) PA_INT32_SWAP(*t)) / 0x10000);
708                         *t = PA_INT32_SWAP(r);
709                     }
710                 }
711
712                 break;
713             }
714
715             case PA_SAMPLE_FLOAT32NE: {
716                 float *t;
717
718                 for (t = p; n > 0; n -= fs) {
719                     float factor = linear_get_float(e, v);
720                     unsigned c;
721                     e->x += fs;
722
723                     for (c = 0; c < e->sample_spec.channels; c++, t++)
724                         *t = *t * factor;
725                 }
726
727                 break;
728             }
729
730             case PA_SAMPLE_FLOAT32RE: {
731                 float *t;
732
733                 for (t = p; n > 0; n -= fs) {
734                     float factor = linear_get_float(e, v);
735                     unsigned c;
736                     e->x += fs;
737
738                     for (c = 0; c < e->sample_spec.channels; c++, t++) {
739                         float r = PA_FLOAT32_SWAP(*t) * factor;
740                         *t = PA_FLOAT32_SWAP(r);
741                     }
742                 }
743
744                 break;
745             }
746
747             case PA_SAMPLE_S24LE:
748             case PA_SAMPLE_S24BE:
749             case PA_SAMPLE_S24_32LE:
750             case PA_SAMPLE_S24_32BE:
751                 /* FIXME */
752                 pa_assert_not_reached();
753
754             case PA_SAMPLE_MAX:
755             case PA_SAMPLE_INVALID:
756                 pa_assert_not_reached();
757         }
758
759         pa_memblock_release(chunk->memblock);
760
761         e->x += chunk->length;
762     } else {
763         /* When we have no envelope to apply we reset our origin */
764         e->x = 0;
765     }
766
767     envelope_commit_read(e, v);
768 }
769
770 void pa_envelope_rewind(pa_envelope *e, size_t n_bytes) {
771     int v;
772
773     pa_assert(e);
774
775     envelope_begin_read(e, &v);
776
777     if (n_bytes < e->x)
778         e->x -= n_bytes;
779     else
780         e->x = 0;
781
782     e->points[v].n_current = 0;
783     e->points[v].cached_valid = FALSE;
784
785     envelope_commit_read(e, v);
786 }