b54538f7 |
1 | /* |
2 | * Copyright (C) 2013 nu774 |
3 | * For conditions of distribution and use, see copyright notice in COPYING |
4 | */ |
5 | #if HAVE_CONFIG_H |
6 | # include "config.h" |
7 | #endif |
8 | #if HAVE_STDINT_H |
9 | # include <stdint.h> |
10 | #endif |
11 | #include <stddef.h> |
12 | #include <stdlib.h> |
13 | #include <string.h> |
14 | #include <math.h> |
15 | #include <assert.h> |
16 | #include "pcm_reader.h" |
17 | #include "lpc.h" |
18 | |
19 | typedef struct buffer_t { |
20 | void *data; |
21 | unsigned count; |
22 | unsigned capacity; |
23 | unsigned head; |
24 | } buffer_t; |
25 | |
26 | typedef struct limiter_t { |
27 | pcm_reader_vtbl_t *vtbl; |
28 | pcm_reader_t *src; |
29 | pcm_sample_description_t format; |
30 | int64_t position; |
31 | buffer_t buffers[1]; |
32 | } limiter_t; |
33 | |
34 | static inline pcm_reader_t *get_source(pcm_reader_t *reader) |
35 | { |
36 | return ((limiter_t *)reader)->src; |
37 | } |
38 | |
39 | static const |
40 | pcm_sample_description_t *get_format(pcm_reader_t *reader) |
41 | { |
42 | return pcm_get_format(get_source(reader)); |
43 | } |
44 | |
45 | static int64_t get_length(pcm_reader_t *reader) |
46 | { |
47 | return pcm_get_length(get_source(reader)); |
48 | } |
49 | |
50 | static int64_t get_position(pcm_reader_t *reader) |
51 | { |
52 | return ((limiter_t *)reader)->position; |
53 | } |
54 | |
55 | static int reserve_buffer(buffer_t *bp, size_t required, unsigned unit) |
56 | { |
57 | if (bp->capacity < required) { |
58 | unsigned newsize = 1; |
59 | void *p; |
60 | while (newsize < required) |
61 | newsize <<= 1; |
62 | p = realloc(bp->data, newsize * unit); |
63 | if (!p) return -1; |
64 | bp->data = p; |
65 | bp->capacity = newsize; |
66 | } |
67 | return 0; |
68 | } |
69 | |
70 | static int read_frames(pcm_reader_t *reader, void *buffer, unsigned nframes) |
71 | { |
72 | limiter_t *self = (limiter_t *)reader; |
73 | unsigned i, n, res, nch = self->format.channels_per_frame; |
74 | size_t bytes = nframes * pcm_get_format(self->src)->bytes_per_frame; |
75 | buffer_t *ibp = &self->buffers[nch]; |
76 | float *obp = buffer; |
77 | |
78 | do { |
79 | if (reserve_buffer(ibp, bytes, 1) < 0) |
80 | return -1; |
81 | res = pcm_read_frames(self->src, ibp->data, nframes); |
82 | for (n = 0; n < nch; ++n) { |
83 | float *ip = (float *)ibp->data, *x; |
84 | buffer_t *bp = &self->buffers[n]; |
85 | unsigned end, limit; |
86 | if (reserve_buffer(bp, bp->count + res, sizeof(float)) < 0) |
87 | return -1; |
88 | x = bp->data; |
89 | for (i = 0; i < res; ++i) |
90 | x[bp->count++] = pcm_clip(ip[i * nch + n], -3.0, 3.0); |
91 | limit = bp->count; |
92 | if (limit > 0 && res > 0) { |
93 | float last = x[limit - 1]; |
94 | for (; limit > 0 && x[limit-1] * last > 0; --limit) |
95 | ; |
96 | } |
97 | end = bp->head; |
98 | while (end < limit) { |
99 | unsigned start, peak_pos; |
100 | float peak; |
101 | for (peak_pos = end; peak_pos < limit; ++peak_pos) |
102 | if (x[peak_pos] > 1.0f || x[peak_pos] < -1.0f) |
103 | break; |
104 | if (peak_pos == limit) |
105 | break; |
106 | start = peak_pos; |
107 | peak = fabs(x[peak_pos]); |
108 | while (start > bp->head && x[peak_pos] * x[start] >= 0.0f) |
109 | --start; |
110 | ++start; |
111 | for (end = peak_pos + 1; end < limit; ++end) { |
112 | float y; |
113 | if (x[peak_pos] * x[end] < 0.0f) |
114 | break; |
115 | y = fabs(x[end]); |
116 | if (y > peak) { |
117 | peak = y; |
118 | peak_pos = end; |
119 | } |
120 | } |
121 | if (peak < 2.0f) { |
122 | float a = (peak - 1.0f) / (peak * peak); |
123 | if (x[peak_pos] > 0.0f) a = -a; |
124 | for (i = start; i < end; ++i) |
125 | x[i] = x[i] + a * x[i] * x[i]; |
126 | } else { |
127 | float u = peak, v = 1.0f; |
128 | float a = (u - 2.0f * v) / (u * u * u); |
129 | float b = (3.0f * v - 2.0f * u) / (u * u); |
130 | if (x[peak_pos] < 0.0f) b = -b; |
131 | for (i = start; i < end; ++i) |
132 | x[i] = x[i] + b * x[i] * x[i] + a * x[i] * x[i] * x[i]; |
133 | } |
134 | } |
135 | bp->head = limit; |
136 | } |
137 | res = nframes; |
138 | for (n = 0; n < nch; ++n) |
139 | if (self->buffers[n].head < res) |
140 | res = self->buffers[n].head; |
141 | for (i = 0; i < res; ++i) |
142 | for (n = 0; n < nch; ++n) |
143 | *obp++ = ((float *)self->buffers[n].data)[i]; |
144 | if (res) { |
145 | for (n = 0; n < nch; ++n) { |
146 | buffer_t *bp = &self->buffers[n]; |
147 | float *p = bp->data; |
148 | memmove(p, p + res, (bp->count - res) * sizeof(float)); |
149 | bp->count -= res; |
150 | bp->head -= res; |
151 | } |
152 | } |
153 | } while (res == 0 && self->buffers[0].count); |
154 | self->position += res; |
155 | return res; |
156 | } |
157 | |
158 | static void teardown(pcm_reader_t **reader) |
159 | { |
160 | int i; |
161 | limiter_t *self = (limiter_t *)*reader; |
162 | pcm_teardown(&self->src); |
163 | for (i = 0; i < self->format.channels_per_frame + 1; ++i) |
164 | free(self->buffers[i].data); |
165 | free(self); |
166 | *reader = 0; |
167 | } |
168 | |
169 | static pcm_reader_vtbl_t my_vtable = { |
170 | get_format, get_length, get_position, read_frames, teardown |
171 | }; |
172 | |
173 | pcm_reader_t *limiter_open(pcm_reader_t *reader) |
174 | { |
175 | limiter_t *self; |
176 | int n = pcm_get_format(reader)->channels_per_frame; |
177 | size_t size = sizeof(limiter_t) + offsetof(limiter_t, buffers[n + 1]); |
178 | |
179 | if ((self = calloc(1, size)) == 0) |
180 | return 0; |
181 | self->src = reader; |
182 | self->vtbl = &my_vtable; |
183 | self->format = *pcm_get_format(reader); |
184 | self->format.bits_per_channel = 32; |
185 | return (pcm_reader_t *)self; |
186 | } |