From 95b76a65c3fa39c265dca000ca75ce5ad7025720 Mon Sep 17 00:00:00 2001
From: Jason Smith <jason.inet@gmail.com>
Date: Tue, 14 Jul 2020 17:07:27 -0700
Subject: [PATCH] Increase STM32F1 Servo Timer Interrupt Priority (#18637)

---
 Marlin/src/HAL/STM32F1/Servo.cpp  |  1 +
 Marlin/src/HAL/STM32F1/timers.cpp | 16 ++++++++++++----
 Marlin/src/HAL/STM32F1/timers.h   |  7 +++++--
 3 files changed, 18 insertions(+), 6 deletions(-)

diff --git a/Marlin/src/HAL/STM32F1/Servo.cpp b/Marlin/src/HAL/STM32F1/Servo.cpp
index 3812dc1ab5..bda7ea1aa7 100644
--- a/Marlin/src/HAL/STM32F1/Servo.cpp
+++ b/Marlin/src/HAL/STM32F1/Servo.cpp
@@ -74,6 +74,7 @@ void libServo::servoWrite(uint8_t inPin, uint16_t duty_cycle) {
 
 libServo::libServo() {
   servoIndex = ServoCount < MAX_SERVOS ? ServoCount++ : INVALID_SERVO;
+  timer_set_interrupt_priority(SERVO0_TIMER_NUM, SERVO0_TIMER_IRQ_PRIO);
 }
 
 bool libServo::attach(const int32_t inPin, const int32_t inMinAngle, const int32_t inMaxAngle) {
diff --git a/Marlin/src/HAL/STM32F1/timers.cpp b/Marlin/src/HAL/STM32F1/timers.cpp
index b3a9ce4f75..bb466cc16f 100644
--- a/Marlin/src/HAL/STM32F1/timers.cpp
+++ b/Marlin/src/HAL/STM32F1/timers.cpp
@@ -47,7 +47,10 @@
  * TODO: Calculate Timer prescale value, so we get the 32bit to adjust
  */
 
-void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
+
+
+
+void timer_set_interrupt_priority(uint_fast8_t timer_num, uint_fast8_t priority) {
   nvic_irq_num irq_num;
   switch (timer_num) {
     case 1: irq_num = NVIC_TIMER1_CC; break;
@@ -64,9 +67,14 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
        * This should never happen. Add a Sanitycheck for timer number.
        * Should be a general timer since basic timers have no CC channels.
        */
-      break;
+      return;
   }
 
+  nvic_irq_set_priority(irq_num, priority);
+}
+
+
+void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
   /**
    * Give the Stepper ISR a higher priority (lower number)
    * so it automatically preempts the Temperature ISR.
@@ -83,7 +91,7 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
       timer_set_compare(STEP_TIMER_DEV, STEP_TIMER_CHAN, _MIN(hal_timer_t(HAL_TIMER_TYPE_MAX), (STEPPER_TIMER_RATE) / frequency));
       timer_no_ARR_preload_ARPE(STEP_TIMER_DEV); // Need to be sure no preload on ARR register
       timer_attach_interrupt(STEP_TIMER_DEV, STEP_TIMER_CHAN, stepTC_Handler);
-      nvic_irq_set_priority(irq_num, STEP_TIMER_IRQ_PRIO);
+      timer_set_interrupt_priority(STEP_TIMER_NUM, STEP_TIMER_IRQ_PRIO);
       timer_generate_update(STEP_TIMER_DEV);
       timer_resume(STEP_TIMER_DEV);
       break;
@@ -95,7 +103,7 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
       timer_set_reload(TEMP_TIMER_DEV, 0xFFFF);
       timer_set_compare(TEMP_TIMER_DEV, TEMP_TIMER_CHAN, _MIN(hal_timer_t(HAL_TIMER_TYPE_MAX), (F_CPU) / (TEMP_TIMER_PRESCALE) / frequency));
       timer_attach_interrupt(TEMP_TIMER_DEV, TEMP_TIMER_CHAN, tempTC_Handler);
-      nvic_irq_set_priority(irq_num, TEMP_TIMER_IRQ_PRIO);
+      timer_set_interrupt_priority(TEMP_TIMER_NUM, TEMP_TIMER_IRQ_PRIO);
       timer_generate_update(TEMP_TIMER_DEV);
       timer_resume(TEMP_TIMER_DEV);
       break;
diff --git a/Marlin/src/HAL/STM32F1/timers.h b/Marlin/src/HAL/STM32F1/timers.h
index 57fd0e3c4c..658a436988 100644
--- a/Marlin/src/HAL/STM32F1/timers.h
+++ b/Marlin/src/HAL/STM32F1/timers.h
@@ -91,8 +91,9 @@ typedef uint16_t hal_timer_t;
   #define SERVO0_TIMER_NUM 1  // SERVO0 or BLTOUCH
 #endif
 
-#define STEP_TIMER_IRQ_PRIO 1
-#define TEMP_TIMER_IRQ_PRIO 2
+#define STEP_TIMER_IRQ_PRIO 2
+#define TEMP_TIMER_IRQ_PRIO 3
+#define SERVO0_TIMER_IRQ_PRIO 1
 
 #define TEMP_TIMER_PRESCALE     1000 // prescaler for setting Temp timer, 72Khz
 #define TEMP_TIMER_FREQUENCY    1000 // temperature interrupt frequency
@@ -193,4 +194,6 @@ FORCE_INLINE static void timer_no_ARR_preload_ARPE(timer_dev *dev) {
   bb_peri_set_bit(&(dev->regs).gen->CR1, TIMER_CR1_ARPE_BIT, 0);
 }
 
+void timer_set_interrupt_priority(uint_fast8_t timer_num, uint_fast8_t priority);
+
 #define TIMER_OC_NO_PRELOAD 0 // Need to disable preload also on compare registers.