From ee8c80e5c438e2da7f67acd6c9dcc32e91a21794 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Thu, 2 Sep 2021 11:18:27 +0200
Subject: [PATCH] Motion: Panic if queue is full

If the queue is full and a new move is queued, panic!

Introduce a new error code QUEUE_FULL to help diagnose situations where
the queue is handled improperly: likely one of the state machines not
waiting for the previous actions to finish.

PulseGen::PlanMove returns a boolean if the queue cannot be moved.
We could extend this to Motion::PlanMove, however all moves would then
have to check for this. Having a global check such as this ensures
we never ignore such situation.
---
 src/logic/error_codes.h                  | 2 +-
 src/modules/motion.cpp                   | 4 ++++
 tests/unit/modules/motion/CMakeLists.txt | 5 ++++-
 tests/unit/modules/stubs/stub_panic.cpp  | 8 ++++++++
 4 files changed, 17 insertions(+), 2 deletions(-)
 create mode 100644 tests/unit/modules/stubs/stub_panic.cpp

diff --git a/src/logic/error_codes.h b/src/logic/error_codes.h
index 11fea30..8b8a00d 100644
--- a/src/logic/error_codes.h
+++ b/src/logic/error_codes.h
@@ -28,8 +28,8 @@ enum class ErrorCode : uint_fast16_t {
 
     INVALID_TOOL = 0x8006, ///< tool/slot index out of range (typically issuing T5 into an MMU with just 5 slots - valid range 0-4)
 
+    QUEUE_FULL = 0x802d, ///< internal logic error - attempt to move with a full queue
     MMU_NOT_RESPONDING = 0x802e, ///< internal error of the printer - communication with the MMU is not working
-
     INTERNAL = 0x802f, ///< internal runtime error (software)
 
     // TMC bit masks
diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp
index edc6e79..3119144 100644
--- a/src/modules/motion.cpp
+++ b/src/modules/motion.cpp
@@ -1,4 +1,5 @@
 #include "motion.h"
+#include "panic.h"
 
 // TODO: use proper timer abstraction
 #ifdef __AVR__
@@ -48,6 +49,9 @@ void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feed_rate, steps_t end_rat
         // move was queued, prepare the axis
         if (!axisData[axis].enabled)
             SetEnabled(axis, true);
+    } else {
+        // queue is full: queue mishandling! trigger a panic
+        Panic(ErrorCode::QUEUE_FULL);
     }
 }
 
diff --git a/tests/unit/modules/motion/CMakeLists.txt b/tests/unit/modules/motion/CMakeLists.txt
index fb7be25..7aafdc4 100644
--- a/tests/unit/modules/motion/CMakeLists.txt
+++ b/tests/unit/modules/motion/CMakeLists.txt
@@ -1,10 +1,13 @@
 # common include and source directories
-set(include_common ${CMAKE_SOURCE_DIR}/src/modules ${CMAKE_SOURCE_DIR}/src/hal)
+set(include_common ${CMAKE_SOURCE_DIR}/src/modules ${CMAKE_SOURCE_DIR}/src/hal
+                   ${CMAKE_SOURCE_DIR}/src
+    )
 set(source_common
     ${CMAKE_SOURCE_DIR}/src/modules/motion.cpp
     ${CMAKE_SOURCE_DIR}/src/modules/speed_table.cpp
     ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
     ${MODULES_STUBS_DIR}/stub_gpio.cpp
+    ${MODULES_STUBS_DIR}/stub_panic.cpp
     ${CMAKE_SOURCE_DIR}/src/hal/tmc2130.cpp
     ${MODULES_STUBS_DIR}/stub_shr16.cpp
     ${MODULES_STUBS_DIR}/stub_spi.cpp
diff --git a/tests/unit/modules/stubs/stub_panic.cpp b/tests/unit/modules/stubs/stub_panic.cpp
new file mode 100644
index 0000000..112251b
--- /dev/null
+++ b/tests/unit/modules/stubs/stub_panic.cpp
@@ -0,0 +1,8 @@
+#include "panic.h"
+
+// For retrival during tests
+ErrorCode panic_code = ErrorCode::RUNNING;
+
+void Panic(ErrorCode ec) {
+    panic_code = ec;
+}