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