src/limiter.c

   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 }