From ca01d52a8658ed01301f14a4d294097d4db533cf Mon Sep 17 00:00:00 2001
From: jaseg <git-bigdata-wsl-arch@jaseg.de>
Date: Mon, 2 Mar 2020 19:42:36 +0100
Subject: Finishing up freq meas

---
 controller/fw/src/freq_meas.c          |  94 ++++++++++++
 controller/fw/src/freq_meas.h          |   7 +
 controller/fw/src/gold_code.h          |   4 +
 controller/fw/src/ldpc_decoder.c       | 267 +++++++++++++++++++++++++++++++++
 controller/fw/src/ldpc_decoder_test.py |  72 +++++++++
 controller/fw/src/test_decoder.py      | 168 +++++++++++++++++++++
 controller/fw/src/test_pyldpc_utils.py | 182 ++++++++++++++++++++++
 7 files changed, 794 insertions(+)
 create mode 100644 controller/fw/src/freq_meas.c
 create mode 100644 controller/fw/src/freq_meas.h
 create mode 100644 controller/fw/src/gold_code.h
 create mode 100644 controller/fw/src/ldpc_decoder.c
 create mode 100644 controller/fw/src/ldpc_decoder_test.py
 create mode 100644 controller/fw/src/test_decoder.py
 create mode 100644 controller/fw/src/test_pyldpc_utils.py

(limited to 'controller/fw/src')

diff --git a/controller/fw/src/freq_meas.c b/controller/fw/src/freq_meas.c
new file mode 100644
index 0000000..e6b3976
--- /dev/null
+++ b/controller/fw/src/freq_meas.c
@@ -0,0 +1,94 @@
+
+#include <unistd.h>
+#include <math.h>
+
+#include <arm_math.h>
+#include <levmarq.h>
+
+#include "freq_meas.h"
+#include "sr_global.h"
+
+
+/* FTT window lookup table defined in generated/fmeas_fft_window.c */
+extern const float * const fmeas_fft_window_table;
+
+/* jury-rig some definitions for these functions since the ARM headers only export an over-generalized variable bin size
+ * variant. */
+extern arm_status arm_rfft_32_fast_init_f32(arm_rfft_fast_instance_f32 * S);
+extern arm_status arm_rfft_64_fast_init_f32(arm_rfft_fast_instance_f32 * S);
+extern arm_status arm_rfft_128_fast_init_f32(arm_rfft_fast_instance_f32 * S);
+extern arm_status arm_rfft_256_fast_init_f32(arm_rfft_fast_instance_f32 * S);
+extern arm_status arm_rfft_512_fast_init_f32(arm_rfft_fast_instance_f32 * S);
+extern arm_status arm_rfft_1024_fast_init_f32(arm_rfft_fast_instance_f32 * S);
+extern arm_status arm_rfft_2048_fast_init_f32(arm_rfft_fast_instance_f32 * S);
+extern arm_status arm_rfft_4096_fast_init_f32(arm_rfft_fast_instance_f32 * S);
+
+#define CONCAT(A, B, C) A ## B ## C
+#define arm_rfft_init_name(nbits) CONCAT(arm_rfft_, nbits, _fast_init_f32)
+
+float func_gauss_grad(float *out, float *params, int x, void *userdata);
+float func_gauss(float *params, int x, void *userdata);
+
+int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
+    int rc;
+    float in_buf[FMEAS_FFT_LEN];
+    float out_buf[FMEAS_FFT_LEN];
+    for (size_t i=0; i<FMEAS_FFT_LEN; i++)
+        in_buf[i] = (float)adc_buf[i] / (float)FMEAS_ADC_MAX * fmeas_fft_window_table[i];
+
+    arm_rfft_fast_instance_f32 fft_inst;
+    if ((rc = arm_rfft_init_name(FMEAS_FFT_LEN)(&fft_inst)) != ARM_MATH_SUCCESS)
+        return rc;
+
+    arm_rfft_fast_f32(&fft_inst, in_buf, out_buf, 0);
+
+#define FMEAS_FFT_WINDOW_MIN_F 30.0f
+#define FMEAS_FFT_WINDOW_MAX_F 70.0f
+    const float binsize = (float)FMEAS_ADC_SAMPLING_RATE / FMEAS_FFT_LEN;
+    const int first_bin = (int)(FMEAS_FFT_WINDOW_MIN_F / binsize);
+    const int last_bin = (int)(FMEAS_FFT_WINDOW_MAX_F / binsize + 0.5f);
+    const int nbins = last_bin - first_bin + 1;
+
+    /* Copy real values of target data to front of output buffer */
+    for (size_t i=0; i<nbins; i++)
+        out_buf[i] = out_buf[2 * (first_bin + i)];
+
+    LMstat lmstat;
+    levmarq_init(&lmstat);
+
+    float a_max = 0.0f;
+    int i_max = 0;
+    for (size_t i=0; i<nbins; i++) {
+        if (out_buf[i] > a_max) {
+            a_max = out_buf[i];
+            i_max = i;
+        }
+    }
+
+    float par[3] = {
+        a_max, i_max, 1.0f
+    };
+
+    if (levmarq(3, &params, nbins, out_buf, NULL, func_gauss, func_gauss_grad, NULL, &lmstat))
+        return -1;
+
+    *out = (params[1] + first_bin) * binsize;
+
+    return 0;
+}
+
+float func_gauss(float *params, int x, void *userdata) {
+    UNUSED(userdata);
+    float a = params[0];
+    float mu = params[1];
+    float sigma = params[2];
+    return a*expf(-arm_power_f32((x-mu), 2.0f/(2.0f*(sigma*sigma))));
+}
+
+float func_gauss_grad(float *out, float *params, int x, void *userdata) {
+    UNUSED(userdata);
+    float a = params[0];
+    float mu = params[1];
+    float sigma = params[2];
+    return -(x-mu) / (  sigma*sigma*sigma * 2.5066282746310002f) * a*expf(-arm_power_f32((x-mu), 2.0f/(2.0f*(sigma*sigma))));
+}
diff --git a/controller/fw/src/freq_meas.h b/controller/fw/src/freq_meas.h
new file mode 100644
index 0000000..1c083f8
--- /dev/null
+++ b/controller/fw/src/freq_meas.h
@@ -0,0 +1,7 @@
+
+#ifndef __FREQ_MEAS_H__
+#define __FREQ_MEAS_H__
+
+int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out);
+
+#endif /* __FREQ_MEAS_H__ */
diff --git a/controller/fw/src/gold_code.h b/controller/fw/src/gold_code.h
new file mode 100644
index 0000000..739b477
--- /dev/null
+++ b/controller/fw/src/gold_code.h
@@ -0,0 +1,4 @@
+
+/* header file for generated gold code tables */
+
+extern const uint8_t * const gold_code_table;
diff --git a/controller/fw/src/ldpc_decoder.c b/controller/fw/src/ldpc_decoder.c
new file mode 100644
index 0000000..fe59d77
--- /dev/null
+++ b/controller/fw/src/ldpc_decoder.c
@@ -0,0 +1,267 @@
+
+#include <stdint.h>
+#include <unistd.h>
+#include <stdbool.h>
+#include <math.h>
+#include <stdio.h>
+
+
+void gausselimination(size_t n, size_t k, int8_t *A, int8_t *b);
+
+void inner_logbp(
+        size_t m, size_t n,
+        size_t bits_count, size_t nodes_count, const uint32_t bits_values[], const uint32_t nodes_values[],
+        int8_t Lc[],
+        float Lq[], float Lr[],
+        unsigned int n_iter,
+        float L_posteriori_out[]);
+
+//decode(384, 6, 8, ...)
+int decode(size_t n, size_t nodes_count, size_t bits_count, uint32_t bits[], int8_t y[], int8_t out[], unsigned int maxiter) {
+    const size_t m = n * nodes_count / bits_count;
+    float Lq[m*n];
+    float Lr[m*n];
+    float L_posteriori[n];
+
+    /* Calculate column bit positions from row bit positions */
+    int32_t bits_transposed[nodes_count * n];
+    for (size_t i=0; i<nodes_count * n; i++)
+        bits_transposed[i] = -1;
+
+    for (size_t i=0; i<m; i++) {
+        for (size_t j=0; j<bits_count; j++) {
+            int32_t *base = bits_transposed + bits[i*bits_count + j] * nodes_count;
+            for (; *base != -1; base++)
+                ;
+            *base = i;
+        }
+    }
+
+    /*
+    printf("Row positions: [");
+    for (size_t i=0; i<m*bits_count; i++) {
+        if (i)
+            printf(", ");
+        if (i%32 == 0)
+            printf("\n    ");
+        printf("%4d", bits[i]);
+    }
+    printf("\n]\n");
+
+    printf("Column positions: [");
+    for (size_t i=0; i<n*nodes_count; i++) {
+        if (i)
+            printf(", ");
+        if (i%32 == 0)
+            printf("\n    ");
+        printf("%4d", bits_transposed[i]);
+    }
+    printf("\n]\n");
+    */
+
+    /* Run iterative optimization algorithm */
+    for (unsigned int n_iter=0; n_iter<maxiter; n_iter++) {
+        inner_logbp(m, n, bits_count, nodes_count, bits, (uint32_t*)bits_transposed, y, Lq, Lr, n_iter, L_posteriori);
+
+        /*
+        float *arrs[3] = {Lq, Lr, L_posteriori};
+        const char *names[3] = {"Lq", "Lr", "L_posteriori"};
+        size_t lens[3] = {m*n, m*n, n};
+        const size_t head_tail = 10;
+        for (int j=0; j<3; j++) {
+            printf("%s=[", names[j]);
+            bool ellipsis = false;
+            const int w = 16;
+            for (size_t i=0; i<lens[j]; i++) {
+                if (lens[j] > 1000 && i/w > head_tail && i/w < m*n/w-head_tail) {
+                    if (!ellipsis) {
+                        ellipsis = true;
+                        printf("\n    ...");
+                    }
+                    continue;
+                }
+                if (i)
+                    printf(", ");
+                if (i%w == 0)
+                    printf("\n    ");
+                float outf = arrs[j][i];
+                char *s = outf < 0 ? "\033[91m" : (outf > 0 ? "\033[92m" : "\033[94m");
+                printf("%s% 012.6g\033[38;5;240m", s, outf);
+            }
+            printf("\n]\n");
+        }
+        */
+
+        for (size_t i=0; i<n; i++)
+            out[i] = L_posteriori[i] <= 0.0f;
+
+        for (size_t i=0; i<m; i++) {
+            bool sum = 0;
+            for (size_t j=0; j<bits_count; j++)
+                sum ^= out[bits[i*bits_count + j]];
+            if (sum)
+                continue;
+        }
+
+        fflush(stdout);
+        return n_iter;
+    }
+
+    fflush(stdout);
+    return -1;
+}
+
+/* Perform inner ext LogBP solver */
+void inner_logbp(
+        size_t m, size_t n,
+        size_t bits_count, size_t nodes_count, uint32_t const bits_values[], const uint32_t nodes_values[],
+        int8_t Lc[],
+        float Lq[], float Lr[],
+        unsigned int n_iter,
+        float L_posteriori_out[]) {
+
+    /*
+    printf("Input data: [");
+    for (size_t i=0; i<n; i++) {
+        if (i)
+            printf(", ");
+        if (i%32 == 0)
+            printf("\n    ");
+        printf("%4d", Lc[i]);
+    }
+    printf("\n]\n");
+    */
+
+    /* step 1 : Horizontal */
+    unsigned int bits_counter = 0;
+    for (size_t i=0; i<m; i++) {
+        //printf("=== i=%zu\n", i);
+        for (size_t p=bits_counter; p<bits_counter+bits_count; p++) {
+            size_t j = bits_values[p];
+            //printf("\033[38;5;240mj=%04zd ", j);
+
+            float x = 1;
+            if (n_iter == 0) {
+                for (size_t q=bits_counter; q<bits_counter+bits_count; q++) {
+                    if (bits_values[q] != j) {
+                        //int lcv = Lc[bits_values[q]];
+                        //char *s = lcv < 0 ? "\033[91m" : (lcv > 0 ? "\033[92m" : "\033[94m");
+                        //printf("nij=%04u Lc=%s%3d\033[38;5;240m ", bits_values[q], s, lcv);
+                        x *= tanhf(0.5f * Lc[bits_values[q]]);
+                    }
+                }
+
+            } else {
+                for (size_t q=bits_counter; q<bits_counter+bits_count; q++) {
+                    if (bits_values[q] != j)
+                        x *= tanhf(0.5f * Lq[i*n + bits_values[q]]);
+                }
+            }
+
+            //printf("\n==== i=%03zd p=%01zd x=%08f\n", i, p-bits_counter, x);
+
+            float num = 1 + x;
+            float denom = 1 - x;
+            if (num == 0)
+                Lr[i*n + j] = -1.0f;
+            else if (denom == 0)
+                Lr[i*n + j] = 1.0f;
+            else
+                Lr[i*n + j] = logf(num/denom);
+        }
+
+        bits_counter += bits_count;
+    }
+
+    /* step 2 : Vertical */
+    unsigned int nodes_counter = 0;
+    for (size_t j=0; j<n; j++) {
+        for (size_t p=bits_counter; p<nodes_counter+nodes_count; p++) {
+            size_t i = nodes_values[p];
+
+            Lq[i*n + j] = Lc[j];
+
+            for (size_t q=bits_counter; q<nodes_counter+nodes_count; q++) {
+                if (nodes_values[q] != i)
+                    Lq[i*n + j] += Lr[nodes_values[q]*n + j];
+            }
+        }
+
+        nodes_counter += nodes_count;
+    }
+
+    /* LLR a posteriori */
+    nodes_counter = 0;
+    for (size_t j=0; j<n; j++) {
+        float sum = 0;
+        for (size_t k=bits_counter; k<nodes_counter+nodes_count; k++)
+            sum += Lr[nodes_values[k]*n + j];
+        nodes_counter += nodes_count;
+
+        L_posteriori_out[j] = Lc[j] + sum;
+    }
+}
+
+/* Compute the original (k) bit message from a (n) bit codeword x.
+ *
+ * tG: (n, k)-matrix
+ * x: (n)-vector
+ * out: (k)-vector
+ */
+void get_message(size_t n, size_t k, int8_t *tG, int8_t *x, int8_t *out) {
+
+    gausselimination(n, k, tG, x);
+
+    out[k - 1] = x[k - 1];
+    for (ssize_t i=k-2; i>=0; i--) {
+        out[i] = x[i];
+
+        uint8_t sum = 0;
+        for (size_t j=i+1; j<k; j++)
+            sum ^= tG[i*k + j] * out[j];
+
+        out[i] = !!(out[i] - sum);
+    }
+}
+
+/* Solve linear system in Z/2Z via Gauss Gauss elimination.
+ *
+ * A: (n, k)-matrix
+ * b: (n)-vector
+ */
+void gausselimination(size_t n, size_t k, int8_t *A, int8_t *b) {
+    ssize_t d = k<n ? k : n;
+    for (ssize_t j=0; j<d; j++) {
+
+        ssize_t pivot = -1;
+        for (size_t i=j; i<n; i++) {
+            if (A[i*k + j]) {
+                pivot = i;
+                break;
+            }
+        }
+        if (pivot == -1)
+            continue;
+
+        if (pivot != j) {
+            for (size_t i=0; i<k; i++) {
+                int8_t tmp = A[j*k + i];
+                A[j*k + i] = A[pivot*k + i];
+                A[pivot*k + i] = tmp;
+            }
+
+            int8_t tmp = b[j];
+            b[j] = b[pivot];
+            b[pivot] = tmp;
+        }
+
+        for (size_t i=j+1; i<n; i++) {
+            if (A[i*k + j]) {
+                for (size_t p=0; p<k; p++)
+                    A[i*k + p] = !!(A[i*k + p] - A[j*k + p]);
+                b[i] = !!(b[i] - b[j]);
+            }
+        }
+    }
+}
+
diff --git a/controller/fw/src/ldpc_decoder_test.py b/controller/fw/src/ldpc_decoder_test.py
new file mode 100644
index 0000000..3b91bba
--- /dev/null
+++ b/controller/fw/src/ldpc_decoder_test.py
@@ -0,0 +1,72 @@
+
+import pyldpc
+import scipy.sparse
+import numpy as np
+import test_decoder
+import os, sys
+import ctypes as C
+import argparse
+
+if __name__ != '__main__':
+    raise RuntimeError("Please don't import this module, this is a command-line program.")
+
+parser = argparse.ArgumentParser()
+parser.add_argument('-r', '--reference', action='store_true', default=False, help='Run reference decoder instead of C implemention')
+args = parser.parse_args()
+
+lib = C.CDLL('./ldpc_decoder_test.so')
+
+n = 5*19
+nodes, bits = 17, 19
+H, G = pyldpc.make_ldpc(n, nodes, bits, systematic=False, seed=0)
+_1, bits_pos, _2 = scipy.sparse.find(H)
+_, k = G.shape
+
+st = np.random.RandomState(seed=0)
+test_data = st.randint(0, 2, k)
+d = np.dot(G, test_data) % 2
+x = (-1) ** d
+x[29:] = 0
+
+bits_pos = bits_pos.astype(np.uint32)
+x = x.astype(np.int8)
+
+lib.decode.argtypes = [C.c_size_t, C.c_size_t, C.c_size_t, C.POINTER(C.c_size_t), C.POINTER(C.c_int8), C.POINTER(C.c_int8), C.c_uint]
+lib.get_message.argtypes = [C.c_size_t, C.c_size_t, C.POINTER(C.c_int8), C.POINTER(C.c_int8), C.POINTER(C.c_int8)]
+
+if args.reference:
+    ref_out = test_decoder.decode(H, x, 3)
+    print('decoder output:', ref_out, flush=True)
+    print('msg reconstruction:', test_decoder.get_message(G, ref_out))
+    print('reference decoder: ', np.all(np.equal(test_decoder.get_message(G, ref_out), test_data)), flush=True)
+    np.set_printoptions(linewidth=220)
+    print(test_data)
+    print(test_decoder.get_message(G, ref_out))
+    print(test_decoder.get_message(G, ref_out) ^ test_data)
+
+else:
+    out = np.zeros(n, dtype=np.uint8)
+    # print('python data:', x, flush=True)
+    print('decoder iterations:', lib.decode(n, nodes, bits,
+        bits_pos.ctypes.data_as(C.POINTER(C.c_ulong)),
+        x.ctypes.data_as(C.POINTER(C.c_int8)),
+        out.ctypes.data_as(C.POINTER(C.c_int8)),
+        25), flush=True)
+    print('decoder output:', out)
+    print('msg reconstruction:', test_decoder.get_message(G, out.astype(np.int64)))
+    print('decoder under test:', np.all(np.equal(test_decoder.get_message(G, out.astype(np.int64)), test_data)))
+    np.set_printoptions(linewidth=220)
+    print(test_data)
+    print(test_decoder.get_message(G, out.astype(np.int64)))
+    G = G.astype(np.int8)
+    msg = np.zeros(k, dtype=np.int8)
+    lib.get_message(
+            n, k,
+            G.ctypes.data_as(C.POINTER(C.c_int8)),
+            out.astype(np.int8).ctypes.data_as(C.POINTER(C.c_int8)),
+            msg.ctypes.data_as(C.POINTER(C.c_int8)))
+    print(msg)
+    print(msg ^ test_data)
+
+print('codeword length:', len(x))
+print('data length:', len(test_data))
diff --git a/controller/fw/src/test_decoder.py b/controller/fw/src/test_decoder.py
new file mode 100644
index 0000000..8be5b02
--- /dev/null
+++ b/controller/fw/src/test_decoder.py
@@ -0,0 +1,168 @@
+"""Decoding module."""
+import numpy as np
+import warnings
+import test_pyldpc_utils as utils
+
+from numba import njit, int64, types, float64
+
+np.set_printoptions(linewidth=180, threshold=1000, edgeitems=20)
+
+def decode(H, y, snr, maxiter=100):
+    """Decode a Gaussian noise corrupted n bits message using BP algorithm.
+
+    Decoding is performed in parallel if multiple codewords are passed in y.
+
+    Parameters
+    ----------
+    H: array (n_equations, n_code). Decoding matrix H.
+    y: array (n_code, n_messages) or (n_code,). Received message(s) in the
+        codeword space.
+    maxiter: int. Maximum number of iterations of the BP algorithm.
+
+    Returns
+    -------
+    x: array (n_code,) or (n_code, n_messages) the solutions in the
+        codeword space.
+
+    """
+    m, n = H.shape
+
+    bits_hist, bits_values, nodes_hist, nodes_values = utils.bitsandnodes(H)
+
+    var = 10 ** (-snr / 10)
+
+    if y.ndim == 1:
+        y = y[:, None]
+    # step 0: initialization
+
+    Lc = 2 * y / var
+    _, n_messages = y.shape
+
+    Lq = np.zeros(shape=(m, n, n_messages))
+
+    Lr = np.zeros(shape=(m, n, n_messages))
+
+    for n_iter in range(maxiter):
+        #print(f'============================ iteration {n_iter} ============================')
+        Lq, Lr, L_posteriori = _logbp_numba(bits_hist, bits_values, nodes_hist,
+                                            nodes_values, Lc, Lq, Lr, n_iter)
+        #print("Lq=", Lq.flatten())
+        #print("Lr=", Lr.flatten())
+        #print("L_posteriori=", L_posteriori.flatten())
+        #print('L_posteriori=[')
+        #for row in L_posteriori.reshape([-1, 16]):
+        #    for val in row:
+        #        cc = '\033[91m' if val < 0 else ('\033[92m' if val > 0 else '\033[94m')
+        #        print(f"{cc}{val: 012.6g}\033[38;5;240m", end=', ')
+        #    print()
+        x = np.array(L_posteriori <= 0).astype(int)
+
+        product = utils.incode(H, x)
+
+        if product:
+            print(f'found, n_iter={n_iter}')
+            break
+
+    if n_iter == maxiter - 1:
+        warnings.warn("""Decoding stopped before convergence. You may want
+                       to increase maxiter""")
+    return x.squeeze()
+
+
+output_type_log2 = types.Tuple((float64[:, :, :], float64[:, :, :],
+                               float64[:, :]))
+
+
+#@njit(output_type_log2(int64[:], int64[:], int64[:], int64[:], float64[:, :],
+#                       float64[:, :, :],  float64[:, :, :], int64), cache=True)
+def _logbp_numba(bits_hist, bits_values, nodes_hist, nodes_values, Lc, Lq, Lr,
+                 n_iter):
+    """Perform inner ext LogBP solver."""
+    m, n, n_messages = Lr.shape
+    # step 1 : Horizontal
+
+    bits_counter = 0
+    nodes_counter = 0
+    for i in range(m):
+        #print(f'=== i={i}')
+        ff = bits_hist[i]
+        ni = bits_values[bits_counter: bits_counter + ff]
+        bits_counter += ff
+        for j_iter, j in enumerate(ni):
+            nij = ni[:]
+            #print(f'\033[38;5;240mj={j:04d}', end=' ')
+
+            X = np.ones(n_messages)
+            if n_iter == 0:
+                for kk in range(len(nij)):
+                    if nij[kk] != j:
+                        lcv = Lc[nij[kk],0]
+                        lcc = '\033[91m' if lcv < 0 else ('\033[92m' if lcv > 0 else '\033[94m')
+                        #print(f'nij={nij[kk]:04d} Lc={lcc}{lcv:> 8f}\033[38;5;240m', end=' ')
+                        X *= np.tanh(0.5 * Lc[nij[kk]])
+            else:
+                for kk in range(len(nij)):
+                    if nij[kk] != j:
+                        X *= np.tanh(0.5 * Lq[i, nij[kk]])
+            #print(f'\n==== {i:03d} {j_iter:01d} {X[0]:> 8f}')
+            num = 1 + X
+            denom = 1 - X
+            for ll in range(n_messages):
+                if num[ll] == 0:
+                    Lr[i, j, ll] = -1
+                elif denom[ll] == 0:
+                    Lr[i, j, ll] = 1
+                else:
+                    Lr[i, j, ll] = np.log(num[ll] / denom[ll])
+    # step 2 : Vertical
+
+    for j in range(n):
+        ff = nodes_hist[j]
+        mj = nodes_values[bits_counter: nodes_counter + ff]
+        nodes_counter += ff
+        for i in mj:
+            mji = mj[:]
+            Lq[i, j] = Lc[j]
+
+            for kk in range(len(mji)):
+                if mji[kk] != i:
+                    Lq[i, j] += Lr[mji[kk], j]
+
+    # LLR a posteriori:
+    L_posteriori = np.zeros((n, n_messages))
+    nodes_counter = 0
+    for j in range(n):
+        ff = nodes_hist[j]
+        mj = nodes_values[bits_counter: nodes_counter + ff]
+        nodes_counter += ff
+        L_posteriori[j] = Lc[j] + Lr[mj, j].sum(axis=0)
+
+    return Lq, Lr, L_posteriori
+
+
+def get_message(tG, x):
+    """Compute the original `n_bits` message from a `n_code` codeword `x`.
+
+    Parameters
+    ----------
+    tG: array (n_code, n_bits) coding matrix tG.
+    x: array (n_code,) decoded codeword of length `n_code`.
+
+    Returns
+    -------
+    message: array (n_bits,). Original binary message.
+
+    """
+    n, k = tG.shape
+
+    rtG, rx = utils.gausselimination(tG, x)
+
+    message = np.zeros(k).astype(int)
+
+    message[k - 1] = rx[k - 1]
+    for i in reversed(range(k - 1)):
+        message[i] = rx[i]
+        message[i] -= utils.binaryproduct(rtG[i, list(range(i+1, k))],
+                                          message[list(range(i+1, k))])
+
+    return abs(message)
diff --git a/controller/fw/src/test_pyldpc_utils.py b/controller/fw/src/test_pyldpc_utils.py
new file mode 100644
index 0000000..6b14532
--- /dev/null
+++ b/controller/fw/src/test_pyldpc_utils.py
@@ -0,0 +1,182 @@
+"""Conversion tools."""
+import math
+import numbers
+import numpy as np
+import scipy
+from scipy.stats import norm
+pi = math.pi
+
+
+def int2bitarray(n, k):
+    """Change an array's base from int (base 10) to binary (base 2)."""
+    binary_string = bin(n)
+    length = len(binary_string)
+    bitarray = np.zeros(k, 'int')
+    for i in range(length - 2):
+        bitarray[k - i - 1] = int(binary_string[length - i - 1])
+
+    return bitarray
+
+
+def bitarray2int(bitarray):
+    """Change array's base from binary (base 2) to int (base 10)."""
+    bitstring = "".join([str(i) for i in bitarray])
+
+    return int(bitstring, 2)
+
+
+def binaryproduct(X, Y):
+    """Compute a matrix-matrix / vector product in Z/2Z."""
+    A = X.dot(Y)
+    try:
+        A = A.toarray()
+    except AttributeError:
+        pass
+    return A % 2
+
+
+def gaussjordan(X, change=0):
+    """Compute the binary row reduced echelon form of X.
+
+    Parameters
+    ----------
+    X: array (m, n)
+    change : boolean (default, False). If True returns the inverse transform
+
+    Returns
+    -------
+    if `change` == 'True':
+        A: array (m, n). row reduced form of X.
+        P: tranformations applied to the identity
+    else:
+        A: array (m, n). row reduced form of X.
+
+    """
+    A = np.copy(X)
+    m, n = A.shape
+
+    if change:
+        P = np.identity(m).astype(int)
+
+    pivot_old = -1
+    for j in range(n):
+        filtre_down = A[pivot_old+1:m, j]
+        pivot = np.argmax(filtre_down)+pivot_old+1
+
+        if A[pivot, j]:
+            pivot_old += 1
+            if pivot_old != pivot:
+                aux = np.copy(A[pivot, :])
+                A[pivot, :] = A[pivot_old, :]
+                A[pivot_old, :] = aux
+                if change:
+                    aux = np.copy(P[pivot, :])
+                    P[pivot, :] = P[pivot_old, :]
+                    P[pivot_old, :] = aux
+
+            for i in range(m):
+                if i != pivot_old and A[i, j]:
+                    if change:
+                        P[i, :] = abs(P[i, :]-P[pivot_old, :])
+                    A[i, :] = abs(A[i, :]-A[pivot_old, :])
+
+        if pivot_old == m-1:
+            break
+
+    if change:
+        return A, P
+    return A
+
+
+def binaryrank(X):
+    """Compute rank of a binary Matrix using Gauss-Jordan algorithm."""
+    A = np.copy(X)
+    m, n = A.shape
+
+    A = gaussjordan(A)
+
+    return sum([a.any() for a in A])
+
+
+def f1(y, sigma):
+    """Compute normal density N(1,sigma)."""
+    f = norm.pdf(y, loc=1, scale=sigma)
+    return f
+
+
+def fm1(y, sigma):
+    """Compute normal density N(-1,sigma)."""
+
+    f = norm.pdf(y, loc=-1, scale=sigma)
+    return f
+
+
+def bitsandnodes(H):
+    """Return bits and nodes of a parity-check matrix H."""
+    if type(H) != scipy.sparse.csr_matrix:
+        bits_indices, bits = np.where(H)
+        nodes_indices, nodes = np.where(H.T)
+    else:
+        bits_indices, bits = scipy.sparse.find(H)[:2]
+        nodes_indices, nodes = scipy.sparse.find(H.T)[:2]
+    bits_histogram = np.bincount(bits_indices)
+    nodes_histogram = np.bincount(nodes_indices)
+
+    return bits_histogram, bits, nodes_histogram, nodes
+
+
+def incode(H, x):
+    """Compute Binary Product of H and x."""
+    return (binaryproduct(H, x) == 0).all()
+
+
+def gausselimination(A, b):
+    """Solve linear system in Z/2Z via Gauss Gauss elimination."""
+    if type(A) == scipy.sparse.csr_matrix:
+        A = A.toarray().copy()
+    else:
+        A = A.copy()
+    b = b.copy()
+    n, k = A.shape
+
+    for j in range(min(k, n)):
+        listedepivots = [i for i in range(j, n) if A[i, j]]
+        if len(listedepivots):
+            pivot = np.min(listedepivots)
+        else:
+            continue
+        if pivot != j:
+            aux = (A[j, :]).copy()
+            A[j, :] = A[pivot, :]
+            A[pivot, :] = aux
+
+            aux = b[j].copy()
+            b[j] = b[pivot]
+            b[pivot] = aux
+
+        for i in range(j+1, n):
+            if A[i, j]:
+                A[i, :] = abs(A[i, :]-A[j, :])
+                b[i] = abs(b[i]-b[j])
+
+    return A, b
+
+
+def check_random_state(seed):
+    """Turn seed into a np.random.RandomState instance
+    Parameters
+    ----------
+    seed : None | int | instance of RandomState
+        If seed is None, return the RandomState singleton used by np.random.
+        If seed is an int, return a new RandomState instance seeded with seed.
+        If seed is already a RandomState instance, return it.
+        Otherwise raise ValueError.
+    """
+    if seed is None or seed is np.random:
+        return np.random.mtrand._rand
+    if isinstance(seed, numbers.Integral):
+        return np.random.RandomState(seed)
+    if isinstance(seed, np.random.RandomState):
+        return seed
+    raise ValueError('%r cannot be used to seed a numpy.random.RandomState'
+                     ' instance' % seed)
-- 
cgit