Initial implementation of polyphase quadrature filterbank

1 files changed, 267 insertions, 0 deletions

diff --git a/src/pqf.c b/src/pqf.c
new file mode 100644
index 0000000..a6e255b
--- /dev/null
+++ b/src/pqf.c
@@ -0,0 +1,267 @@
+/*
+ * This file is part of libpqf.
+ *
+ * libpqf is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * AtracDEnc is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with AtracDEnc; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+/*
+ * Musepack pqf implementation was used as reference.
+ * see svn.musepack.net
+ */
+
+#include <stdlib.h>
+
+#include <string.h>
+#include <math.h>
+
+#include "libpqf.h"
+
+struct pqf_a_ctx {
+    float* buf;
+    float* proto;
+    float* y;
+    float* c;
+    float* m;
+
+    uint16_t proto_sz;
+    uint16_t subbands_num;
+    uint16_t subband_size;
+
+    uint16_t proto_step;
+};
+
+static inline uint16_t get_frame_sz(pqf_a_ctx_t ctx)
+{
+    return ctx->subbands_num * ctx->subband_size;
+}
+
+static void init(pqf_a_ctx_t ctx)
+{
+    uint16_t i, j;
+    const float* const t = ctx->proto;
+    uint16_t subbands_num = ctx->subbands_num; 
+    float* c = ctx->c;
+    float* m = ctx->m;
+    uint16_t dsb = subbands_num * 2;
+    uint16_t proto_step = ctx->proto_step;
+
+    for (i = 0; i < subbands_num; i++) {
+        for (j = 0; j < subbands_num; j++) {
+            m[i * subbands_num + j] = (float)cos(((2 * i + 1) * j & 127) * M_PI / (subbands_num * 2));
+        }
+    }
+
+    for (i = 0; i < proto_step; i++) {
+        for (j = 0; j < dsb; j++) {
+            if (i & 1) {
+                c[j * proto_step + i] = 1 * t[j + dsb * i];
+            } else {
+                c[j * proto_step + i] = -1 * t[j + dsb * i];
+            }
+        }
+    }
+}
+
+static void vectoring(const float* x, float* y, pqf_a_ctx_t ctx)
+{
+    uint16_t i, j;
+
+    const float* c = ctx->c;
+    uint16_t subbands_num = ctx->subbands_num;
+    uint16_t dsb = ctx->subbands_num << 1;
+    uint16_t proto_step = ctx->proto_step;
+
+    for (i = 0; i < dsb; i++, c += proto_step, x += 1, y += 1 ) {
+        y[0] = 0;
+        for (j = 0; j < proto_step; j++) {
+            y[0] += c[j] * x[j * dsb];
+        }
+    }
+}
+
+static void matrixing(const float* y, float* samples, pqf_a_ctx_t ctx)
+{
+    uint16_t i, j, k;
+    uint16_t subbands_num = ctx->subbands_num;
+    uint16_t subband_size = ctx->subband_size;
+    const float* mi = ctx->m;
+
+    for (i = 0; i < subbands_num; i++, mi += subbands_num, samples += subband_size) {
+        for (j = 0, k = subbands_num / 2; j < subbands_num / 2; j++, k--) {
+            samples[0] += (y[j] + y[subbands_num - j]) * mi[k];
+        }
+        for (j = 1; j < subbands_num / 2; j++) {
+            samples[0] += (y[subbands_num + j] - y[subbands_num * 2 - j]) * mi[subbands_num/2  + j];
+        }
+	samples[0] += y[subbands_num/2];
+    }
+}
+
+static void a_init(pqf_a_ctx_t ctx)
+{
+    init(ctx);
+}
+
+static pqf_status_t check_params(uint16_t subband_sz, uint16_t subbands_num, uint16_t proto_sz)
+{
+    if (!subbands_num || subbands_num > 128) {
+        return PQF_WRONG_SUBBANDS_NUM;
+    }
+
+    if (!proto_sz || proto_sz < subbands_num) {
+        return PQF_WRONG_SUBBAND_SZ;
+    }
+
+    if (!subband_sz || subband_sz > (0x7fff / subbands_num)) {
+        return PQF_FRAME_TOO_LONG;
+    }
+
+    return PQF_SUCCESS;
+}
+
+pqf_status_t pqf_create_a_ctx(uint16_t subband_sz, uint16_t subbands_num, uint16_t proto_sz, float* proto, pqf_a_ctx_t* ctx_result)
+{
+    uint16_t i = 0;
+
+    uint16_t frame_sz = subband_sz * subbands_num;
+    uint16_t extra_sz = proto_sz - subbands_num;
+    uint16_t buf_sz = frame_sz + extra_sz;
+    pqf_status_t status;
+
+    if (ctx_result == NULL)
+        return PQF_CONTRACT_VIOLATION;
+
+    if ((status = check_params(subband_sz, subbands_num, proto_sz)) != PQF_SUCCESS)
+        return status;
+
+    pqf_a_ctx_t ctx = (pqf_a_ctx_t)malloc(sizeof(struct pqf_a_ctx));
+
+    if (!ctx)
+        return PQF_NOMEM;
+
+    ctx->proto_sz = proto_sz;
+    ctx->subbands_num = subbands_num;
+    ctx->subband_size = subband_sz;
+    ctx->proto_step = proto_sz / (subbands_num << 1);
+
+    ctx->buf = (float*)malloc(sizeof(float) * buf_sz);
+
+    if (!ctx->buf)
+        goto nomem_buf;
+
+    for (i = 0; i < buf_sz; i++) {
+        ctx->buf[i] = 0.0;
+    }
+
+    ctx->proto = (float*)malloc(sizeof(float) * proto_sz);
+
+    if (!ctx->proto)
+        goto nomem_proto;
+
+    memcpy(ctx->proto, proto, sizeof(float) * proto_sz);
+
+    ctx->y = (float*)malloc(sizeof(float) * subbands_num * 2);
+
+    if (!ctx->y)
+        goto nomem_y;
+
+    ctx->c = (float*)malloc(sizeof(float) * proto_sz);
+
+    if (!ctx->c)
+        goto nomem_c;
+
+    ctx->m = (float*)malloc(sizeof(float) * subbands_num * subbands_num);
+
+    if (!ctx->c)
+        goto nomem_m;
+
+    a_init(ctx);
+
+    *ctx_result = ctx;
+
+    return PQF_SUCCESS;
+
+nomem_m:
+    free(ctx->c);
+
+nomem_c:
+    free(ctx->y);
+
+nomem_y:
+    free(ctx->proto);
+
+nomem_proto:
+    free(ctx->buf);
+
+nomem_buf:
+    free(ctx);
+
+    return PQF_NOMEM;
+}
+
+uint16_t pqf_get_frame_sz(pqf_a_ctx_t ctx)
+{
+    return get_frame_sz(ctx);
+}
+
+uint16_t pqf_get_subband_sz(pqf_a_ctx_t ctx)
+{
+    return ctx->subband_size;
+}
+
+uint16_t pqf_get_subbands_num(pqf_a_ctx_t ctx)
+{
+    return ctx->subbands_num;
+}
+
+void pqf_free_a_ctx(pqf_a_ctx_t ctx)
+{
+    free(ctx->m);
+    free(ctx->y);
+    free(ctx->c);
+    free(ctx->proto);
+    free(ctx->buf);
+    free(ctx);
+}
+
+void pqf_do_analyse(pqf_a_ctx_t ctx, const float* in, float* out)
+{
+    float* y = ctx->y;
+
+    float* x;
+    const float* pcm;
+    uint16_t n, i;
+
+    float* buf = ctx->buf;
+    uint16_t subband_size = ctx->subband_size;
+    uint16_t subbands_num = ctx->subbands_num;
+    uint16_t frame_sz = subband_size * subbands_num;
+    uint16_t extra_sz = ctx->proto_sz - subbands_num;
+
+    memcpy(buf + frame_sz, buf, extra_sz * sizeof(float));
+    x = buf + frame_sz;
+
+    pcm = in + (subbands_num - 1);
+
+    for (n = 0; n < subband_size; n++, pcm += subbands_num * 2) {
+        x -= subbands_num;
+        for (i = 0; i < subbands_num; i++) {
+            x[i] = *pcm--;
+        }
+        vectoring(x, y, ctx);
+        matrixing (y, &out[n], ctx);
+    }
+}
+