From a456c4a52df5b78fac43455888f2c7708cc372ca Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Wed, 17 Feb 2021 13:04:46 +0100
Subject: [PATCH 1/2] Make watchdogReset() force_inline

It makes no sense keeping watchdogReset as a separate function which
must be called, since it only contains one instruction: "wdr".

Not only was the code larger by 32 bytes in total, but also much slower
(call+ret take 4 cycles together for no reason in this case).

Surprisingly, doing just this on FW 3.9.3 solves issue #2954 on the one
affected EINSY board, even though it makes not much sense (there must be
some other timing issue).
---
 Firmware/optiboot_w25x20cl.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Firmware/optiboot_w25x20cl.cpp b/Firmware/optiboot_w25x20cl.cpp
index dce4074e..2d3a76c7 100644
--- a/Firmware/optiboot_w25x20cl.cpp
+++ b/Firmware/optiboot_w25x20cl.cpp
@@ -43,7 +43,7 @@ static void watchdogConfig(uint8_t x) {
   WDTCSR = x;
 }
 
-static void watchdogReset() {
+static FORCE_INLINE void watchdogReset() {
   __asm__ __volatile__ (
     "wdr\n"
   );

From 4e768057e7cc5449677220b5bddd41acb6ee073b Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Wed, 17 Feb 2021 15:59:52 +0100
Subject: [PATCH 2/2] Use standard wdt_reset() from AVR lib

which translates to the one `wdr` instruction like before
+ wrap configuration of watchdog into cli/sei
---
 Firmware/optiboot_w25x20cl.cpp | 21 +++++++++------------
 1 file changed, 9 insertions(+), 12 deletions(-)

diff --git a/Firmware/optiboot_w25x20cl.cpp b/Firmware/optiboot_w25x20cl.cpp
index 2d3a76c7..a18c8832 100644
--- a/Firmware/optiboot_w25x20cl.cpp
+++ b/Firmware/optiboot_w25x20cl.cpp
@@ -7,6 +7,7 @@
 #include "w25x20cl.h"
 #include "stk500.h"
 #include "bootapp.h"
+#include <avr/wdt.h>
 
 #define OPTIBOOT_MAJVER 6
 #define OPTIBOOT_CUSTOMVER 0
@@ -39,14 +40,10 @@ static unsigned const int __attribute__((section(".version")))
 #endif
 
 static void watchdogConfig(uint8_t x) {
+  CRITICAL_SECTION_START
   WDTCSR = _BV(WDCE) | _BV(WDE);
   WDTCSR = x;
-}
-
-static FORCE_INLINE void watchdogReset() {
-  __asm__ __volatile__ (
-    "wdr\n"
-  );
+  CRITICAL_SECTION_END
 }
 
 #define RECV_READY ((UCSR0A & _BV(RXC0)) != 0)
@@ -63,7 +60,7 @@ static uint8_t getch(void) {
        * the application "soon", if it keeps happening.  (Note that we
        * don't care that an invalid char is returned...)
        */
-    watchdogReset();
+    wdt_reset();
   }
   ch = UDR0;
   return ch;
@@ -117,7 +114,7 @@ uint8_t optiboot_w25x20cl_enter()
   // Handshake sequence: Initialize the serial line, flush serial line, send magic, receive magic.
   // If the magic is not received on time, or it is not received correctly, continue to the application.
   {
-    watchdogReset();
+    wdt_reset();
     unsigned long  boot_timeout = 2000000;
     unsigned long  boot_timer = 0;
     const char    *ptr = entry_magic_send;
@@ -125,7 +122,7 @@ uint8_t optiboot_w25x20cl_enter()
     const uint8_t selectedSerialPort_bak = selectedSerialPort;
     // Flush the serial line.
     while (RECV_READY) {
-      watchdogReset();
+      wdt_reset();
       // Dummy register read (discard)
       (void)(*(char *)UDR0);
     }
@@ -135,14 +132,14 @@ uint8_t optiboot_w25x20cl_enter()
     // Send the initial magic string.
     while (ptr != end)
       putch(pgm_read_byte(ptr ++));
-    watchdogReset();
+    wdt_reset();
     // Wait for two seconds until a magic string (constant entry_magic) is received
     // from the serial line.
     ptr = entry_magic_receive;
     end = strlen_P(entry_magic_receive) + ptr;
     while (ptr != end) {
       while (rx_buffer.head == SerialHead) {
-        watchdogReset();
+        wdt_reset();
         delayMicroseconds(1);
         if (++ boot_timer > boot_timeout)
         {
@@ -159,7 +156,7 @@ uint8_t optiboot_w25x20cl_enter()
           selectedSerialPort = selectedSerialPort_bak; //revert Serial setting
           return 0;
       }
-      watchdogReset();
+      wdt_reset();
     }
     cbi(UCSR0B, RXCIE0); //disable the MarlinSerial0 interrupt
     // Send the cfm magic string.