From 1b59766fcba97a003709660a07687bd0caaf5c29 Mon Sep 17 00:00:00 2001
From: Sebastianv650 <sebastian_v650@kabelmail.de>
Date: Sun, 12 Feb 2017 13:09:06 +0100
Subject: [PATCH 1/3] Cleanup position_float

Hopefully fixes Marlin #5481
---
 Marlin/planner.cpp | 36 ++++++++++++++++++++++++++++++++----
 1 file changed, 32 insertions(+), 4 deletions(-)

diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index f6a3edfa763..8aba9ddc170 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -673,10 +673,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
 
   #if ENABLED(LIN_ADVANCE)
     const float target_float[XYZE] = { a, b, c, e },
-                de_float = target_float[E_AXIS] - position_float[E_AXIS],
                 mm_D_float = sqrt(sq(target_float[X_AXIS] - position_float[X_AXIS]) + sq(target_float[Y_AXIS] - position_float[Y_AXIS]));
-
-    memcpy(position_float, target_float, sizeof(position_float));
   #endif
 
   const long da = target[X_AXIS] - position[X_AXIS],
@@ -707,15 +704,27 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
   //*/
 
   // DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
-  if (DEBUGGING(DRYRUN)) position[E_AXIS] = target[E_AXIS];
+  if (DEBUGGING(DRYRUN)) {
+    position[E_AXIS] = target[E_AXIS];
+    #if ENABLED(LIN_ADVANCE)
+      position_float[E_AXIS] = target_float[E_AXIS];
+    #endif
+  }
 
   long de = target[E_AXIS] - position[E_AXIS];
+  #if ENABLED(LIN_ADVANCE)
+    float de_float = target_float[E_AXIS] - position_float[E_AXIS];
+  #endif
 
   #if ENABLED(PREVENT_COLD_EXTRUSION)
     if (de) {
       if (thermalManager.tooColdToExtrude(extruder)) {
         position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
         de = 0; // no difference
+        #if ENABLED(LIN_ADVANCE)
+          position_float[E_AXIS] = target_float[E_AXIS];
+          de_float = 0;
+        #endif
         SERIAL_ECHO_START;
         SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
       }
@@ -723,6 +732,10 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
         if (labs(de) > (int32_t)axis_steps_per_mm[E_AXIS_N] * (EXTRUDE_MAXLENGTH)) { // It's not important to get max. extrusion length in a precision < 1mm, so save some cycles and cast to int
           position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
           de = 0; // no difference
+          #if ENABLED(LIN_ADVANCE)
+            position_float[E_AXIS] = target_float[E_AXIS];
+            de_float = 0;
+          #endif
           SERIAL_ECHO_START;
           SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
         }
@@ -1342,6 +1355,9 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
 
   // Update the position (only when a move was queued)
   memcpy(position, target, sizeof(position));
+  #if ENABLED(LIN_ADVANCE)
+    memcpy(position_float, target_float, sizeof(position_float));
+  #endif
 
   recalculate();
 
@@ -1367,6 +1383,12 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
        nb = position[Y_AXIS] = lround(b * axis_steps_per_mm[Y_AXIS]),
        nc = position[Z_AXIS] = lround(c * axis_steps_per_mm[Z_AXIS]),
        ne = position[E_AXIS] = lround(e * axis_steps_per_mm[_EINDEX]);
+  #if ENABLED(LIN_ADVANCE)
+    position_float[X_AXIS] = a;
+    position_float[Y_AXIS] = b;
+    position_float[Z_AXIS] = c;
+    position_float[E_AXIS] = e;
+  #endif
   stepper.set_position(na, nb, nc, ne);
   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
   ZERO(previous_speed);
@@ -1392,6 +1414,9 @@ void Planner::set_position_mm_kinematic(const float position[NUM_AXIS]) {
  */
 void Planner::sync_from_steppers() {
   LOOP_XYZE(i) position[i] = stepper.position((AxisEnum)i);
+  #if ENABLED(LIN_ADVANCE)
+    LOOP_XYZE(i) position_float[i] = stepper.position((AxisEnum)i) * steps_to_mm[i];
+  #endif
 }
 
 /**
@@ -1405,6 +1430,9 @@ void Planner::set_position_mm(const AxisEnum axis, const float& v) {
     const uint8_t axis_index = axis;
   #endif
   position[axis] = lround(v * axis_steps_per_mm[axis_index]);
+  #if ENABLED(LIN_ADVANCE)
+    position_float[axis] = v;
+  #endif
   stepper.set_position(axis, v);
   previous_speed[axis] = 0.0;
 }

From 271ced73413ebf8d662cce2a582ac9b795104a8c Mon Sep 17 00:00:00 2001
From: Sebastianv650 <sebastian_v650@kabelmail.de>
Date: Sun, 12 Feb 2017 18:00:14 +0100
Subject: [PATCH 2/3] Prevent re-entering of temperature ISR

If Marlin is inside the temperature ISR, the stepper ISR is enabled. If
a stepper event is now happening Marlin will proceed with the stepper
ISR. Now, at the end of the stepper ISR, the temperatre ISR gets enabled
again. While Marlin proceed the rest of the temperature ISR, it's now
vulnerable to a second ISR call.
---
 Marlin/temperature.cpp | 11 ++++++++++-
 Marlin/temperature.h   |  2 ++
 2 files changed, 12 insertions(+), 1 deletion(-)

diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index 8120e0d6c5e..d7462bc99db 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -1483,8 +1483,15 @@ void Temperature::set_current_temp_raw() {
  */
 ISR(TIMER0_COMPB_vect) { Temperature::isr(); }
 
+volatile bool Temperature::in_temp_isr = false;
+
 void Temperature::isr() {
-  //Allow UART and stepper ISRs
+  // The stepper ISR can interrupt this ISR. When it does it re-enables this ISR
+  // at the end of its run, potentially causing re-entry. This flag prevents it.
+  if (in_temp_isr) return;
+  in_temp_isr = true;
+  
+  // Allow UART and stepper ISRs
   CBI(TIMSK0, OCIE0B); //Disable Temperature ISR
   sei();
 
@@ -1949,5 +1956,7 @@ void Temperature::isr() {
     }
   #endif
 
+  cli();
+  in_temp_isr = false;
   SBI(TIMSK0, OCIE0B); //re-enable Temperature ISR
 }
diff --git a/Marlin/temperature.h b/Marlin/temperature.h
index 182efd5645e..e5921484478 100644
--- a/Marlin/temperature.h
+++ b/Marlin/temperature.h
@@ -61,6 +61,8 @@ class Temperature {
                  current_temperature_bed_raw,
                  target_temperature_bed;
 
+    static volatile bool in_temp_isr;
+
     #if ENABLED(TEMP_SENSOR_1_AS_REDUNDANT)
       static float redundant_temperature;
     #endif

From 97b6fb6381248481c9182e4f2bb6dcc50af4e8b9 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 14 Feb 2017 06:00:17 -0600
Subject: [PATCH 3/3] Reduce / optimize LIN_ADVANCE code

---
 Marlin/planner.cpp | 17 ++++++++++-------
 Marlin/stepper.cpp | 24 +++++++++++++-----------
 2 files changed, 23 insertions(+), 18 deletions(-)

diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 8aba9ddc170..37d2adf20a8 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -672,8 +672,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
   #endif
 
   #if ENABLED(LIN_ADVANCE)
-    const float target_float[XYZE] = { a, b, c, e },
-                mm_D_float = sqrt(sq(target_float[X_AXIS] - position_float[X_AXIS]) + sq(target_float[Y_AXIS] - position_float[Y_AXIS]));
+    const float mm_D_float = sqrt(sq(a - position_float[X_AXIS]) + sq(b - position_float[Y_AXIS]));
   #endif
 
   const long da = target[X_AXIS] - position[X_AXIS],
@@ -707,13 +706,14 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
   if (DEBUGGING(DRYRUN)) {
     position[E_AXIS] = target[E_AXIS];
     #if ENABLED(LIN_ADVANCE)
-      position_float[E_AXIS] = target_float[E_AXIS];
+      position_float[E_AXIS] = e;
     #endif
   }
 
   long de = target[E_AXIS] - position[E_AXIS];
+
   #if ENABLED(LIN_ADVANCE)
-    float de_float = target_float[E_AXIS] - position_float[E_AXIS];
+    float de_float = e - position_float[E_AXIS];
   #endif
 
   #if ENABLED(PREVENT_COLD_EXTRUSION)
@@ -722,7 +722,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
         position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
         de = 0; // no difference
         #if ENABLED(LIN_ADVANCE)
-          position_float[E_AXIS] = target_float[E_AXIS];
+          position_float[E_AXIS] = e;
           de_float = 0;
         #endif
         SERIAL_ECHO_START;
@@ -733,7 +733,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
           position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
           de = 0; // no difference
           #if ENABLED(LIN_ADVANCE)
-            position_float[E_AXIS] = target_float[E_AXIS];
+            position_float[E_AXIS] = e;
             de_float = 0;
           #endif
           SERIAL_ECHO_START;
@@ -1356,7 +1356,10 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
   // Update the position (only when a move was queued)
   memcpy(position, target, sizeof(position));
   #if ENABLED(LIN_ADVANCE)
-    memcpy(position_float, target_float, sizeof(position_float));
+    position_float[X_AXIS] = a;
+    position_float[Y_AXIS] = b;
+    position_float[Z_AXIS] = c;
+    position_float[E_AXIS] = e;
   #endif
 
   recalculate();
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index ddab5416eaf..da2ee14cc18 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -342,13 +342,14 @@ ISR(TIMER1_COMPA_vect) {
   #endif
 }
 
-void Stepper::isr() {
-  #define _ENABLE_ISRs() cli(); SBI(TIMSK0, OCIE0B); ENABLE_STEPPER_DRIVER_INTERRUPT()
+#define _ENABLE_ISRs() do { cli(); if (thermalManager.in_temp_isr) CBI(TIMSK0, OCIE0B); else SBI(TIMSK0, OCIE0B); ENABLE_STEPPER_DRIVER_INTERRUPT(); } while(0)
 
-  uint16_t timer, remainder, ocr_val;
+void Stepper::isr() {
 
   static uint32_t step_remaining = 0;
 
+  uint16_t ocr_val;
+
   #define ENDSTOP_NOMINAL_OCR_VAL 3000    // check endstops every 1.5ms to guarantee two stepper ISRs within 5ms for BLTouch
   #define OCR_VAL_TOLERANCE 1000          // First max delay is 2.0ms, last min delay is 0.5ms, all others 1.5ms
 
@@ -366,7 +367,7 @@ void Stepper::isr() {
     #define SPLIT(L) do { \
       _SPLIT(L); \
       if (ENDSTOPS_ENABLED && L > ENDSTOP_NOMINAL_OCR_VAL) { \
-        remainder = (uint16_t)L % (ENDSTOP_NOMINAL_OCR_VAL); \
+        uint16_t remainder = (uint16_t)L % (ENDSTOP_NOMINAL_OCR_VAL); \
         ocr_val = (remainder < OCR_VAL_TOLERANCE) ? ENDSTOP_NOMINAL_OCR_VAL + remainder : ENDSTOP_NOMINAL_OCR_VAL; \
         step_remaining = (uint16_t)L - ocr_val; \
       } \
@@ -374,13 +375,16 @@ void Stepper::isr() {
 
     if (step_remaining && ENDSTOPS_ENABLED) {   // Just check endstops - not yet time for a step
       endstops.update();
-      ocr_val = step_remaining;
       if (step_remaining > ENDSTOP_NOMINAL_OCR_VAL) {
-        step_remaining = step_remaining - ENDSTOP_NOMINAL_OCR_VAL;
+        step_remaining -= ENDSTOP_NOMINAL_OCR_VAL;
         ocr_val = ENDSTOP_NOMINAL_OCR_VAL;
       }
-      else step_remaining = 0;  //  last one before the ISR that does the step
-      _NEXT_ISR(ocr_val);  //
+      else {
+        ocr_val = step_remaining;
+        step_remaining = 0;  //  last one before the ISR that does the step
+      }
+
+      _NEXT_ISR(ocr_val);
 
       NOLESS(OCR1A, TCNT1 + 16);
 
@@ -867,9 +871,7 @@ void Stepper::isr() {
     NOLESS(OCR1A, TCNT1 + 16);
 
     // Restore original ISR settings
-    cli();
-    SBI(TIMSK0, OCIE0B);
-    ENABLE_STEPPER_DRIVER_INTERRUPT();
+    _ENABLE_ISRs();
   }
 
 #endif // ADVANCE or LIN_ADVANCE