]>
Commit | Line | Data |
---|---|---|
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 | } |