From c7f3fae266b800cd54679a104c00ae36de209a4d Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Mon, 16 Aug 2021 13:28:37 +0200
Subject: [PATCH] Implement an initial Motion ISR for AVR

This is a tentative/crude implementation of an Init and ISR for the MMU
in order to check the motion API.

We remove the "extern void Isr", declaring it "static inline" instead.
We need to inline the ISR here in order to avoid the function call.

Include the missing speed_table data in the executable. This bumps the
code size to ~60% of the flash.

Implemement motion::Init to setup the ISR and timers, and replace the
call in main from tmc::Init to motion::Init. Motion will init each
driver every time the axis is enabled, so there should be no need for
a global module initialization (we need SPI, but this is initialized
earlier on by it's own module anyway).

The timer is currently setup without any HAL or proper TIMER1 wrapper.
This is to be improved later.

The real MMU unit seems to slow down quite a bit during acceleration.
At this point we need to inline some methods in PulseGen to avoid
overhead, however this breaks the stubs.
---
 src/main.cpp               |  3 ++-
 src/modules/CMakeLists.txt |  1 +
 src/modules/motion.cpp     | 53 +++++++++++++++++++++++++++++++++++++-
 src/modules/motion.h       |  4 +--
 4 files changed, 57 insertions(+), 4 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index 28d5d43..30794ae 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -19,6 +19,7 @@
 #include "modules/selector.h"
 #include "modules/user_input.h"
 #include "modules/timebase.h"
+#include "modules/motion.h"
 
 #include "logic/command_base.h"
 #include "logic/cut_filament.h"
@@ -126,7 +127,7 @@ void setup() {
     ml::leds.SetMode(2, ml::Color::green, ml::Mode::on);
     ml::leds.Step();
 
-    // tmc::Init()
+    mm::Init();
     ml::leds.SetMode(1, ml::Color::green, ml::Mode::on);
     ml::leds.Step();
 
diff --git a/src/modules/CMakeLists.txt b/src/modules/CMakeLists.txt
index 3c5856c..139f1e0 100644
--- a/src/modules/CMakeLists.txt
+++ b/src/modules/CMakeLists.txt
@@ -12,6 +12,7 @@ target_sources(
           movable_base.cpp
           permanent_storage.cpp
           selector.cpp
+          speed_table.cpp
           timebase.cpp
           user_input.cpp
           pulse_gen.cpp
diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp
index 58ffb5e..2ee45fa 100644
--- a/src/modules/motion.cpp
+++ b/src/modules/motion.cpp
@@ -1,5 +1,12 @@
 #include "motion.h"
 
+// TODO: use proper timer abstraction
+#ifdef __AVR__
+#include <avr/interrupt.h>
+#else
+//#include "../hal/timers.h"
+#endif
+
 namespace modules {
 namespace motion {
 
@@ -89,7 +96,51 @@ st_timer_t Motion::Step() {
     return next;
 }
 
-void Isr() {}
+static inline void Isr() {
+    st_timer_t next = motion.Step();
+#ifdef __AVR__
+    // TODO: use proper timer abstraction
+    if (next)
+        OCR1A = next;
+    else {
+        // Idling: plan the next interrupt after 8ms from now.
+        OCR1A = 0x4000;
+    }
+#endif
+}
+
+void Init() {
+#ifdef __AVR__
+    // TODO: use proper timer abstraction
+
+    // waveform generation = 0100 = CTC
+    TCCR1B &= ~(1 << WGM13);
+    TCCR1B |= (1 << WGM12);
+    TCCR1A &= ~(1 << WGM11);
+    TCCR1A &= ~(1 << WGM10);
+
+    // output mode = 00 (disconnected)
+    TCCR1A &= ~(3 << COM1A0);
+    TCCR1A &= ~(3 << COM1B0);
+
+    // Set the timer pre-scaler
+    // We use divider of 8, resulting in a 2MHz timer frequency on a 16MHz MCU
+    TCCR1B = (TCCR1B & ~(0x07 << CS10)) | (2 << CS10);
+
+    // Plan the first interrupt after 8ms from now.
+    OCR1A = 0x4000;
+    TCNT1 = 0;
+
+    // Enable interrupt
+    TIMSK1 |= (1 << OCIE1A);
+#endif
+}
 
 } // namespace motion
 } // namespace modules
+
+#ifdef __AVR__
+ISR(TIMER1_COMPA_vect) {
+    modules::motion::Isr();
+}
+#endif
diff --git a/src/modules/motion.h b/src/modules/motion.h
index 79b8a2c..5fd3e08 100644
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@@ -250,8 +250,8 @@ private:
     };
 };
 
-/// ISR stepping routine
-//extern void Isr();
+/// ISR initialization
+extern void Init();
 
 extern Motion motion;