aboutsummaryrefslogtreecommitdiffstats
path: root/src/pcm/dsd2pcm/noiseshape.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/pcm/dsd2pcm/noiseshape.c')
-rw-r--r--src/pcm/dsd2pcm/noiseshape.c83
1 files changed, 83 insertions, 0 deletions
diff --git a/src/pcm/dsd2pcm/noiseshape.c b/src/pcm/dsd2pcm/noiseshape.c
new file mode 100644
index 000000000..ecd2f251d
--- /dev/null
+++ b/src/pcm/dsd2pcm/noiseshape.c
@@ -0,0 +1,83 @@
+#include <stdlib.h>
+#include <string.h>
+
+#include "noiseshape.h"
+
+extern int noise_shape_init(
+ noise_shape_ctx *ctx,
+ int sos_count,
+ const float *coeffs)
+{
+ int i;
+ ctx->sos_count = sos_count;
+ ctx->bbaa = coeffs;
+ ctx->t1 = (float*) malloc(sizeof(float)*sos_count);
+ if (!ctx->t1) goto escape1;
+ ctx->t2 = (float*) malloc(sizeof(float)*sos_count);
+ if (!ctx->t2) goto escape2;
+ for (i=0; i<sos_count; ++i) {
+ ctx->t1[i] = 0.f;
+ ctx->t2[i] = 0.f;
+ }
+ return 0;
+escape2:
+ free(ctx->t1);
+escape1:
+ return -1;
+}
+
+extern void noise_shape_destroy(
+ noise_shape_ctx *ctx)
+{
+ free(ctx->t1);
+ free(ctx->t2);
+}
+
+extern int noise_shape_clone(
+ const noise_shape_ctx *from,
+ noise_shape_ctx *to)
+{
+ to->sos_count = from->sos_count;
+ to->bbaa = from->bbaa;
+ to->t1 = (float*) malloc(sizeof(float)*to->sos_count);
+ if (!to->t1) goto error1;
+ to->t2 = (float*) malloc(sizeof(float)*to->sos_count);
+ if (!to->t2) goto error2;
+ memcpy(to->t1,from->t1,sizeof(float)*to->sos_count);
+ memcpy(to->t2,from->t2,sizeof(float)*to->sos_count);
+ return 0;
+error2:
+ free(to->t1);
+error1:
+ return -1;
+}
+
+extern float noise_shape_get(noise_shape_ctx *ctx)
+{
+ int i;
+ float acc;
+ const float *c;
+ acc = 0.0;
+ c = ctx->bbaa;
+ for (i=0; i<ctx->sos_count; ++i) {
+ float t1i = ctx->t1[i];
+ float t2i = ctx->t2[i];
+ ctx->t2[i] = acc -= t1i * c[2] + t2i * c[3];
+ acc += t1i * c[0] + t2i * c[1];
+ c += 4;
+ }
+ return acc;
+}
+
+extern void noise_shape_update(noise_shape_ctx *ctx, float qerror)
+{
+ float *p;
+ int i;
+ for (i=0; i<ctx->sos_count; ++i) {
+ ctx->t2[i] += qerror;
+ }
+ p = ctx->t1;
+ ctx->t1 = ctx->t2;
+ ctx->t2 = p;
+}
+