add SampleBuffer and ErrorManager; WAV testing

10 files changed, 476 insertions, 248 deletions

diff --git a/Makefile b/Makefile
index 95a3b7b..33e1cf0 100644
--- a/Makefile
+++ b/Makefile
@@ -117,8 +117,8 @@ include $(CHIBIOS)/tools/mk/autobuild.mk
 #include $(CHIBIOS)/test/oslib/oslib_test.mk

 

 # Define linker script file here.

-#LDSCRIPT= $(STARTUPLD)/STM32L476xG.ld

-LDSCRIPT= STM32L432xC_stmdsp.ld

+LDSCRIPT= STM32L476xG_stmdsp.ld

+#LDSCRIPT= STM32L432xC_stmdsp.ld

 

 # C sources that can be compiled in ARM or THUMB mode depending on the global

 # setting.

diff --git a/STM32L476xG_stmdsp.ld b/STM32L476xG_stmdsp.ld
new file mode 100644
index 0000000..f54ab51
--- /dev/null
+++ b/STM32L476xG_stmdsp.ld
@@ -0,0 +1,85 @@
+/*

+    ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio

+

+    Licensed under the Apache License, Version 2.0 (the "License");

+    you may not use this file except in compliance with the License.

+    You may obtain a copy of the License at

+

+        http://www.apache.org/licenses/LICENSE-2.0

+

+    Unless required by applicable law or agreed to in writing, software

+    distributed under the License is distributed on an "AS IS" BASIS,

+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

+    See the License for the specific language governing permissions and

+    limitations under the License.

+*/

+

+/*

+ * STM32L476xG memory setup.

+ */

+MEMORY

+{

+    flash0 (rx) : org = 0x08000000, len = 1M

+    flash1 (rx) : org = 0x00000000, len = 0

+    flash2 (rx) : org = 0x00000000, len = 0

+    flash3 (rx) : org = 0x00000000, len = 0

+    flash4 (rx) : org = 0x00000000, len = 0

+    flash5 (rx) : org = 0x00000000, len = 0

+    flash6 (rx) : org = 0x00000000, len = 0

+    flash7 (rx) : org = 0x00000000, len = 0

+    ram0   (wx) : org = 0x20000000, len = 96k

+    ram1   (wx) : org = 0x00000000, len = 0

+    ram2   (wx) : org = 0x00000000, len = 0

+    ram3   (wx) : org = 0x00000000, len = 0

+    ram4   (wx) : org = 0x10000000, len = 0 /* Save 32k for ELF load */

+    ram5   (wx) : org = 0x00000000, len = 0

+    ram6   (wx) : org = 0x00000000, len = 0

+    ram7   (wx) : org = 0x00000000, len = 0

+}

+

+/* For each data/text section two region are defined, a virtual region

+   and a load region (_LMA suffix).*/

+

+/* Flash region to be used for exception vectors.*/

+REGION_ALIAS("VECTORS_FLASH", flash0);

+REGION_ALIAS("VECTORS_FLASH_LMA", flash0);

+

+/* Flash region to be used for constructors and destructors.*/

+REGION_ALIAS("XTORS_FLASH", flash0);

+REGION_ALIAS("XTORS_FLASH_LMA", flash0);

+

+/* Flash region to be used for code text.*/

+REGION_ALIAS("TEXT_FLASH", flash0);

+REGION_ALIAS("TEXT_FLASH_LMA", flash0);

+

+/* Flash region to be used for read only data.*/

+REGION_ALIAS("RODATA_FLASH", flash0);

+REGION_ALIAS("RODATA_FLASH_LMA", flash0);

+

+/* Flash region to be used for various.*/

+REGION_ALIAS("VARIOUS_FLASH", flash0);

+REGION_ALIAS("VARIOUS_FLASH_LMA", flash0);

+

+/* Flash region to be used for RAM(n) initialization data.*/

+REGION_ALIAS("RAM_INIT_FLASH_LMA", flash0);

+

+/* RAM region to be used for Main stack. This stack accommodates the processing

+   of all exceptions and interrupts.*/

+REGION_ALIAS("MAIN_STACK_RAM", ram0);

+

+/* RAM region to be used for the process stack. This is the stack used by

+   the main() function.*/

+REGION_ALIAS("PROCESS_STACK_RAM", ram0);

+

+/* RAM region to be used for data segment.*/

+REGION_ALIAS("DATA_RAM", ram0);

+REGION_ALIAS("DATA_RAM_LMA", flash0);

+

+/* RAM region to be used for BSS segment.*/

+REGION_ALIAS("BSS_RAM", ram0);

+

+/* RAM region to be used for the default heap.*/

+REGION_ALIAS("HEAP_RAM", ram0);

+

+/* Generic rules inclusion.*/

+INCLUDE rules.ld

diff --git a/gui/stmdsp.cpp b/gui/stmdsp.cpp
index 9119284..897e643 100644
--- a/gui/stmdsp.cpp
+++ b/gui/stmdsp.cpp
@@ -36,24 +36,10 @@ namespace stmdsp
         }
     }
 
-    /*std::vector<adcsample_t> device::sample(unsigned long int count) {
-        if (connected()) {
-            uint8_t request[3] = {
-                'd',
-                static_cast<uint8_t>(count),
-                static_cast<uint8_t>(count >> 8)
-            };
-            m_serial.write(request, 3);
-            std::vector<adcsample_t> data (count);
-            m_serial.read(reinterpret_cast<uint8_t *>(data.data()), data.size() * sizeof(adcsample_t));
-            return data;
-        } else {
-            return {};
-        }
-    }*/
-
     void device::continuous_set_buffer_size(unsigned int size) {
         if (connected()) {
+            m_buffer_size = size;
+
             uint8_t request[3] = {
                 'B',
                 static_cast<uint8_t>(size),
diff --git a/gui/stmdsp.hpp b/gui/stmdsp.hpp
index 5dda89c..4551483 100644
--- a/gui/stmdsp.hpp
+++ b/gui/stmdsp.hpp
@@ -19,7 +19,7 @@
 
 namespace stmdsp
 {
-    constexpr unsigned int SAMPLES_MAX = 4000;
+    constexpr unsigned int SAMPLES_MAX = 3000;
 
     class scanner
     {
@@ -55,6 +55,7 @@ namespace stmdsp
         //std::vector<adcsample_t> sample(unsigned long int count = 1);
 
         void continuous_set_buffer_size(unsigned int size);
+        unsigned int get_buffer_size() const { return m_buffer_size; }
         void set_sample_rate(unsigned int id);
         unsigned int get_sample_rate();
         void continuous_start();
@@ -73,6 +74,7 @@ namespace stmdsp
 
     private:
         serial::Serial m_serial;
+        unsigned int m_buffer_size = SAMPLES_MAX;
     };
 }
 
diff --git a/gui/wav.hpp b/gui/wav.hpp
new file mode 100644
index 0000000..a87efb1
--- /dev/null
+++ b/gui/wav.hpp
@@ -0,0 +1,95 @@
+#ifndef WAV_HPP_
+#define WAV_HPP_
+
+#include <cstdint>
+#include <cstring>
+#include <fstream>
+
+namespace wav
+{
+    struct header {
+        char riff[4];      // "RIFF"
+        uint32_t filesize; // Total file size minus eight bytes
+        char wave[4];      // "WAVE"
+
+        bool valid() const {
+            return strncmp(riff, "RIFF", 4) == 0 && filesize > 8 && strncmp(wave, "WAVE", 4) == 0;
+        }
+    } __attribute__ ((packed));
+
+    struct format {
+        char fmt_[4]; // "fmt "
+        uint32_t size;
+        uint16_t type;
+        uint16_t channelcount;
+        uint32_t samplerate;
+        uint32_t byterate;
+        uint16_t unused;
+        uint16_t bps;
+
+        bool valid() const {
+            return strncmp(fmt_, "fmt ", 4) == 0;
+        }
+    } __attribute__ ((packed));
+
+    struct data {
+        char data[4]; // "data"
+        uint32_t size;
+
+        bool valid() const {
+            return strncmp(data, "data", 4) == 0;
+        }
+    } __attribute__ ((packed));
+
+    class clip {
+    public:
+        clip(const char *path) {
+            std::ifstream file (path);
+            if (!file.good())
+                return;
+            {
+                header h;
+                file.read(reinterpret_cast<char *>(&h), sizeof(header));
+                if (!h.valid())
+                    return;
+            }
+            {
+                format f;
+                file.read(reinterpret_cast<char *>(&f), sizeof(format));
+                if (!f.valid() || f.type != 1) // ensure PCM
+                    return;
+            }
+            {
+                wav::data d;
+                file.read(reinterpret_cast<char *>(&d), sizeof(wav::data));
+                if (!d.valid())
+                    return;
+                m_data = new char[d.size + 4096 - (d.size % 4096)];
+                m_size = d.size;
+                file.read(m_data, d.size);
+            }
+        }
+
+        bool valid() const {
+            return m_data != nullptr && m_size > 0;
+        }
+        auto data() const {
+            return m_data;
+        }
+        auto next(unsigned int chunksize = 3000) {
+            if (m_pos == m_size) {
+                m_pos = 0;
+            }
+            auto ret = m_data + m_pos;
+            m_pos = std::min(m_pos + chunksize, m_size);
+            return ret;
+        }
+    private:
+        char *m_data = nullptr;
+        uint32_t m_size = 0;
+        uint32_t m_pos = 0;
+    };
+}
+
+#endif // WAV_HPP_
+
diff --git a/gui/wxmain.cpp b/gui/wxmain.cpp
index 8f2e386..cbc1f8a 100644
--- a/gui/wxmain.cpp
+++ b/gui/wxmain.cpp
@@ -24,8 +24,67 @@
 #include <wx/statusbr.h>
 #include <wx/textdlg.h>
 
+#include <array>
 #include <vector>
 
+static const std::array<wxString, 6> srateValues {
+    "8 kS/s",
+    "16 kS/s",
+    "20 kS/s",
+    "32 kS/s",
+    "48 kS/s",
+    "96 kS/s"
+};
+static const std::array<unsigned int, 6> srateNums {
+    8000,
+    16000,
+    20000,
+    32000,
+    48000,
+    96000
+};
+
+static const char *makefile_text = R"make(
+all:
+	@arm-none-eabi-g++ -x c++ -Os -fno-exceptions -fno-rtti \
+	                   -mcpu=cortex-m4 -mthumb -mfloat-abi=hard -mfpu=fpv4-sp-d16 -mtune=cortex-m4 \
+	                   -nostartfiles \
+	                   -Wl,-Ttext-segment=0x10000000 -Wl,-zmax-page-size=512 -Wl,-eprocess_data_entry \
+	                   $0 -o $0.o
+	@cp $0.o $0.orig.o
+	@arm-none-eabi-strip -s -S --strip-unneeded $0.o
+	@arm-none-eabi-objcopy --remove-section .ARM.attributes \
+                           --remove-section .comment \
+                           --remove-section .noinit \
+                           $0.o
+	arm-none-eabi-size $0.o
+)make";
+
+static const char *file_header = R"cpp(
+#include <cstdint>
+
+using adcsample_t = uint16_t;
+constexpr unsigned int SIZE = $0;
+
+adcsample_t *process_data(adcsample_t *samples, unsigned int size);
+
+extern "C" void process_data_entry()
+{
+    ((void (*)())process_data)();
+}
+
+// End stmdspgui header code
+
+)cpp";
+
+static const char *file_content = 
+R"cpp(adcsample_t *process_data(adcsample_t *samples, unsigned int size)
+{
+    return samples;
+}
+)cpp";
+
+
 enum Id {
     MeasureTimer = 1,
 
@@ -78,17 +137,9 @@ MainFrame::MainFrame() : wxFrame(nullptr, wxID_ANY, "stmdspgui", wxPoint(50, 50)
     splitter->SetMinimumPaneSize(20);
 
     auto comp = new wxButton(panelToolbar, Id::MCodeCompile, "Compile");
-    static const wxString srateValues[] = {
-        "8 kS/s",
-        "16 kS/s",
-        "20 kS/s",
-        "32 kS/s",
-        "48 kS/s",
-        "96 kS/s"
-    };
     m_rate_select = new wxComboBox(panelToolbar, wxID_ANY,
                                    wxEmptyString, wxDefaultPosition, wxDefaultSize,
-                                   6, srateValues, wxCB_READONLY);
+                                   srateValues.size(), srateValues.data(), wxCB_READONLY);
     m_rate_select->Disable();
 
     sizerToolbar->Add(comp, 0, wxLEFT, 4);
@@ -182,11 +233,23 @@ void MainFrame::onMeasureTimer([[maybe_unused]] wxTimerEvent&)
     if (m_conv_result_log != nullptr) {
         if (auto samples = m_device->continuous_read(); samples.size() > 0) {
             for (auto& s : samples) {
-                auto str = wxString::Format("%u\n", s);
-                m_conv_result_log->Write(str.ToAscii(), str.Len());
+                auto str = std::to_string(s);
+                m_conv_result_log->Write(str.c_str(), str.size());
             }
         }
-    } else if (m_status_bar && m_run_measure && m_run_measure->IsChecked()) {
+    }
+
+    if (m_wav_clip != nullptr) {
+        auto size = m_device->get_buffer_size();
+        auto chunk = new stmdsp::adcsample_t[size];
+        auto src = reinterpret_cast<uint16_t *>(m_wav_clip->next(size));
+        for (unsigned int i = 0; i < size; i++)
+            chunk[i] = ((uint32_t)*src++) / 16 + 2048;
+        m_device->siggen_upload(chunk, size);
+        delete[] chunk;
+    }
+
+    if (m_status_bar && m_run_measure && m_run_measure->IsChecked()) {
         m_status_bar->SetStatusText(wxString::Format(wxT("Execution time: %u cycles"),
                                                      m_device->continuous_start_get_measurement()));
     }
@@ -230,46 +293,16 @@ void MainFrame::prepareEditor()
     onFileNew(dummy);
 }
 
-static const char *makefile_text = R"make(
-all:
-	@arm-none-eabi-g++ -x c++ -Os -fno-exceptions -fno-rtti \
-	                   -mcpu=cortex-m4 -mthumb -mfloat-abi=hard -mfpu=fpv4-sp-d16 -mtune=cortex-m4 \
-	                   -nostartfiles \
-	                   -Wl,-Ttext-segment=0x10000000 -Wl,-zmax-page-size=512 -Wl,-eprocess_data_entry \
-	                   $0 -o $0.o
-	@cp $0.o $0.orig.o
-	@arm-none-eabi-strip -s -S --strip-unneeded $0.o
-	@arm-none-eabi-objcopy --remove-section .ARM.attributes \
-                           --remove-section .comment \
-                           --remove-section .noinit \
-                           $0.o
-	arm-none-eabi-size $0.o
-)make";
-
-static wxString file_header (R"cpp(
-#include <cstdint>
-
-using adcsample_t = uint16_t;
-constexpr unsigned int SIZE = 3000;
-
-adcsample_t *process_data(adcsample_t *samples, unsigned int size);
-
-extern "C" void process_data_entry()
-{
-    ((void (*)())process_data)();
-}
-
-// End stmdspgui header code
-
-)cpp");
-
 wxString MainFrame::compileEditorCode()
 {
     if (m_temp_file_name.IsEmpty())
         m_temp_file_name = wxFileName::CreateTempFileName("stmdspgui");
 
     wxFile file (m_temp_file_name, wxFile::write);
-    file.Write(file_header + m_text_editor->GetText());
+    wxString file_text (file_header);
+    file_text.Replace("$0", std::to_string(m_device != nullptr ? m_device->get_buffer_size()
+                                                               : stmdsp::SAMPLES_MAX));
+    file.Write(wxString(file_text) + m_text_editor->GetText());
     file.Close();
 
     wxFile makefile (m_temp_file_name + "make", wxFile::write);
@@ -301,12 +334,7 @@ wxString MainFrame::compileEditorCode()
 void MainFrame::onFileNew([[maybe_unused]] wxCommandEvent&)
 {
     m_open_file_path = "";
-    m_text_editor->SetText(
-R"cpp(adcsample_t *process_data(adcsample_t *samples, unsigned int size)
-{
-    return samples;
-}
-)cpp");
+    m_text_editor->SetText(file_content);
     m_text_editor->DiscardEdits();
     m_status_bar->SetStatusText("Ready.");
 }
@@ -444,6 +472,10 @@ void MainFrame::onRunStart(wxCommandEvent& ce)
             if (m_run_measure && m_run_measure->IsChecked()) {
                 m_device->continuous_start_measure();
                 m_measure_timer->StartOnce(1000);
+            } else if (m_wav_clip != nullptr) {
+                m_device->continuous_start();
+                m_measure_timer->Start(m_device->get_buffer_size() * 500 / 
+                                       srateNums[m_rate_select->GetSelection()]);
             } else {
                 m_device->continuous_start();
                 m_measure_timer->Start(15);
@@ -458,6 +490,7 @@ void MainFrame::onRunStart(wxCommandEvent& ce)
         }
     } else {
         m_device->continuous_stop();
+        m_measure_timer->Stop();
 
         menuItem->SetItemLabel("&Start");
         m_status_bar->SetStatusText("Ready.");
@@ -536,32 +569,43 @@ void MainFrame::onRunGenUpload([[maybe_unused]] wxCommandEvent&)
                                         "between zero and 4095.", "Enter Generator Values");
         if (dialog.ShowModal() == wxID_OK) {
             if (wxString values = dialog.GetValue(); !values.IsEmpty()) {
-                std::vector<stmdsp::dacsample_t> samples;
-                while (!values.IsEmpty()) {
-                    if (auto number_end = values.find_first_not_of("0123456789");
-                        number_end != wxString::npos && number_end > 0)
-                    {
-                        auto number = values.Left(number_end);
-                        if (unsigned long n; number.ToULong(&n))
-                            samples.push_back(n & 4095);
-
-                        if (auto next = values.find_first_of("0123456789", number_end + 1);
-                            next != wxString::npos)
+                if (values[0] == '/') {
+                    m_wav_clip = new wav::clip(values.Mid(1));
+                    if (m_wav_clip->valid()) {
+                        m_status_bar->SetStatusText("Generator ready.");
+                    } else {
+                        delete m_wav_clip;
+                        m_wav_clip = nullptr;
+                        m_status_bar->SetStatusText("Error: Bad WAV file.");
+                    }
+                } else {
+                    std::vector<stmdsp::dacsample_t> samples;
+                    while (!values.IsEmpty()) {
+                        if (auto number_end = values.find_first_not_of("0123456789");
+                            number_end != wxString::npos && number_end > 0)
                         {
-                            values = values.Mid(next);
+                            auto number = values.Left(number_end);
+                            if (unsigned long n; number.ToULong(&n))
+                                samples.push_back(n & 4095);
+
+                            if (auto next = values.find_first_of("0123456789", number_end + 1);
+                                next != wxString::npos)
+                            {
+                                values = values.Mid(next);
+                            } else {
+                                break;
+                            }
                         } else {
                             break;
                         }
-                    } else {
-                        break;
                     }
-                }
 
-                if (samples.size() <= stmdsp::SAMPLES_MAX) {
-                    m_device->siggen_upload(&samples[0], samples.size());
-                    m_status_bar->SetStatusText("Generator ready.");
-                } else {
-                    m_status_bar->SetStatusText("Error: Too many samples.");
+                    if (samples.size() <= stmdsp::SAMPLES_MAX) {
+                        m_device->siggen_upload(&samples[0], samples.size());
+                        m_status_bar->SetStatusText("Generator ready.");
+                    } else {
+                        m_status_bar->SetStatusText("Error: Too many samples.");
+                    }
                 }
             } else {
                 m_status_bar->SetStatusText("Error: No samples given.");
diff --git a/gui/wxmain.hpp b/gui/wxmain.hpp
index ec30f1f..8068784 100644
--- a/gui/wxmain.hpp
+++ b/gui/wxmain.hpp
@@ -13,6 +13,7 @@
 #define WXMAIN_HPP_
 
 #include "stmdsp.hpp"
+#include "wav.hpp"
 
 #include <fstream>
 #include <future>
@@ -75,6 +76,7 @@ private:
     wxString m_temp_file_name;
 
     stmdsp::device *m_device = nullptr;
+    wav::clip *m_wav_clip = nullptr;
 
     bool tryDevice();
     void prepareEditor();
diff --git a/source/common.hpp b/source/common.hpp
new file mode 100644
index 0000000..1045b32
--- /dev/null
+++ b/source/common.hpp
@@ -0,0 +1,56 @@
+#include <array>
+#include <cstdint>
+
+constexpr unsigned int MAX_SAMPLE_BUFFER_SIZE = 6000;
+
+using Sample = uint16_t;
+
+class SampleBuffer
+{
+public:
+    void clear() {
+        m_buffer.fill(0);
+    }
+    void modify(Sample *data, unsigned int srcsize) {
+        auto size = srcsize < m_size ? srcsize : m_size;
+        std::copy(data, data + size, m_buffer.data());
+        m_modified = m_buffer.data();
+    }
+    void midmodify(Sample *data, unsigned int srcsize) {
+        auto size = srcsize < m_size / 2 ? srcsize : m_size / 2;
+        std::copy(data, data + size, middata());
+        m_modified = middata();
+    }
+
+    void setSize(unsigned int size) {
+        m_size = size < MAX_SAMPLE_BUFFER_SIZE ? size : MAX_SAMPLE_BUFFER_SIZE;
+    }
+
+    Sample *data() /*const*/ {
+        return m_buffer.data();
+    }
+    Sample *middata() /*const*/ {
+        return m_buffer.data() + m_size / 2;
+    }
+    uint8_t *bytedata() /*const*/ {
+        return reinterpret_cast<uint8_t *>(m_buffer.data());
+    }
+
+    Sample *modified() {
+        auto m = m_modified;
+        m_modified = nullptr;
+        return m;
+    }
+    unsigned int size() const {
+        return m_size;
+    }
+    unsigned int bytesize() const {
+        return m_size * sizeof(Sample);
+    }
+
+private:
+    std::array<Sample, MAX_SAMPLE_BUFFER_SIZE> m_buffer;
+    unsigned int m_size = MAX_SAMPLE_BUFFER_SIZE;
+    Sample *m_modified = nullptr;
+};
+
diff --git a/source/error.hpp b/source/error.hpp
new file mode 100644
index 0000000..699c746
--- /dev/null
+++ b/source/error.hpp
@@ -0,0 +1,38 @@
+#include <array>
+
+constexpr unsigned int MAX_ERROR_QUEUE_SIZE = 8;
+
+enum class Error : char
+{
+    None = 0,
+    BadParam,
+    BadParamSize,
+    BadUserCodeLoad,
+    BadUserCodeSize,
+    NotIdle,
+    ConversionAborted
+};
+
+class ErrorManager
+{
+public:
+    void add(Error error) {
+        if (m_index < m_queue.size())
+            m_queue[m_index++] = error;
+    }
+
+    bool assert(bool condition, Error error) {
+        if (!condition)
+            add(error);
+        return condition;
+    }
+
+    Error pop() {
+        return m_index == 0 ? Error::None : m_queue[--m_index];
+    }
+
+private:
+    std::array<Error, MAX_ERROR_QUEUE_SIZE> m_queue;
+    unsigned int m_index = 0;
+};
+
diff --git a/source/main.cpp b/source/main.cpp
index f635953..b77bd2f 100644
--- a/source/main.cpp
+++ b/source/main.cpp
@@ -12,6 +12,12 @@
 #include "ch.h"
 #include "hal.h"
 
+static_assert(sizeof(adcsample_t) == sizeof(uint16_t));
+static_assert(sizeof(dacsample_t) == sizeof(uint16_t));
+
+#include "common.hpp"
+#include "error.hpp"
+
 #include "adc.hpp"
 #include "dac.hpp"
 #include "elf_load.hpp"
@@ -20,45 +26,22 @@
 #include <array>
 
 constexpr unsigned int MAX_ELF_FILE_SIZE = 8 * 1024;
-constexpr unsigned int MAX_ERROR_QUEUE_SIZE = 8;
-constexpr unsigned int MAX_SAMPLE_BUFFER_SIZE = 6000; // operate on buffers size this / 2
-constexpr unsigned int MAX_SIGGEN_BUFFER_SIZE = MAX_SAMPLE_BUFFER_SIZE / 2;
 
 enum class RunStatus : char
 {
     Idle = '1',
     Running
 };
-enum class Error : char
-{
-    None = 0,
-    BadParam,
-    BadParamSize,
-    BadUserCodeLoad,
-    BadUserCodeSize,
-    NotIdle,
-    ConversionAborted
-};
-
 static RunStatus run_status = RunStatus::Idle;
-static Error error_queue[MAX_ERROR_QUEUE_SIZE];
-static unsigned int error_queue_index = 0;
-
-static void error_queue_add(Error error)
-{
-    if (error_queue_index < MAX_ERROR_QUEUE_SIZE)
-        error_queue[error_queue_index++] = error;
-}
-static Error error_queue_pop()
-{
-    return error_queue_index == 0 ? Error::None : error_queue[--error_queue_index];
-}
 
 #define MSG_CONVFIRST          (1)
 #define MSG_CONVSECOND         (2)
 #define MSG_CONVFIRST_MEASURE  (3)
 #define MSG_CONVSECOND_MEASURE (4)
 
+#define MSG_FOR_FIRST(m)   (m & 1)
+#define MSG_FOR_MEASURE(m) (m > 2)
+
 static msg_t conversionMBBuffer[4];
 static MAILBOX_DECL(conversionMB, conversionMBBuffer, 4);
 
@@ -67,22 +50,11 @@ static THD_FUNCTION(conversionThread, arg);
 
 static time_measurement_t conversion_time_measurement;
 
-static_assert(sizeof(adcsample_t) == sizeof(uint16_t));
-static_assert(sizeof(dacsample_t) == sizeof(uint16_t));
+static ErrorManager EM;
 
-#if CACHE_LINE_SIZE > 0
-CC_ALIGN(CACHE_LINE_SIZE)
-#endif
-static std::array<adcsample_t, CACHE_SIZE_ALIGN(adcsample_t, MAX_SAMPLE_BUFFER_SIZE)> adc_samples;
-#if CACHE_LINE_SIZE > 0
-CC_ALIGN(CACHE_LINE_SIZE)
-#endif
-static std::array<dacsample_t, CACHE_SIZE_ALIGN(dacsample_t, MAX_SAMPLE_BUFFER_SIZE)> dac_samples;
-static volatile const dacsample_t *dac_samples_new = nullptr;
-#if CACHE_LINE_SIZE > 0
-CC_ALIGN(CACHE_LINE_SIZE)
-#endif
-static std::array<dacsample_t, CACHE_SIZE_ALIGN(dacsample_t, MAX_SIGGEN_BUFFER_SIZE)> dac2_samples;
+static SampleBuffer samplesIn;
+static SampleBuffer samplesOut;
+static SampleBuffer samplesSigGen;
 
 static unsigned char elf_file_store[MAX_ELF_FILE_SIZE];
 static ELF::Entry elf_entry = nullptr;
@@ -117,10 +89,6 @@ int main()
     main_loop();
 }
 
-static unsigned int dac_sample_count = MAX_SAMPLE_BUFFER_SIZE;
-static unsigned int dac2_sample_count = MAX_SIGGEN_BUFFER_SIZE;
-static unsigned int adc_sample_count = MAX_SAMPLE_BUFFER_SIZE;
-
 void main_loop()
 {
 
@@ -132,71 +100,50 @@ void main_loop()
                 switch (cmd[0]) {
 
                 case 'a':
-                    USBSerial::write((uint8_t *)adc_samples.data(),
-                                     adc_sample_count * sizeof(adcsample_t));
+                    USBSerial::write(samplesIn.bytedata(), samplesIn.bytesize());
                     break;
                 case 'A':
-                    USBSerial::read((uint8_t *)&adc_samples[0],
-                                    adc_sample_count * sizeof(adcsample_t));
+                    USBSerial::read(samplesIn.bytedata(), samplesIn.bytesize());
                     break;
 
                 case 'B':
-                    if (run_status == RunStatus::Idle) {
-                        if (USBSerial::read(&cmd[1], 2) == 2) {
-                            unsigned int count = cmd[1] | (cmd[2] << 8);
-                            if (count <= MAX_SAMPLE_BUFFER_SIZE / 2) {
-                                adc_sample_count = count * 2;
-                                dac_sample_count = count * 2;
-                            } else {
-                                error_queue_add(Error::BadParam);
-                            }
-                        } else {
-                            error_queue_add(Error::BadParamSize);
+                    if (EM.assert(run_status == RunStatus::Idle, Error::NotIdle) &&
+                        EM.assert(USBSerial::read(&cmd[1], 2) == 2, Error::BadParamSize))
+                    {
+                        unsigned int count = (cmd[1] | (cmd[2] << 8)) * 2;
+                        if (EM.assert(count <= MAX_SAMPLE_BUFFER_SIZE, Error::BadParam)) {
+                            samplesIn.setSize(count);
+                            samplesOut.setSize(count);
                         }
-                    } else {
-                        error_queue_add(Error::NotIdle);
                     }
                     break;
 
                 case 'd':
-                    USBSerial::write((uint8_t *)dac_samples.data(),
-                                     dac_sample_count * sizeof(dacsample_t));
+                    USBSerial::write(samplesOut.bytedata(), samplesOut.bytesize());
                     break;
                 case 'D':
-                    if (USBSerial::read(&cmd[1], 2) == 2) {
+                    if (EM.assert(USBSerial::read(&cmd[1], 2) == 2, Error::BadParamSize)) {
                         unsigned int count = cmd[1] | (cmd[2] << 8);
-                        if (count <= MAX_SIGGEN_BUFFER_SIZE) {
-                            dac2_sample_count = count;
-                            USBSerial::read((uint8_t *)&dac2_samples[0],
-                                            dac2_sample_count * sizeof(dacsample_t));
-                        } else {
-                            error_queue_add(Error::BadParam);
+                        if (EM.assert(count <= MAX_SAMPLE_BUFFER_SIZE, Error::BadParam)) {
+                            samplesSigGen.setSize(count);
+                            USBSerial::read(samplesSigGen.bytedata(), samplesSigGen.bytesize());
                         }
-                    } else {
-                        error_queue_add(Error::BadParamSize);
                     }
                     break;
 
                 // 'E' - Reads in and loads the compiled conversion code binary from USB.
                 case 'E':
-                    if (run_status == RunStatus::Idle) {
-                        if (USBSerial::read(&cmd[1], 2) == 2) {
-                            // Only load the binary if it can fit in the memory reserved for it.
-                            unsigned int size = cmd[1] | (cmd[2] << 8);
-                            if (size < sizeof(elf_file_store)) {
-                                USBSerial::read(elf_file_store, size);
-                                elf_entry = ELF::load(elf_file_store);
-
-                                if (elf_entry == nullptr)
-                                    error_queue_add(Error::BadUserCodeLoad);
-                            } else {
-                                error_queue_add(Error::BadUserCodeSize);
-                            }
-                        } else {
-                            error_queue_add(Error::BadParamSize);
+                    if (EM.assert(run_status == RunStatus::Idle, Error::NotIdle) &&
+                        EM.assert(USBSerial::read(&cmd[1], 2) == 2, Error::BadParamSize))
+                    {
+                        // Only load the binary if it can fit in the memory reserved for it.
+                        unsigned int size = cmd[1] | (cmd[2] << 8);
+                        if (EM.assert(size < sizeof(elf_file_store), Error::BadUserCodeSize)) {
+                            USBSerial::read(elf_file_store, size);
+                            elf_entry = ELF::load(elf_file_store);
+
+                            EM.assert(elf_entry != nullptr, Error::BadUserCodeLoad);
                         }
-                    } else {
-                        error_queue_add(Error::NotIdle);
                     }
                     break;
 
@@ -207,7 +154,7 @@ void main_loop()
 
                 // 'i' - Sends an identifying string to confirm that this is the stmdsp device.
                 case 'i':
-                    USBSerial::write((uint8_t *)"stmdsp", 6);
+                    USBSerial::write(reinterpret_cast<const uint8_t *>("stmdsp"), 6);
                     break;
 
                 // 'I' - Sends the current run status.
@@ -215,7 +162,7 @@ void main_loop()
                     {
                         unsigned char buf[2] = {
                             static_cast<unsigned char>(run_status),
-                            static_cast<unsigned char>(error_queue_pop())
+                            static_cast<unsigned char>(EM.pop())
                         };
                         USBSerial::write(buf, sizeof(buf));
                     }
@@ -224,45 +171,40 @@ void main_loop()
                 // 'M' - Begins continuous sampling, but measures the execution time of the first
                 //       sample processing. This duration can be later read through 'm'.
                 case 'M':
-                    if (run_status == RunStatus::Idle) {
+                    if (EM.assert(run_status == RunStatus::Idle, Error::NotIdle)) {
                         run_status = RunStatus::Running;
-                        dac_samples.fill(0);
-                        ADC::start(&adc_samples[0], adc_sample_count, signal_operate_measure);
-                        DAC::start(0, &dac_samples[0], dac_sample_count);
-                    } else {
-                        error_queue_add(Error::NotIdle);
+                        samplesOut.clear();
+                        ADC::start(samplesIn.data(), samplesIn.size(), signal_operate_measure);
+                        DAC::start(0, samplesOut.data(), samplesOut.size());
                     }
                     break;
 
                 // 'm' - Returns the last measured sample processing time, presumably in processor
                 //       ticks.
                 case 'm':
-                    USBSerial::write((uint8_t *)&conversion_time_measurement.last, sizeof(rtcnt_t));
+                    USBSerial::write(reinterpret_cast<uint8_t *>(&conversion_time_measurement.last),
+                                     sizeof(rtcnt_t));
                     break;
 
                 // 'R' - Begin continuous sampling/conversion of the ADC. Samples will go through
                 //       the conversion code, and will be sent out over the DAC.
                 case 'R':
-                    if (run_status == RunStatus::Idle) {
+                    if (EM.assert(run_status == RunStatus::Idle, Error::NotIdle)) {
                         run_status = RunStatus::Running;
-                        dac_samples.fill(0);
-                        ADC::start(&adc_samples[0], adc_sample_count, signal_operate);
-                        DAC::start(0, &dac_samples[0], dac_sample_count);
-                    } else {
-                        error_queue_add(Error::NotIdle);
+                        samplesOut.clear();
+                        ADC::start(samplesIn.data(), samplesIn.size(), signal_operate);
+                        DAC::start(0, samplesOut.data(), samplesOut.size());
                     }
                     break;
 
                 case 'r':
-                    if (USBSerial::read(&cmd[1], 1) == 1) {
+                    if (EM.assert(USBSerial::read(&cmd[1], 1) == 1, Error::BadParamSize)) {
                         if (cmd[1] == 0xFF) {
                             unsigned char r = static_cast<unsigned char>(ADC::getRate());
                             USBSerial::write(&r, 1);
                         } else {
                             ADC::setRate(static_cast<ADC::Rate>(cmd[1]));
                         }
-                    } else {
-                        error_queue_add(Error::BadParamSize);
                     }
                     break;
 
@@ -276,34 +218,30 @@ void main_loop()
                     break;
 
                 case 's':
-                    if (dac_samples_new != nullptr) {
-                        auto samps = reinterpret_cast<const uint8_t *>(
-                            const_cast<const dacsample_t *>(dac_samples_new));
-                        dac_samples_new = nullptr;
-
+                    if (auto samps = samplesOut.modified(); samps != nullptr) {
                         unsigned char buf[2] = {
-                            static_cast<unsigned char>(dac_sample_count / 2 & 0xFF),
-                            static_cast<unsigned char>(((dac_sample_count / 2) >> 8) & 0xFF)
+                            static_cast<unsigned char>(samplesOut.size() / 2 & 0xFF),
+                            static_cast<unsigned char>(((samplesOut.size() / 2) >> 8) & 0xFF)
                         };
                         USBSerial::write(buf, 2);
-                        unsigned int total = dac_sample_count / 2 * sizeof(dacsample_t);
+                        unsigned int total = samplesOut.bytesize() / 2;
                         unsigned int offset = 0;
                         unsigned char unused;
                         while (total > 512) {
-                            USBSerial::write(samps + offset, 512);
+                            USBSerial::write(reinterpret_cast<uint8_t *>(samps) + offset, 512);
                             while (USBSerial::read(&unused, 1) == 0);
                             offset += 512;
                             total -= 512;
                         }
-                        USBSerial::write(samps + offset, total);
+                        USBSerial::write(reinterpret_cast<uint8_t *>(samps) + offset, total);
                         while (USBSerial::read(&unused, 1) == 0);
                     } else {
-                        USBSerial::write((uint8_t *)"\0\0", 2);
+                        USBSerial::write(reinterpret_cast<const uint8_t *>("\0\0"), 2);
                     }
                     break;
 
                 case 'W':
-                    DAC::start(1, &dac2_samples[0], dac2_sample_count);
+                    DAC::start(1, samplesSigGen.data(), samplesSigGen.size());
                     break;
                 case 'w':
                     DAC::stop(1);
@@ -324,7 +262,7 @@ void conversion_abort()
     elf_entry = nullptr;
     DAC::stop(0);
     ADC::stop();
-    error_queue_add(Error::ConversionAborted);
+    EM.add(Error::ConversionAborted);
 }
 
 THD_FUNCTION(conversionThread, arg)
@@ -334,41 +272,23 @@ THD_FUNCTION(conversionThread, arg)
     while (1) {
         msg_t message;
         if (chMBFetchTimeout(&conversionMB, &message, TIME_INFINITE) == MSG_OK) {
-            adcsample_t *samples = nullptr;
-            auto halfsize = adc_sample_count / 2;
-            if (message == MSG_CONVFIRST) {
-                if (elf_entry)
-                    samples = elf_entry(&adc_samples[0], halfsize);
-                if (!samples)
-                    samples = &adc_samples[0];
-                std::copy(samples, samples + halfsize, &dac_samples[0]);
-                dac_samples_new = &dac_samples[0];
-            } else if (message == MSG_CONVSECOND) {
-                if (elf_entry)
-                    samples = elf_entry(&adc_samples[halfsize], halfsize);
-                if (!samples)
-                    samples = &adc_samples[halfsize];
-                std::copy(samples, samples + halfsize, &dac_samples[dac_sample_count / 2]);
-                dac_samples_new = &dac_samples[dac_sample_count / 2];
-            } else if (message == MSG_CONVFIRST_MEASURE) {
-                chTMStartMeasurementX(&conversion_time_measurement);
-                if (elf_entry)
-                    samples = elf_entry(&adc_samples[0], halfsize);
-                chTMStopMeasurementX(&conversion_time_measurement);
-                if (!samples)
-                    samples = &adc_samples[0];
-                std::copy(samples, samples + halfsize, &dac_samples[0]);
-                dac_samples_new = &dac_samples[0];
-            } else if (message == MSG_CONVSECOND_MEASURE) {
-                chTMStartMeasurementX(&conversion_time_measurement);
-                if (elf_entry)
-                    samples = elf_entry(&adc_samples[halfsize], halfsize);
-                chTMStopMeasurementX(&conversion_time_measurement);
-                if (!samples)
-                    samples = &adc_samples[halfsize];
-                std::copy(samples, samples + halfsize, &dac_samples[dac_sample_count / 2]);
-                dac_samples_new = &dac_samples[dac_sample_count / 2];
+            auto samples = MSG_FOR_FIRST(message) ? samplesIn.data() : samplesIn.middata();
+            auto size = samplesIn.size() / 2;
+
+            if (elf_entry) {
+                if (!MSG_FOR_MEASURE(message)) {
+                    samples = elf_entry(samples, size);
+                } else {
+                    chTMStartMeasurementX(&conversion_time_measurement);
+                    samples = elf_entry(samples, size);
+                    chTMStopMeasurementX(&conversion_time_measurement);
+                } 
             }
+
+            if (MSG_FOR_FIRST(message))
+                samplesOut.modify(samples, size); 
+            else
+                samplesOut.midmodify(samples, size); 
         }
     }
 }
@@ -378,12 +298,12 @@ void signal_operate(adcsample_t *buffer, [[maybe_unused]] size_t count)
     if (chMBGetUsedCountI(&conversionMB) > 1)
         conversion_abort();
     else
-        chMBPostI(&conversionMB, buffer == &adc_samples[0] ? MSG_CONVFIRST : MSG_CONVSECOND);
+        chMBPostI(&conversionMB, buffer == samplesIn.data() ? MSG_CONVFIRST : MSG_CONVSECOND);
 }
 
 void signal_operate_measure(adcsample_t *buffer, [[maybe_unused]] size_t count)
 {
-    chMBPostI(&conversionMB, buffer == &adc_samples[0] ? MSG_CONVFIRST_MEASURE : MSG_CONVSECOND_MEASURE);
+    chMBPostI(&conversionMB, buffer == samplesIn.data() ? MSG_CONVFIRST_MEASURE : MSG_CONVSECOND_MEASURE);
     ADC::setOperation(signal_operate);
 }