apply limiter when input is float

[fdkaac.git] / src / limiter.c

diff --git a/src/limiter.c b/src/limiter.c
new file mode 100644 (file)
index 0000000..75eb855
--- /dev/null
+++ b/src/limiter.c
@@ -0,0 +1,186 @@
+/* 
+ * Copyright (C) 2013 nu774
+ * For conditions of distribution and use, see copyright notice in COPYING
+ */
+#if HAVE_CONFIG_H
+#  include "config.h"
+#endif
+#if HAVE_STDINT_H
+#  include <stdint.h>
+#endif
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <assert.h>
+#include "pcm_reader.h"
+#include "lpc.h"
+
+typedef struct buffer_t {
+    void *data;
+    unsigned count;
+    unsigned capacity;
+    unsigned head;
+} buffer_t;
+
+typedef struct limiter_t {
+    pcm_reader_vtbl_t *vtbl;
+    pcm_reader_t *src;
+    pcm_sample_description_t format;
+    int64_t position;
+    buffer_t buffers[1];
+} limiter_t;
+
+static inline pcm_reader_t *get_source(pcm_reader_t *reader)
+{
+    return ((limiter_t *)reader)->src;
+}
+
+static const
+pcm_sample_description_t *get_format(pcm_reader_t *reader)
+{
+    return pcm_get_format(get_source(reader));
+}
+
+static int64_t get_length(pcm_reader_t *reader)
+{
+    return pcm_get_length(get_source(reader));
+}
+
+static int64_t get_position(pcm_reader_t *reader)
+{
+    return ((limiter_t *)reader)->position;
+}
+
+static int reserve_buffer(buffer_t *bp, size_t required, unsigned unit)
+{
+    if (bp->capacity < required) {
+        unsigned newsize = 1;
+        void *p;
+        while (newsize < required)
+            newsize <<= 1;
+        p = realloc(bp->data, newsize * unit);
+        if (!p) return -1;
+        bp->data = p;
+        bp->capacity = newsize;
+    }
+    return 0;
+}
+
+static int read_frames(pcm_reader_t *reader, void *buffer, unsigned nframes)
+{
+    limiter_t *self = (limiter_t *)reader;
+    unsigned i, n, res, nch = self->format.channels_per_frame;
+    size_t bytes = nframes * pcm_get_format(self->src)->bytes_per_frame;
+    buffer_t *ibp = &self->buffers[nch];
+    float *obp = buffer;
+
+    do {
+        if (reserve_buffer(ibp, bytes, 1) < 0)
+           return -1;
+        res = pcm_read_frames(self->src, ibp->data, nframes);
+        for (n = 0; n < nch; ++n) {
+            float *ip = (float *)ibp->data, *x;
+            buffer_t *bp = &self->buffers[n];
+            unsigned end, limit;
+            if (reserve_buffer(bp, bp->count + res, sizeof(float)) < 0)
+                return -1;
+            x = bp->data;
+            for (i = 0; i < res; ++i)
+                x[bp->count++] = pcm_clip(ip[i * nch + n], -3.0, 3.0);
+            limit = bp->count;
+            if (limit > 0 && res > 0) {
+                float last = x[limit - 1];
+                for (; limit > 0 && x[limit-1] * last > 0; --limit)
+                    ;
+            }
+            end = bp->head;
+            while (end < limit) {
+                unsigned start, peak_pos;
+                float peak;
+                for (peak_pos = end; peak_pos < limit; ++peak_pos)
+                    if (x[peak_pos] > 1.0f || x[peak_pos] < -1.0f)
+                        break;
+                if (peak_pos == limit)
+                    break;
+                start = peak_pos;
+                peak = fabs(x[peak_pos]);
+                while (start > bp->head && x[peak_pos] * x[start] >= 0.0f)
+                    --start;
+                ++start;
+                for (end = peak_pos + 1; end < limit; ++end) {
+                    float y;
+                    if (x[peak_pos] * x[end] < 0.0f)
+                        break;
+                    y = fabs(x[end]);
+                    if (y > peak) {
+                        peak = y;
+                        peak_pos = end;
+                    }
+                }
+                if (peak < 2.0f) {
+                    float a = (peak - 1.0f) / (peak * peak);
+                    if (x[peak_pos] > 0.0f) a = -a;
+                    for (i = start; i < end; ++i)
+                        x[i] = x[i] + a * x[i] * x[i];
+                } else {
+                    float u = peak, v = 1.0f;
+                    float a = (u - 2.0f * v) / (u * u * u);
+                    float b = (3.0f * v - 2.0f * u) / (u * u);
+                    if (x[peak_pos] < 0.0f) b = -b;
+                    for (i = start; i < end; ++i)
+                        x[i] = x[i] + b * x[i] * x[i] + a * x[i] * x[i] * x[i];
+                }
+            }
+            bp->head = limit;
+        }
+        res = nframes;
+        for (n = 0; n < nch; ++n)
+            if (self->buffers[n].head < res)
+                res = self->buffers[n].head;
+        for (i = 0; i < res; ++i)
+            for (n = 0; n < nch; ++n)
+                *obp++ = ((float *)self->buffers[n].data)[i];
+        if (res) {
+            for (n = 0; n < nch; ++n) {
+                buffer_t *bp = &self->buffers[n];
+                float *p = bp->data;
+                memmove(p, p + res, (bp->count - res) * sizeof(float));
+                bp->count -= res;
+                bp->head  -= res;
+            }
+        }
+    } while (res == 0 && self->buffers[0].count);
+    self->position += res;
+    return res;
+}
+
+static void teardown(pcm_reader_t **reader)
+{
+    int i;
+    limiter_t *self = (limiter_t *)*reader;
+    pcm_teardown(&self->src);
+    for (i = 0; i < self->format.channels_per_frame + 1; ++i)
+        free(self->buffers[i].data);
+    free(self);
+    *reader = 0;
+}
+
+static pcm_reader_vtbl_t my_vtable = {
+    get_format, get_length, get_position, read_frames, teardown
+};
+
+pcm_reader_t *limiter_open(pcm_reader_t *reader)
+{
+    limiter_t *self;
+    int n = pcm_get_format(reader)->channels_per_frame;
+    size_t size = sizeof(limiter_t) + offsetof(limiter_t, buffers[n + 1]);
+
+    if ((self = calloc(1, size)) == 0)
+        return 0;
+    self->src = reader;
+    self->vtbl = &my_vtable;
+    self->format = *pcm_get_format(reader);
+    self->format.bits_per_channel = 32;
+    return (pcm_reader_t *)self;
+}