fw: add working reed-solomon encoder/decoder

14 files changed, 252 insertions, 465 deletions

diff --git a/controller/fw/Makefile b/controller/fw/Makefile
index ab9d66c..b6fa43f 100644
--- a/controller/fw/Makefile
+++ b/controller/fw/Makefile
@@ -9,6 +9,7 @@ MSPDEBUG_DIR 	?= mspdebug
 LIBSODIUM_DIR	?= libsodium
 TINYAES_DIR		?= tinyaes
 MUSL_DIR		?= musl
+RSLIB_DIR		?= reed_solomon
 
 ########################################################################################################################
 # Algorithm parameters
@@ -56,34 +57,38 @@ C_SOURCES		+= tinyprintf/tinyprintf.c
 
 C_SOURCES		+= $(MSPDEBUG_DIR)/drivers/jtaglib.c
 
-CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_rfft_fast_init_f32.c
-CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_rfft_fast_f32.c
-CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_cfft_init_f32.c
-CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_cfft_f32.c
-CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix8_f32.c
-CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/CommonTables/arm_const_structs.c
-CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/CommonTables/arm_common_tables.c
-CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_bitreversal.c
-CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_bitreversal2.c
-
-MUSL_SOURCES		+= $(MUSL_DIR)/src/math/tanhf.c $(MUSL_DIR)/src/math/atanhf.c
-MUSL_SOURCES		+= $(MUSL_DIR)/src/math/expm1f.c $(MUSL_DIR)/src/math/log1pf.c
-MUSL_SOURCES		+= $(MUSL_DIR)/src/math/expf.c $(MUSL_DIR)/src/math/exp2f_data.c
-MUSL_SOURCES		+= $(MUSL_DIR)/src/math/powf.c
-MUSL_SOURCES		+= $(MUSL_DIR)/src/math/sqrtf.c
-MUSL_SOURCES		+= $(MUSL_DIR)/src/math/fabsf.c
-MUSL_SOURCES		+= $(MUSL_DIR)/src/stdlib/abs.c
-MUSL_SOURCES		+= $(MUSL_DIR)/src/string/memset.c
-MUSL_SOURCES		+= $(MUSL_DIR)/src/string/memcpy.c
-MUSL_SOURCES		+= $(MUSL_DIR)/src/math/__math_oflowf.c
-MUSL_SOURCES		+= $(MUSL_DIR)/src/math/__math_uflowf.c
-MUSL_SOURCES		+= $(MUSL_DIR)/src/math/__math_xflowf.c
-MUSL_SOURCES		+= $(MUSL_DIR)/src/math/__math_invalidf.c
-MUSL_SOURCES		+= $(MUSL_DIR)/src/math/powf_data.c
-
-C_SOURCES += $(CMSIS_SOURCES) $(MUSL_SOURCES)
-
-CXX_SOURCES		+= src/ldpc_wrapper.cpp
+CMSIS_SOURCES		+= TransformFunctions/arm_rfft_fast_init_f32.c
+CMSIS_SOURCES		+= TransformFunctions/arm_rfft_fast_f32.c
+CMSIS_SOURCES		+= TransformFunctions/arm_cfft_init_f32.c
+CMSIS_SOURCES		+= TransformFunctions/arm_cfft_f32.c
+CMSIS_SOURCES		+= TransformFunctions/arm_cfft_radix8_f32.c
+CMSIS_SOURCES		+= CommonTables/arm_const_structs.c
+CMSIS_SOURCES		+= CommonTables/arm_common_tables.c
+CMSIS_SOURCES		+= TransformFunctions/arm_bitreversal.c
+CMSIS_SOURCES		+= TransformFunctions/arm_bitreversal2.c
+CMSIS_SOURCES		:= $(addprefix $(CMSIS_DIR)/CMSIS/DSP/Source/,$(CMSIS_SOURCES))
+
+MUSL_SOURCES		+= math/tanhf.c math/atanhf.c
+MUSL_SOURCES		+= math/expm1f.c math/log1pf.c
+MUSL_SOURCES		+= math/expf.c math/exp2f_data.c
+MUSL_SOURCES		+= math/powf.c
+MUSL_SOURCES		+= math/sqrtf.c
+MUSL_SOURCES		+= math/fabsf.c
+MUSL_SOURCES		+= stdlib/abs.c
+MUSL_SOURCES		+= string/memset.c
+MUSL_SOURCES		+= string/memcpy.c
+MUSL_SOURCES		+= math/__math_oflowf.c
+MUSL_SOURCES		+= math/__math_uflowf.c
+MUSL_SOURCES		+= math/__math_xflowf.c
+MUSL_SOURCES		+= math/__math_invalidf.c
+MUSL_SOURCES		+= math/powf_data.c
+MUSL_SOURCES		:= $(addprefix $(MUSL_DIR)/src/,$(MUSL_SOURCES))
+
+RSLIB_SOURCES		+= $(addprefix $(RSLIB_DIR)/src/,rs.c ecc.c berlekamp.c galois.c)
+
+C_SOURCES += $(CMSIS_SOURCES) $(MUSL_SOURCES) $(RSLIB_SOURCES)
+
+CXX_SOURCES			+=
 
 BUILDDIR 	?= build
 BINARY 		:= safetyreset.elf
@@ -299,8 +304,8 @@ $(BUILDDIR)/tinyaes/aes.o:
 # 	mkdir -p $(BUILDDIR)/musl
 # 	cd $(BUILDDIR)/musl && CFLAGS="$(SIM_CFLAGS) $(COMMON_CFLAGS)" CC=$(CC) LD=$(LD) AR=$(AR) $(MUSL_DIR_ABS)/configure && $(MAKE) TARGET=arm-linux-musleabihf GCC_CONFIG="-mcpu=cortex-m4 -mfloat-abi=soft" -j $(shell nproc)
 
-build/ldpc_decoder_test.so: src/ldpc_decoder.c
-	gcc -fPIC -shared -Wall -Wextra -Wpedantic -std=gnu11 -O0 -g -o $@ $^
+build/rslib.so: $(RSLIB_SOURCES) src/rslib.c
+	gcc -fPIC -shared -Wall -Wextra -Wpedantic -std=gnu11 -O0 -g -o $@ -Isrc -I$(RSLIB_DIR)/src $^
 
 clean:
 	rm -rf $(BUILDDIR)/src
diff --git a/controller/fw/reed_solomon b/controller/fw/reed_solomon
new file mode 160000
+Subproject 63ff1482d42547422896418d5990a66a80e88d3
diff --git a/controller/fw/src/gpio_helpers.c b/controller/fw/src/gpio_helpers.c
new file mode 100644
index 0000000..07b9a33
--- /dev/null
+++ b/controller/fw/src/gpio_helpers.c
@@ -0,0 +1,46 @@
+
+#include "gpio_helpers.h"
+
+void gpio_pin_mode(GPIO_TypeDef *gpio, int pin, int mode) {
+    gpio->MODER &= ~(3 << (2*pin));
+    gpio->MODER |= mode << (2*pin);
+}
+
+void gpio_pin_setup(GPIO_TypeDef *gpio, int pin, int mode, int speed, int pullups, int afsel) {
+    int gpio_idx = ((uint32_t)gpio>>10) & 0xf;
+    RCC->AHB1ENR |= 1<<gpio_idx;
+    
+    gpio->MODER &= ~(3 << (2*pin));
+    gpio->MODER |= mode << (2*pin);
+    gpio->OSPEEDR &= ~(3 << (2*pin));
+    gpio->OSPEEDR |= speed << (2*pin);
+    gpio->PUPDR &= ~(3 << (2*pin));
+    gpio->PUPDR |= pullups << (2*pin);
+    gpio->AFR[pin>>3] &= ~(0xf << (4*(pin&7)));
+    gpio->AFR[pin>>3] |= afsel << (4*(pin&7));
+    gpio->BSRR = 1<<pin<<16;
+}
+
+void gpio_pin_output(GPIO_TypeDef *gpio, int pin, int speed) {
+    gpio_pin_setup(gpio, pin, 1, speed, 0, 0);
+}
+
+void gpio_pin_tristate(GPIO_TypeDef *gpio, int pin, int tristate) {
+    if (tristate)
+        gpio->MODER &= ~(3 << (2*pin));
+    else
+        gpio->MODER |= 1 << (2*pin);
+}
+
+void gpio_pin_input(GPIO_TypeDef *gpio, int pin, int pullups) {
+    gpio_pin_setup(gpio, pin, 0, 0, pullups, 0);
+}
+
+void gpio_pin_af(GPIO_TypeDef *gpio, int pin, int speed, int pullups, int afsel) {
+    gpio_pin_setup(gpio, pin, 2, speed, pullups, afsel);
+}
+
+void gpio_pin_analog(GPIO_TypeDef *gpio, int pin) {
+    gpio_pin_setup(gpio, pin, 3, 0, 0, 0);
+}
+
diff --git a/controller/fw/src/gpio_helpers.h b/controller/fw/src/gpio_helpers.h
new file mode 100644
index 0000000..abe2e85
--- /dev/null
+++ b/controller/fw/src/gpio_helpers.h
@@ -0,0 +1,14 @@
+#ifndef __GPIO_HELPERS_H__
+#define __GPIO_HELPERS_H__
+
+#include <stm32f407xx.h>
+
+void gpio_pin_mode(GPIO_TypeDef *gpio, int pin, int mode);
+void gpio_pin_setup(GPIO_TypeDef *gpio, int pin, int mode, int speed, int pullups, int afsel);
+void gpio_pin_output(GPIO_TypeDef *gpio, int pin, int speed);
+void gpio_pin_input(GPIO_TypeDef *gpio, int pin, int pullups);
+void gpio_pin_af(GPIO_TypeDef *gpio, int pin, int speed, int pullups, int afsel);
+void gpio_pin_analog(GPIO_TypeDef *gpio, int pin);
+void gpio_pin_tristate(GPIO_TypeDef *gpio, int pin, int tristate);
+
+#endif /* __GPIO_HELPERS_H__ */
diff --git a/controller/fw/src/ldpc_decoder.c b/controller/fw/src/ldpc_decoder.c
deleted file mode 100644
index fe59d77..0000000
--- a/controller/fw/src/ldpc_decoder.c
+++ /dev/null
@@ -1,267 +0,0 @@
-
-#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
deleted file mode 100644
index 3b91bba..0000000
--- a/controller/fw/src/ldpc_decoder_test.py
+++ /dev/null
@@ -1,72 +0,0 @@
-
-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/ldpc_wrapper.cpp b/controller/fw/src/ldpc_wrapper.cpp
deleted file mode 100644
index da63343..0000000
--- a/controller/fw/src/ldpc_wrapper.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-
-#include <stdlib.h>
-#include <algorithm>
-#include <cstdint>
-#include <cmath>
-
-#include "ldpc/generic.hh"
-#include "ldpc/layered_decoder.hh"
-
-static const int DEFAULT_TRIALS = 25;
-
-struct DVB_S2_TABLE_C9
-{
-	static const int M = 360;
-	static const int N = 16200;
-	static const int K = 13320;
-	static const int LINKS_MIN_CN = 15;
-	static const int LINKS_MAX_CN = 19;
-	static const int LINKS_TOTAL = 49319;
-	static const int DEG_MAX = 13;
-	static const int DEG[];
-	static const int LEN[];
-	static const int POS[];
-};
-
-const int DVB_S2_TABLE_C9::DEG[] = {
-    13, 3, 0
-};
-
-const int DVB_S2_TABLE_C9::LEN[] = {
-    1, 36, 0
-};
-
-const int DVB_S2_TABLE_C9::POS[] = {
-    3,	2409,	499,	1481,	908,	559,	716,	1270,	333,	2508,	2264,	1702,	2805,
-    4,	2447,	1926,
-    5,	414,	1224,
-    6,	2114,	842,
-    7,	212,	573,
-    0,	2383,	2112,
-    1,	2286,	2348,
-    2,	545,	819,
-    3,	1264,	143,
-    4,	1701,	2258,
-    5,	964,	166,
-    6,	114,	2413,
-    7,	2243,	81,
-    0,	1245,	1581,
-    1,	775,	169,
-    2,	1696,	1104,
-    3,	1914,	2831,
-    4,	532,	1450,
-    5,	91,	974,
-    6,	497,	2228,
-    7,	2326,	1579,
-    0,	2482,	256,
-    1,	1117,	1261,
-    2,	1257,	1658,
-    3,	1478,	1225,
-    4,	2511,	980,
-    5,	2320,	2675,
-    6,	435,	1278,
-    7,	228,	503,
-    0,	1885,	2369,
-    1,	57,	483,
-    2,	838,	1050,
-    3,	1231,	1990,
-    4,	1738,	68,
-    5,	2392,	951,
-    6,	163,	645,
-    7,	2644,	1704,
-};
-
-extern "C" {
-
-    int ldpc_decode(float *symbols, int trials) {
-        if (trials < 0)
-            trials = DEFAULT_TRIALS;
-
-        LDPCDecoder<float, SumProductAlgorithm<float, SelfCorrectedUpdate<float>>, LDPC<DVB_S2_TABLE_C9>> decoder;
-        return decoder.run(symbols, symbols+decoder.K, trials, 1);
-    }
-
-}
diff --git a/controller/fw/src/main.c b/controller/fw/src/main.c
index 6e117f6..6547c38 100644
--- a/controller/fw/src/main.c
+++ b/controller/fw/src/main.c
@@ -190,9 +190,11 @@ static struct jtag_img_descriptor {
     .img_len = 0x004000,
 };
 
-const char fw_dump[0x4000] = {
+char fw_dump[0x4000] = {
 #include "EasyMeter_Q3DA1002_V3.03_fw_dump_0xc000.h"
 };
+const int fw_dump_offx = 0xc000;
+const int row2_offx = 0xf438 - fw_dump_offx; 
 
 ssize_t jt_spi_flash_read_block(void *usr, int addr, size_t len, uint8_t *out) {
     /*
@@ -201,11 +203,25 @@ ssize_t jt_spi_flash_read_block(void *usr, int addr, size_t len, uint8_t *out) {
     */
 
     for (size_t i=0; i<len; i++)
-        out[i] = fw_dump[addr - 0xc000 + i];
+        out[i] = fw_dump[addr - fw_dump_offx + i];
 
     return len;
 }
 
+void update_image_flash_counter(void) {
+    static int flash_counter = 0;
+    flash_counter ++;
+    fw_dump[row2_offx + 0] = flash_counter/10000 + '0';
+    flash_counter %= 10000;
+    fw_dump[row2_offx + 1] = flash_counter/1000 + '0';
+    flash_counter %= 1000;
+    fw_dump[row2_offx + 2] = flash_counter/100 + '0';
+    flash_counter %= 100;
+    fw_dump[row2_offx + 3] = flash_counter/10 + '0';
+    flash_counter %= 10;
+    fw_dump[row2_offx + 4] = flash_counter + '0';
+}
+
 static unsigned int measurement_errors = 0;
 static struct dsss_demod_state demod_state;
 static uint32_t freq_sample_ts = 0;
@@ -248,16 +264,18 @@ int main(void)
     dsss_demod_init(&demod_state);
 
     con_printf("Booted.\r\n");
-    int i=0;
-    while (23) {
+    con_printf("Attempting to flash meter...\r\n");
+    int flash_tries = 0;
+    while (flash_tries++ < 25) {
         mspd_jtag_init();
-        int rv = mspd_jtag_flash_and_reset(jtag_img.devmem_img_start, jtag_img.img_len, jt_spi_flash_read_block, &jtag_img);
-        con_printf("%d flash result: %d\r\n", i, rv);
-        while (!rv) {}
+        if (!mspd_jtag_flash_and_reset(jtag_img.devmem_img_start, jtag_img.img_len, jt_spi_flash_read_block, &jtag_img))
+            break;
         for (int j=0; j<168*1000*5; j++)
             asm volatile ("nop");
-        i++;
     }
+    if (flash_tries == 25)
+        con_printf("Giving up.\r\n");
+
     while (23) {
         if (adc_fft_buf_ready_idx != -1) {
             for (int j=0; j<168*1000*2; j++)
diff --git a/controller/fw/src/mspdebug_wrapper.c b/controller/fw/src/mspdebug_wrapper.c
index aaf75ab..c30864c 100644
--- a/controller/fw/src/mspdebug_wrapper.c
+++ b/controller/fw/src/mspdebug_wrapper.c
@@ -125,10 +125,7 @@ int mspd_jtag_flash_and_reset(size_t img_start, size_t img_len, ssize_t (*read_b
 			return -EPIPE;
 	}
 
-	/* Verify flash */
-	/* FIXME
-	word = jtag_read_mem(NULL, 16, addr+index );
-	*/
+    /* TODO: Verify flash here. */
 
 	/* Execute power on reset */
 	jtag_execute_puc(&sr_jtdev);
diff --git a/controller/fw/src/protocol.c b/controller/fw/src/protocol.c
index bbee5ef..4740917 100644
--- a/controller/fw/src/protocol.c
+++ b/controller/fw/src/protocol.c
@@ -10,4 +10,6 @@ void handle_dsss_received(uint8_t data[static TRANSMISSION_SYMBOLS]) {
         con_printf("%3d ", x);
     }
     con_printf("\r\n");
+
+    update_image_flash_counter();
 }
diff --git a/controller/fw/src/rscode-config.h b/controller/fw/src/rscode-config.h
new file mode 100644
index 0000000..922aca9
--- /dev/null
+++ b/controller/fw/src/rscode-config.h
@@ -0,0 +1,9 @@
+/* Config header for reed-solomon library */
+
+#ifndef __RSCODE_CONFIG_H__
+#define __RSCODE_CONFIG_H__
+
+#define NPAR 4
+#define NBITS 6
+
+#endif /* __RSCODE_CONFIG_H__ */
diff --git a/controller/fw/src/rslib.c b/controller/fw/src/rslib.c
new file mode 100644
index 0000000..ce54a6f
--- /dev/null
+++ b/controller/fw/src/rslib.c
@@ -0,0 +1,29 @@
+#include <stdint.h>
+#include <string.h>
+
+#include "rscode-config.h"
+#include <ecc.h>
+
+#include "rslib.h"
+
+void rslib_encode(int nbits, size_t msglen, char msg[static msglen], char out[msglen + NPAR]) {
+    struct rscode_driver driver;
+    rscode_init(&driver, nbits);
+    rscode_encode(&driver, (unsigned char *)msg, msglen, (unsigned char *)out);
+}
+
+int rslib_decode(int nbits, size_t msglen, char msg_inout[static msglen]) {
+    struct rscode_driver driver;
+    rscode_init(&driver, nbits);
+    return rscode_decode(&driver, (unsigned char *)msg_inout, msglen);
+}
+
+int rslib_gexp(int z, int nbits) {
+    struct rscode_driver driver;
+    rscode_init(&driver, nbits);
+    return gexp(&driver, z);
+}
+
+size_t rslib_npar() {
+    return NPAR;
+}
diff --git a/controller/fw/src/rslib.h b/controller/fw/src/rslib.h
new file mode 100644
index 0000000..3ef6d19
--- /dev/null
+++ b/controller/fw/src/rslib.h
@@ -0,0 +1,9 @@
+#ifndef __RSLIB_H__
+#define __RSLIB_H__
+
+void rslib_encode(int nbits, size_t msglen, char msg[static msglen], char out[msglen + NPAR]);
+int rslib_decode(int nbits, size_t msglen, char msg_inout[static msglen]);
+int rslib_gexp(int z, int nbits);
+size_t rslib_npar();
+
+#endif /* __RSLIB_H__ */
diff --git a/controller/fw/tools/reed_solomon.py b/controller/fw/tools/reed_solomon.py
new file mode 100644
index 0000000..9eee6be
--- /dev/null
+++ b/controller/fw/tools/reed_solomon.py
@@ -0,0 +1,81 @@
+import os, sys
+import ctypes as C
+import argparse
+import binascii
+import numpy as np
+import timeit
+import statistics
+
+lib = C.CDLL('rslib.so')
+
+lib.rslib_encode.argtypes = [C.c_int, C.c_size_t, C.POINTER(C.c_char), C.POINTER(C.c_char)]
+lib.rslib_decode.argtypes = [C.c_int, C.c_size_t, C.POINTER(C.c_char)]
+lib.rslib_gexp.argtypes = [C.c_int, C.c_int]
+lib.rslib_gexp.restype = C.c_int
+lib.rslib_decode.restype = C.c_int
+lib.rslib_npar.restype = C.c_size_t
+
+def npar():
+    return lib.rslib_npar()
+
+def encode(data: bytes, nbits=8):
+    out = C.create_string_buffer(len(data) + lib.rslib_npar())
+    lib.rslib_encode(nbits, len(data), data, out)
+    return out.raw
+
+def decode(data: bytes, nbits=8):
+    inout = C.create_string_buffer(data)
+    lib.rslib_decode(nbits, len(data), inout)
+    return inout.raw[:-lib.rslib_npar() - 1]
+
+def cmdline_func_test(args, print=lambda *args, **kwargs: None, benchmark=False):
+    st = np.random.RandomState(seed=args.seed)
+
+    lfsr = [lib.rslib_gexp(i, args.bits) for i in range(2**args.bits - 1)]
+    print('LFSR', len(set(lfsr)), lfsr)
+    assert all(0 < x < 2**args.bits for x in lfsr)
+    assert len(set(lfsr)) == 2**args.bits - 1
+
+    print('Seed', args.seed)
+    for i in range(args.repeat):
+        print(f'Run {i}')
+        test_data = bytes(st.randint(2**args.bits, size=args.message_length, dtype=np.uint8))
+        print('        Raw:', binascii.hexlify(test_data).decode())
+        encoded = encode(test_data, nbits=args.bits)
+        print('     Encoded:', binascii.hexlify(encoded).decode())
+
+        indices = st.permutation(len(encoded))
+        encoded = list(encoded)
+        for pos in indices[:args.errors]:
+            encoded[pos] = st.randint(2**args.bits)
+        encoded = bytes(encoded)
+        print('    Modified:', ''.join(f'\033[91m{b:02x}\033[0m' if pos in indices[:args.errors] else f'{b:02x}' for pos, b in enumerate(encoded)))
+
+        if benchmark:
+            rpt = 10000
+            delta = timeit.timeit('decode(encoded, nbits=args.bits)',
+                    globals={'args': args, 'decode': decode, 'encoded': encoded},
+                    number=rpt)/rpt
+            print(f'Decoding runtime: {delta*1e6:.3f}μs')
+        decoded = decode(encoded, nbits=args.bits)
+        print('     Decoded:', binascii.hexlify(decoded).decode())
+        print('       Delta:', binascii.hexlify(
+                bytes(x^y for x, y in zip(test_data, decoded))
+            ).decode().replace('0', '.'))
+        assert test_data == decoded
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+    cmd_parser = parser.add_subparsers(required=True)
+    test_parser = cmd_parser.add_parser('test', help='Test reed-solomon implementation')
+    test_parser.add_argument('-m', '--message-length', type=int, default=6, help='Test message (plaintext) length in bytes')
+    test_parser.add_argument('-e', '--errors', type=int, default=2, help='Number of byte errors to insert into simulation')
+    test_parser.add_argument('-r', '--repeat', type=int, default=1000, help='Repeat experiment -r times')
+    test_parser.add_argument('-b', '--bits', type=int, default=8, help='Symbol bit size')
+    test_parser.add_argument('-s', '--seed', type=int, default=0, help='Random seed')
+    test_parser.set_defaults(func=cmdline_func_test)
+    enc_parser = cmd_parser.add_parser('encode', help='RS-Encode given hex string')
+    enc_parser.add_argument('hex_str')
+    args = parser.parse_args()
+    args.func(args, print=print)
+