From cfe1d52bf247f5152e51f87ff7d770c0aef63a56 Mon Sep 17 00:00:00 2001
From: Protomosh <43253582+Protomosh@users.noreply.github.com>
Date: Fri, 19 Aug 2022 20:57:27 +0300
Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix=20DGUS=20Reloaded=20+=20STM3?=
 =?UTF-8?q?2=20(#24600)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Marlin/src/lcd/extui/dgus/DGUSDisplay.h       |   1 -
 .../src/lcd/extui/dgus/DGUSScreenHandler.cpp  |  47 +++----
 Marlin/src/lcd/extui/dgus/DGUSScreenHandler.h |   4 +
 .../lcd/extui/dgus/mks/DGUSScreenHandler.cpp  | 121 ++++++++----------
 .../src/lcd/extui/dgus_reloaded/DGUSDisplay.h |  11 +-
 .../lcd/extui/dgus_reloaded/DGUSRxHandler.cpp |  22 ++--
 .../lcd/extui/dgus_reloaded/DGUSRxHandler.h   |   2 +-
 .../lcd/extui/dgus_reloaded/DGUSTxHandler.h   |   2 +
 8 files changed, 102 insertions(+), 108 deletions(-)

diff --git a/Marlin/src/lcd/extui/dgus/DGUSDisplay.h b/Marlin/src/lcd/extui/dgus/DGUSDisplay.h
index b6773db03b..c307ff4478 100644
--- a/Marlin/src/lcd/extui/dgus/DGUSDisplay.h
+++ b/Marlin/src/lcd/extui/dgus/DGUSDisplay.h
@@ -39,7 +39,6 @@
 
 enum DGUSLCD_Screens : uint8_t;
 
-//#define DEBUG_DGUSLCD
 #define DEBUG_OUT ENABLED(DEBUG_DGUSLCD)
 #include "../../../core/debug_out.h"
 
diff --git a/Marlin/src/lcd/extui/dgus/DGUSScreenHandler.cpp b/Marlin/src/lcd/extui/dgus/DGUSScreenHandler.cpp
index 0f34d76cfa..37543a237c 100644
--- a/Marlin/src/lcd/extui/dgus/DGUSScreenHandler.cpp
+++ b/Marlin/src/lcd/extui/dgus/DGUSScreenHandler.cpp
@@ -152,10 +152,10 @@ void DGUSScreenHandler::DGUSLCD_SendPrintTimeToDisplay(DGUS_VP_Variable &var) {
 // Send an uint8_t between 0 and 100 to a variable scale to 0..255
 void DGUSScreenHandler::DGUSLCD_PercentageToUint8(DGUS_VP_Variable &var, void *val_ptr) {
   if (var.memadr) {
-    uint16_t value = swap16(*(uint16_t*)val_ptr);
-    DEBUG_ECHOLNPGM("FAN value get:", value);
+    const uint16_t value = BE16_P(val_ptr);
+    DEBUG_ECHOLNPGM("Got percent:", value);
     *(uint8_t*)var.memadr = map(constrain(value, 0, 100), 0, 100, 0, 255);
-    DEBUG_ECHOLNPGM("FAN value change:", *(uint8_t*)var.memadr);
+    DEBUG_ECHOLNPGM("Set uint8:", *(uint8_t*)var.memadr);
   }
 }
 
@@ -264,10 +264,10 @@ void DGUSScreenHandler::DGUSLCD_SendHeaterStatusToDisplay(DGUS_VP_Variable &var)
     static uint16_t period = 0;
     static uint16_t index = 0;
     //DEBUG_ECHOPGM(" DGUSLCD_SendWaitingStatusToDisplay ", var.VP);
-    //DEBUG_ECHOLNPGM(" data ", swap16(index));
+    //DEBUG_ECHOLNPGM(" data ", BE16_P(&index));
     if (period++ > DGUS_UI_WAITING_STATUS_PERIOD) {
       dgusdisplay.WriteVariable(var.VP, index);
-      //DEBUG_ECHOLNPGM(" data ", swap16(index));
+      //DEBUG_ECHOLNPGM(" data ", BE16_P(&index));
       if (++index >= DGUS_UI_WAITING_STATUS) index = 0;
       period = 0;
     }
@@ -306,7 +306,7 @@ void DGUSScreenHandler::DGUSLCD_SendHeaterStatusToDisplay(DGUS_VP_Variable &var)
 
   void DGUSScreenHandler::DGUSLCD_SD_ScrollFilelist(DGUS_VP_Variable& var, void *val_ptr) {
     auto old_top = top_file;
-    const int16_t scroll = (int16_t)swap16(*(uint16_t*)val_ptr);
+    const int16_t scroll = (int16_t)BE16_P(val_ptr);
     if (scroll) {
       top_file += scroll;
       DEBUG_ECHOPGM("new topfile calculated:", top_file);
@@ -391,7 +391,7 @@ void DGUSScreenHandler::HandleAllHeatersOff(DGUS_VP_Variable &var, void *val_ptr
 }
 
 void DGUSScreenHandler::HandleTemperatureChanged(DGUS_VP_Variable &var, void *val_ptr) {
-  celsius_t newvalue = swap16(*(uint16_t*)val_ptr);
+  celsius_t newvalue = BE16_P(val_ptr);
   celsius_t acceptedvalue;
 
   switch (var.VP) {
@@ -426,7 +426,7 @@ void DGUSScreenHandler::HandleTemperatureChanged(DGUS_VP_Variable &var, void *va
 
 void DGUSScreenHandler::HandleFlowRateChanged(DGUS_VP_Variable &var, void *val_ptr) {
   #if HAS_EXTRUDERS
-    uint16_t newvalue = swap16(*(uint16_t*)val_ptr);
+    const uint16_t newvalue = BE16_P(val_ptr);
     uint8_t target_extruder;
     switch (var.VP) {
       default: return;
@@ -446,7 +446,7 @@ void DGUSScreenHandler::HandleFlowRateChanged(DGUS_VP_Variable &var, void *val_p
 void DGUSScreenHandler::HandleManualExtrude(DGUS_VP_Variable &var, void *val_ptr) {
   DEBUG_ECHOLNPGM("HandleManualExtrude");
 
-  int16_t movevalue = swap16(*(uint16_t*)val_ptr);
+  const int16_t movevalue = BE16_P(val_ptr);
   float target = movevalue * 0.01f;
   ExtUI::extruder_t target_extruder;
 
@@ -468,19 +468,19 @@ void DGUSScreenHandler::HandleManualExtrude(DGUS_VP_Variable &var, void *val_ptr
 #if ENABLED(DGUS_UI_MOVE_DIS_OPTION)
   void DGUSScreenHandler::HandleManualMoveOption(DGUS_VP_Variable &var, void *val_ptr) {
     DEBUG_ECHOLNPGM("HandleManualMoveOption");
-    *(uint16_t*)var.memadr = swap16(*(uint16_t*)val_ptr);
+    *(uint16_t*)var.memadr = BE16_P(val_ptr);
   }
 #endif
 
 void DGUSScreenHandler::HandleMotorLockUnlock(DGUS_VP_Variable &var, void *val_ptr) {
   DEBUG_ECHOLNPGM("HandleMotorLockUnlock");
-  const int16_t lock = swap16(*(uint16_t*)val_ptr);
+  const int16_t lock = BE16_P(val_ptr);
   queue.enqueue_one_now(lock ? F("M18") : F("M17"));
 }
 
 void DGUSScreenHandler::HandleSettings(DGUS_VP_Variable &var, void *val_ptr) {
   DEBUG_ECHOLNPGM("HandleSettings");
-  uint16_t value = swap16(*(uint16_t*)val_ptr);
+  const uint16_t value = BE16_P(val_ptr);
   switch (value) {
     default: break;
     case 1:
@@ -494,11 +494,9 @@ void DGUSScreenHandler::HandleSettings(DGUS_VP_Variable &var, void *val_ptr) {
 }
 
 void DGUSScreenHandler::HandleStepPerMMChanged(DGUS_VP_Variable &var, void *val_ptr) {
-  DEBUG_ECHOLNPGM("HandleStepPerMMChanged");
-
-  uint16_t value_raw = swap16(*(uint16_t*)val_ptr);
-  DEBUG_ECHOLNPGM("value_raw:", value_raw);
-  float value = (float)value_raw / 10;
+  const uint16_t value_raw = BE16_P(val_ptr);
+  DEBUG_ECHOLNPGM("HandleStepPerMMChanged:", value_raw);
+  const float value = (float)value_raw / 10;
   ExtUI::axis_t axis;
   switch (var.VP) {
     case VP_X_STEP_PER_MM: axis = ExtUI::axis_t::X; break;
@@ -510,15 +508,12 @@ void DGUSScreenHandler::HandleStepPerMMChanged(DGUS_VP_Variable &var, void *val_
   ExtUI::setAxisSteps_per_mm(value, axis);
   DEBUG_ECHOLNPGM("value_set:", ExtUI::getAxisSteps_per_mm(axis));
   skipVP = var.VP; // don't overwrite value the next update time as the display might autoincrement in parallel
-  return;
 }
 
 void DGUSScreenHandler::HandleStepPerMMExtruderChanged(DGUS_VP_Variable &var, void *val_ptr) {
-  DEBUG_ECHOLNPGM("HandleStepPerMMExtruderChanged");
-
-  uint16_t value_raw = swap16(*(uint16_t*)val_ptr);
-  DEBUG_ECHOLNPGM("value_raw:", value_raw);
-  float value = (float)value_raw / 10;
+  const uint16_t value_raw = BE16_P(val_ptr);
+  DEBUG_ECHOLNPGM("HandleStepPerMMExtruderChanged:", value_raw);
+  const float value = (float)value_raw / 10;
   ExtUI::extruder_t extruder;
   switch (var.VP) {
     default: return;
@@ -575,7 +570,7 @@ void DGUSScreenHandler::HandleStepPerMMExtruderChanged(DGUS_VP_Variable &var, vo
   void DGUSScreenHandler::HandleProbeOffsetZChanged(DGUS_VP_Variable &var, void *val_ptr) {
     DEBUG_ECHOLNPGM("HandleProbeOffsetZChanged");
 
-    const float offset = float(int16_t(swap16(*(uint16_t*)val_ptr))) / 100.0f;
+    const float offset = float(int16_t(BE16_P(val_ptr))) / 100.0f;
     ExtUI::setZOffset_mm(offset);
     skipVP = var.VP; // don't overwrite value the next update time as the display might autoincrement in parallel
     return;
@@ -621,7 +616,7 @@ void DGUSScreenHandler::HandleHeaterControl(DGUS_VP_Variable &var, void *val_ptr
   void DGUSScreenHandler::HandlePreheat(DGUS_VP_Variable &var, void *val_ptr) {
     DEBUG_ECHOLNPGM("HandlePreheat");
 
-    const uint16_t preheat_option = swap16(*(uint16_t*)val_ptr);
+    const uint16_t preheat_option = BE16_P(val_ptr);
     switch (preheat_option) {
       default:
       switch (var.VP) {
@@ -644,7 +639,7 @@ void DGUSScreenHandler::HandleHeaterControl(DGUS_VP_Variable &var, void *val_ptr
 #if ENABLED(POWER_LOSS_RECOVERY)
 
   void DGUSScreenHandler::HandlePowerLossRecovery(DGUS_VP_Variable &var, void *val_ptr) {
-    uint16_t value = swap16(*(uint16_t*)val_ptr);
+    uint16_t value = BE16_P(val_ptr);
     if (value) {
       queue.inject(F("M1000"));
       dgusdisplay.WriteVariable(VP_SD_Print_Filename, filelist.filename(), 32, true);
diff --git a/Marlin/src/lcd/extui/dgus/DGUSScreenHandler.h b/Marlin/src/lcd/extui/dgus/DGUSScreenHandler.h
index 4b627fe0f6..575a71d289 100644
--- a/Marlin/src/lcd/extui/dgus/DGUSScreenHandler.h
+++ b/Marlin/src/lcd/extui/dgus/DGUSScreenHandler.h
@@ -42,6 +42,10 @@
 
 #endif
 
+// endianness swap
+#define BE16_P(V) ( ((uint8_t*)(V))[0] << 8U | ((uint8_t*)(V))[1] )
+#define BE32_P(V) ( ((uint8_t*)(V))[0] << 24U | ((uint8_t*)(V))[1] << 16U | ((uint8_t*)(V))[2] << 8U | ((uint8_t*)(V))[3] )
+
 #if ENABLED(DGUS_LCD_UI_ORIGIN)
   #include "origin/DGUSScreenHandler.h"
 #elif ENABLED(DGUS_LCD_UI_MKS)
diff --git a/Marlin/src/lcd/extui/dgus/mks/DGUSScreenHandler.cpp b/Marlin/src/lcd/extui/dgus/mks/DGUSScreenHandler.cpp
index 531788cc3f..36ab016b35 100644
--- a/Marlin/src/lcd/extui/dgus/mks/DGUSScreenHandler.cpp
+++ b/Marlin/src/lcd/extui/dgus/mks/DGUSScreenHandler.cpp
@@ -54,9 +54,6 @@
 bool DGUSAutoTurnOff = false;
 MKS_Language mks_language_index; // Initialized by settings.load()
 
-// endianness swap
-uint32_t swap32(const uint32_t value) { return (value & 0x000000FFU) << 24U | (value & 0x0000FF00U) << 8U | (value & 0x00FF0000U) >> 8U | (value & 0xFF000000U) >> 24U; }
-
 #if 0
 void DGUSScreenHandlerMKS::sendinfoscreen_ch(const uint16_t *line1, const uint16_t *line2, const uint16_t *line3, const uint16_t *line4) {
   dgusdisplay.WriteVariable(VP_MSGSTR1, line1, 32, true);
@@ -108,10 +105,10 @@ void DGUSScreenHandlerMKS::DGUSLCD_SendPrintTimeToDisplay(DGUS_VP_Variable &var)
 
 void DGUSScreenHandlerMKS::DGUSLCD_SetUint8(DGUS_VP_Variable &var, void *val_ptr) {
   if (var.memadr) {
-    const uint16_t value = swap16(*(uint16_t*)val_ptr);
-    DEBUG_ECHOLNPGM("FAN value get:", value);
+    const uint16_t value = BE16_P(val_ptr);
+    DEBUG_ECHOLNPGM("Got uint8:", value);
     *(uint8_t*)var.memadr = map(constrain(value, 0, 255), 0, 255, 0, 255);
-    DEBUG_ECHOLNPGM("FAN value change:", *(uint8_t*)var.memadr);
+    DEBUG_ECHOLNPGM("Set uint8:", *(uint8_t*)var.memadr);
   }
 }
 
@@ -152,7 +149,7 @@ void DGUSScreenHandlerMKS::DGUSLCD_SendTMCStepValue(DGUS_VP_Variable &var) {
 #if ENABLED(SDSUPPORT)
 
   void DGUSScreenHandler::DGUSLCD_SD_FileSelected(DGUS_VP_Variable &var, void *val_ptr) {
-    uint16_t touched_nr = (int16_t)swap16(*(uint16_t*)val_ptr) + top_file;
+    uint16_t touched_nr = (int16_t)BE16_P(val_ptr) + top_file;
     if (touched_nr != 0x0F && touched_nr > filelist.count()) return;
     if (!filelist.seek(touched_nr) && touched_nr != 0x0F) return;
 
@@ -191,7 +188,7 @@ void DGUSScreenHandlerMKS::DGUSLCD_SendTMCStepValue(DGUS_VP_Variable &var) {
   void DGUSScreenHandler::DGUSLCD_SD_ResumePauseAbort(DGUS_VP_Variable &var, void *val_ptr) {
 
     if (!ExtUI::isPrintingFromMedia()) return; // avoid race condition when user stays in this menu and printer finishes.
-    switch (swap16(*(uint16_t*)val_ptr)) {
+    switch (BE16_P(val_ptr)) {
       case 0: { // Resume
         auto cs = getCurrentScreen();
         if (runout_mks.runout_status != RUNOUT_WAITING_STATUS && runout_mks.runout_status != UNRUNOUT_STATUS) {
@@ -268,7 +265,7 @@ void DGUSScreenHandlerMKS::DGUSLCD_SendTMCStepValue(DGUS_VP_Variable &var) {
 
 #else
   void DGUSScreenHandlerMKS::PrintReturn(DGUS_VP_Variable& var, void *val_ptr) {
-    uint16_t value = swap16(*(uint16_t*)val_ptr);
+    const uint16_t value = BE16_P(val_ptr);
     if (value == 0x0F) GotoScreen(DGUSLCD_SCREEN_MAIN);
   }
 #endif // SDSUPPORT
@@ -315,7 +312,7 @@ void DGUSScreenHandler::ScreenChangeHook(DGUS_VP_Variable &var, void *val_ptr) {
 }
 
 void DGUSScreenHandlerMKS::ScreenBackChange(DGUS_VP_Variable &var, void *val_ptr) {
-  const uint16_t target = swap16(*(uint16_t *)val_ptr);
+  const uint16_t target = BE16_P(val_ptr);
   DEBUG_ECHOLNPGM(" back = 0x%x", target);
   switch (target) {
   }
@@ -331,7 +328,7 @@ void DGUSScreenHandlerMKS::ZoffsetConfirm(DGUS_VP_Variable &var, void *val_ptr)
 
 void DGUSScreenHandlerMKS::GetTurnOffCtrl(DGUS_VP_Variable &var, void *val_ptr) {
   DEBUG_ECHOLNPGM("GetTurnOffCtrl\n");
-  const uint16_t value = swap16(*(uint16_t *)val_ptr);
+  const uint16_t value = BE16_P(val_ptr);
   switch (value) {
     case 0 ... 1: DGUSAutoTurnOff = (bool)value; break;
     default: break;
@@ -340,7 +337,7 @@ void DGUSScreenHandlerMKS::GetTurnOffCtrl(DGUS_VP_Variable &var, void *val_ptr)
 
 void DGUSScreenHandlerMKS::GetMinExtrudeTemp(DGUS_VP_Variable &var, void *val_ptr) {
   DEBUG_ECHOLNPGM("GetMinExtrudeTemp");
-  const uint16_t value = swap16(*(uint16_t *)val_ptr);
+  const uint16_t value = BE16_P(val_ptr);
   TERN_(PREVENT_COLD_EXTRUSION, thermalManager.extrude_min_temp = value);
   mks_min_extrusion_temp = value;
   settings.save();
@@ -348,7 +345,7 @@ void DGUSScreenHandlerMKS::GetMinExtrudeTemp(DGUS_VP_Variable &var, void *val_pt
 
 void DGUSScreenHandlerMKS::GetZoffsetDistance(DGUS_VP_Variable &var, void *val_ptr) {
   DEBUG_ECHOLNPGM("GetZoffsetDistance");
-  const uint16_t value = swap16(*(uint16_t *)val_ptr);
+  const uint16_t value = BE16_P(val_ptr);
   float val_distance = 0;
   switch (value) {
     case 0: val_distance = 0.01; break;
@@ -362,11 +359,11 @@ void DGUSScreenHandlerMKS::GetZoffsetDistance(DGUS_VP_Variable &var, void *val_p
 
 void DGUSScreenHandlerMKS::GetManualMovestep(DGUS_VP_Variable &var, void *val_ptr) {
   DEBUG_ECHOLNPGM("\nGetManualMovestep");
-  *(uint16_t *)var.memadr = swap16(*(uint16_t *)val_ptr);
+  *(uint16_t *)var.memadr = BE16_P(val_ptr);
 }
 
 void DGUSScreenHandlerMKS::EEPROM_CTRL(DGUS_VP_Variable &var, void *val_ptr) {
-  const uint16_t eep_flag = swap16(*(uint16_t *)val_ptr);
+  const uint16_t eep_flag = BE16_P(val_ptr);
   switch (eep_flag) {
     case 0:
       settings.save();
@@ -384,7 +381,7 @@ void DGUSScreenHandlerMKS::EEPROM_CTRL(DGUS_VP_Variable &var, void *val_ptr) {
 }
 
 void DGUSScreenHandlerMKS::Z_offset_select(DGUS_VP_Variable &var, void *val_ptr) {
-  const uint16_t z_value = swap16(*(uint16_t *)val_ptr);
+  const uint16_t z_value = BE16_P(val_ptr);
   switch (z_value) {
     case 0: Z_distance = 0.01; break;
     case 1: Z_distance = 0.1; break;
@@ -396,22 +393,22 @@ void DGUSScreenHandlerMKS::Z_offset_select(DGUS_VP_Variable &var, void *val_ptr)
 void DGUSScreenHandlerMKS::GetOffsetValue(DGUS_VP_Variable &var, void *val_ptr) {
 
   #if HAS_BED_PROBE
-    int32_t value = swap32(*(int32_t *)val_ptr);
-    float Offset = value / 100.0f;
+    const int32_t value = BE32_P(val_ptr);
+    const float Offset = value / 100.0f;
     DEBUG_ECHOLNPGM("\nget int6 offset >> ", value, 6);
-  #endif
 
-  switch (var.VP) {
-    case VP_OFFSET_X: TERN_(HAS_BED_PROBE, probe.offset.x = Offset); break;
-    case VP_OFFSET_Y: TERN_(HAS_BED_PROBE, probe.offset.y = Offset); break;
-    case VP_OFFSET_Z: TERN_(HAS_BED_PROBE, probe.offset.z = Offset); break;
-    default: break;
-  }
-  settings.save();
+    switch (var.VP) {
+      default: break;
+        case VP_OFFSET_X: probe.offset.x = Offset; break;
+        case VP_OFFSET_Y: probe.offset.y = Offset; break;
+        case VP_OFFSET_Z: probe.offset.z = Offset; break;
+    }
+    settings.save();
+  #endif
 }
 
 void DGUSScreenHandlerMKS::LanguageChange(DGUS_VP_Variable &var, void *val_ptr) {
-  const uint16_t lag_flag = swap16(*(uint16_t *)val_ptr);
+  const uint16_t lag_flag = BE16_P(val_ptr);
   switch (lag_flag) {
     case MKS_SimpleChinese:
       DGUS_LanguageDisplay(MKS_SimpleChinese);
@@ -436,10 +433,10 @@ void DGUSScreenHandlerMKS::LanguageChange(DGUS_VP_Variable &var, void *val_ptr)
 #endif
 
 void DGUSScreenHandlerMKS::Level_Ctrl(DGUS_VP_Variable &var, void *val_ptr) {
-  const uint16_t lev_but = swap16(*(uint16_t *)val_ptr);
   #if ENABLED(MESH_BED_LEVELING)
     auto cs = getCurrentScreen();
   #endif
+  const uint16_t lev_but = BE16_P(val_ptr);
   switch (lev_but) {
     case 0:
       #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
@@ -483,7 +480,7 @@ void DGUSScreenHandlerMKS::Level_Ctrl(DGUS_VP_Variable &var, void *val_ptr) {
 }
 
 void DGUSScreenHandlerMKS::MeshLevelDistanceConfig(DGUS_VP_Variable &var, void *val_ptr) {
-  const uint16_t mesh_dist = swap16(*(uint16_t *)val_ptr);
+  const uint16_t mesh_dist = BE16_P(val_ptr);
   switch (mesh_dist) {
     case 0: mesh_adj_distance = 0.01; break;
     case 1: mesh_adj_distance = 0.1; break;
@@ -494,7 +491,7 @@ void DGUSScreenHandlerMKS::MeshLevelDistanceConfig(DGUS_VP_Variable &var, void *
 
 void DGUSScreenHandlerMKS::MeshLevel(DGUS_VP_Variable &var, void *val_ptr) {
   #if ENABLED(MESH_BED_LEVELING)
-    const uint16_t mesh_value = swap16(*(uint16_t *)val_ptr);
+    const uint16_t mesh_value = BE16_P(val_ptr);
     // static uint8_t a_first_level = 1;
     char cmd_buf[30];
     float offset = mesh_adj_distance;
@@ -592,8 +589,8 @@ void DGUSScreenHandlerMKS::SD_FileBack(DGUS_VP_Variable&, void*) {
 }
 
 void DGUSScreenHandlerMKS::LCD_BLK_Adjust(DGUS_VP_Variable &var, void *val_ptr) {
-  const uint16_t lcd_value = swap16(*(uint16_t *)val_ptr);
 
+  const uint16_t lcd_value = BE16_P(val_ptr);
   lcd_default_light = constrain(lcd_value, 10, 100);
 
   const uint16_t lcd_data[2] = { lcd_default_light, lcd_default_light };
@@ -601,7 +598,7 @@ void DGUSScreenHandlerMKS::LCD_BLK_Adjust(DGUS_VP_Variable &var, void *val_ptr)
 }
 
 void DGUSScreenHandlerMKS::ManualAssistLeveling(DGUS_VP_Variable &var, void *val_ptr) {
-  const int16_t point_value = swap16(*(uint16_t *)val_ptr);
+  const int16_t point_value = BE16_P(val_ptr);
 
   // Insist on leveling first time at this screen
   static bool first_level_flag = false;
@@ -655,7 +652,7 @@ void DGUSScreenHandlerMKS::ManualAssistLeveling(DGUS_VP_Variable &var, void *val
 #define mks_max(a, b) ((a) > (b)) ? (a) : (b)
 void DGUSScreenHandlerMKS::TMC_ChangeConfig(DGUS_VP_Variable &var, void *val_ptr) {
   #if EITHER(HAS_TRINAMIC_CONFIG, HAS_STEALTHCHOP)
-    const uint16_t tmc_value = swap16(*(uint16_t*)val_ptr);
+    const uint16_t tmc_value = BE16_P(val_ptr);
   #endif
 
   switch (var.VP) {
@@ -748,7 +745,7 @@ void DGUSScreenHandlerMKS::TMC_ChangeConfig(DGUS_VP_Variable &var, void *val_ptr
 void DGUSScreenHandler::HandleManualMove(DGUS_VP_Variable &var, void *val_ptr) {
   DEBUG_ECHOLNPGM("HandleManualMove");
 
-  int16_t movevalue = swap16(*(uint16_t*)val_ptr);
+  int16_t movevalue = BE16_P(val_ptr);
 
   // Choose Move distance
        if (manualMoveStep == 0x01) manualMoveStep =   10;
@@ -893,7 +890,7 @@ void DGUSScreenHandler::HandleManualMove(DGUS_VP_Variable &var, void *val_ptr) {
 }
 
 void DGUSScreenHandlerMKS::GetParkPos(DGUS_VP_Variable &var, void *val_ptr) {
-  const int16_t value_pos = swap16(*(int16_t*)val_ptr);
+  const int16_t value_pos = BE16_P(val_ptr);
 
   switch (var.VP) {
     case VP_X_PARK_POS: mks_park_pos.x = value_pos; break;
@@ -907,7 +904,7 @@ void DGUSScreenHandlerMKS::GetParkPos(DGUS_VP_Variable &var, void *val_ptr) {
 void DGUSScreenHandlerMKS::HandleChangeLevelPoint(DGUS_VP_Variable &var, void *val_ptr) {
   DEBUG_ECHOLNPGM("HandleChangeLevelPoint");
 
-  const int16_t value_raw = swap16(*(int16_t*)val_ptr);
+  const int16_t value_raw = BE16_P(val_ptr);
   DEBUG_ECHOLNPGM("value_raw:", value_raw);
 
   *(int16_t*)var.memadr = value_raw;
@@ -919,7 +916,7 @@ void DGUSScreenHandlerMKS::HandleChangeLevelPoint(DGUS_VP_Variable &var, void *v
 void DGUSScreenHandlerMKS::HandleStepPerMMChanged(DGUS_VP_Variable &var, void *val_ptr) {
   DEBUG_ECHOLNPGM("HandleStepPerMMChanged");
 
-  const uint16_t value_raw = swap16(*(uint16_t*)val_ptr);
+  const uint16_t value_raw = BE16_P(val_ptr);
   const float value = (float)value_raw;
 
   DEBUG_ECHOLNPGM("value_raw:", value_raw);
@@ -941,7 +938,7 @@ void DGUSScreenHandlerMKS::HandleStepPerMMChanged(DGUS_VP_Variable &var, void *v
 void DGUSScreenHandlerMKS::HandleStepPerMMExtruderChanged(DGUS_VP_Variable &var, void *val_ptr) {
   DEBUG_ECHOLNPGM("HandleStepPerMMExtruderChanged");
 
-  const uint16_t value_raw = swap16(*(uint16_t*)val_ptr);
+  const uint16_t value_raw = BE16_P(val_ptr);
   const float value = (float)value_raw;
 
   DEBUG_ECHOLNPGM("value_raw:", value_raw);
@@ -966,7 +963,7 @@ void DGUSScreenHandlerMKS::HandleStepPerMMExtruderChanged(DGUS_VP_Variable &var,
 void DGUSScreenHandlerMKS::HandleMaxSpeedChange(DGUS_VP_Variable &var, void *val_ptr) {
   DEBUG_ECHOLNPGM("HandleMaxSpeedChange");
 
-  const uint16_t value_raw = swap16(*(uint16_t*)val_ptr);
+  const uint16_t value_raw = BE16_P(val_ptr);
   const float value = (float)value_raw;
 
   DEBUG_ECHOLNPGM("value_raw:", value_raw);
@@ -988,7 +985,7 @@ void DGUSScreenHandlerMKS::HandleMaxSpeedChange(DGUS_VP_Variable &var, void *val
 void DGUSScreenHandlerMKS::HandleExtruderMaxSpeedChange(DGUS_VP_Variable &var, void *val_ptr) {
   DEBUG_ECHOLNPGM("HandleExtruderMaxSpeedChange");
 
-  const uint16_t value_raw = swap16(*(uint16_t*)val_ptr);
+  const uint16_t value_raw = BE16_P(val_ptr);
   const float value = (float)value_raw;
 
   DEBUG_ECHOLNPGM("value_raw:", value_raw);
@@ -1013,7 +1010,7 @@ void DGUSScreenHandlerMKS::HandleExtruderMaxSpeedChange(DGUS_VP_Variable &var, v
 void DGUSScreenHandlerMKS::HandleMaxAccChange(DGUS_VP_Variable &var, void *val_ptr) {
   DEBUG_ECHOLNPGM("HandleMaxAccChange");
 
-  const uint16_t value_raw = swap16(*(uint16_t*)val_ptr);
+  const uint16_t value_raw = BE16_P(val_ptr);
   const float value = (float)value_raw;
 
   DEBUG_ECHOLNPGM("value_raw:", value_raw);
@@ -1035,7 +1032,7 @@ void DGUSScreenHandlerMKS::HandleMaxAccChange(DGUS_VP_Variable &var, void *val_p
 void DGUSScreenHandlerMKS::HandleExtruderAccChange(DGUS_VP_Variable &var, void *val_ptr) {
   DEBUG_ECHOLNPGM("HandleExtruderAccChange");
 
-  uint16_t value_raw = swap16(*(uint16_t*)val_ptr);
+  uint16_t value_raw = BE16_P(val_ptr);
   DEBUG_ECHOLNPGM("value_raw:", value_raw);
   float value = (float)value_raw;
   ExtUI::extruder_t extruder;
@@ -1056,32 +1053,32 @@ void DGUSScreenHandlerMKS::HandleExtruderAccChange(DGUS_VP_Variable &var, void *
 }
 
 void DGUSScreenHandlerMKS::HandleTravelAccChange(DGUS_VP_Variable &var, void *val_ptr) {
-  uint16_t value_travel = swap16(*(uint16_t*)val_ptr);
+  uint16_t value_travel = BE16_P(val_ptr);
   planner.settings.travel_acceleration = (float)value_travel;
   skipVP = var.VP; // don't overwrite value the next update time as the display might autoincrement in parallel
 }
 
 void DGUSScreenHandlerMKS::HandleFeedRateMinChange(DGUS_VP_Variable &var, void *val_ptr) {
-  uint16_t value_t = swap16(*(uint16_t*)val_ptr);
+  uint16_t value_t = BE16_P(val_ptr);
   planner.settings.min_feedrate_mm_s = (float)value_t;
   skipVP = var.VP; // don't overwrite value the next update time as the display might autoincrement in parallel
 }
 
 void DGUSScreenHandlerMKS::HandleMin_T_F(DGUS_VP_Variable &var, void *val_ptr) {
-  uint16_t value_t_f = swap16(*(uint16_t*)val_ptr);
+  uint16_t value_t_f = BE16_P(val_ptr);
   planner.settings.min_travel_feedrate_mm_s = (float)value_t_f;
   skipVP = var.VP; // don't overwrite value the next update time as the display might autoincrement in parallel
 }
 
 void DGUSScreenHandlerMKS::HandleAccChange(DGUS_VP_Variable &var, void *val_ptr) {
-  uint16_t value_acc = swap16(*(uint16_t*)val_ptr);
+  uint16_t value_acc = BE16_P(val_ptr);
   planner.settings.acceleration = (float)value_acc;
   skipVP = var.VP; // don't overwrite value the next update time as the display might autoincrement in parallel
 }
 
 #if ENABLED(PREVENT_COLD_EXTRUSION)
   void DGUSScreenHandlerMKS::HandleGetExMinTemp(DGUS_VP_Variable &var, void *val_ptr) {
-    const uint16_t value_ex_min_temp = swap16(*(uint16_t*)val_ptr);
+    const uint16_t value_ex_min_temp = BE16_P(val_ptr);
     thermalManager.extrude_min_temp = value_ex_min_temp;
     skipVP = var.VP; // don't overwrite value the next update time as the display might autoincrement in parallel
   }
@@ -1089,7 +1086,7 @@ void DGUSScreenHandlerMKS::HandleAccChange(DGUS_VP_Variable &var, void *val_ptr)
 
 #if HAS_PID_HEATING
   void DGUSScreenHandler::HandleTemperaturePIDChanged(DGUS_VP_Variable &var, void *val_ptr) {
-    const uint16_t rawvalue = swap16(*(uint16_t*)val_ptr);
+    const uint16_t rawvalue = BE16_P(val_ptr);
     DEBUG_ECHOLNPGM("V1:", rawvalue);
     const float value = 1.0f * rawvalue;
     DEBUG_ECHOLNPGM("V2:", value);
@@ -1125,9 +1122,9 @@ void DGUSScreenHandlerMKS::HandleAccChange(DGUS_VP_Variable &var, void *val_ptr)
 #if ENABLED(BABYSTEPPING)
   void DGUSScreenHandler::HandleLiveAdjustZ(DGUS_VP_Variable &var, void *val_ptr) {
     DEBUG_ECHOLNPGM("HandleLiveAdjustZ");
-    float step = ZOffset_distance;
+    const float step = ZOffset_distance;
 
-    uint16_t flag = swap16(*(uint16_t*)val_ptr);
+    const uint16_t flag = BE16_P(val_ptr);
     switch (flag) {
       case 0:
         if (step == 0.01)
@@ -1159,34 +1156,26 @@ void DGUSScreenHandlerMKS::HandleAccChange(DGUS_VP_Variable &var, void *val_ptr)
         z_offset_add += ZOffset_distance;
         break;
 
-      default:
-        break;
+      default: break;
     }
     ForceCompleteUpdate();
   }
 #endif // BABYSTEPPING
 
 void DGUSScreenHandlerMKS::GetManualFilament(DGUS_VP_Variable &var, void *val_ptr) {
-  DEBUG_ECHOLNPGM("GetManualFilament");
+  const uint16_t value_len = BE16_P(val_ptr);
+  const float value = (float)value_len;
 
-  uint16_t value_len = swap16(*(uint16_t*)val_ptr);
-
-  float value = (float)value_len;
-
-  DEBUG_ECHOLNPGM("Get Filament len value:", value);
+  DEBUG_ECHOLNPGM("GetManualFilament:", value);
   distanceFilament = value;
 
   skipVP = var.VP; // don't overwrite value the next update time as the display might autoincrement in parallel
 }
 
 void DGUSScreenHandlerMKS::GetManualFilamentSpeed(DGUS_VP_Variable &var, void *val_ptr) {
-  DEBUG_ECHOLNPGM("GetManualFilamentSpeed");
-
-  uint16_t value_len = swap16(*(uint16_t*)val_ptr);
-
-  DEBUG_ECHOLNPGM("filamentSpeed_mm_s value:", value_len);
-
+  const uint16_t value_len = BE16_P(val_ptr);
   filamentSpeed_mm_s = value_len;
+  DEBUG_ECHOLNPGM("GetManualFilamentSpeed:", value_len);
 
   skipVP = var.VP; // don't overwrite value the next update time as the display might autoincrement in parallel
 }
@@ -1205,7 +1194,7 @@ void DGUSScreenHandlerMKS::FilamentLoadUnload(DGUS_VP_Variable &var, void *val_p
   if (!print_job_timer.isPaused() && !queue.ring_buffer.empty())
     return;
 
-  const uint16_t val_t = swap16(*(uint16_t*)val_ptr);
+  const uint16_t val_t = BE16_P(val_ptr);
   switch (val_t) {
     default: break;
     case 0:
@@ -1291,7 +1280,7 @@ void DGUSScreenHandlerMKS::FilamentUnLoad(DGUS_VP_Variable &var, void *val_ptr)
 
     uint8_t e_temp = 0;
     filament_data.heated = false;
-    uint16_t preheat_option = swap16(*(uint16_t*)val_ptr);
+    uint16_t preheat_option = BE16_P(val_ptr);
     if (preheat_option >= 10) {     // Unload filament type
       preheat_option -= 10;
       filament_data.action = 2;
diff --git a/Marlin/src/lcd/extui/dgus_reloaded/DGUSDisplay.h b/Marlin/src/lcd/extui/dgus_reloaded/DGUSDisplay.h
index fa5bf30396..d115f7c02b 100644
--- a/Marlin/src/lcd/extui/dgus_reloaded/DGUSDisplay.h
+++ b/Marlin/src/lcd/extui/dgus_reloaded/DGUSDisplay.h
@@ -21,7 +21,10 @@
  */
 #pragma once
 
-/* DGUS implementation written by coldtobi in 2019 for Marlin */
+/**
+ * DGUS implementation written by coldtobi in 2019.
+ * Updated for STM32G0B1RE by Protomosh in 2022.
+ */
 
 #include "config/DGUS_Screen.h"
 #include "config/DGUS_Control.h"
@@ -30,11 +33,13 @@
 #include "../../../inc/MarlinConfigPre.h"
 #include "../../../MarlinCore.h"
 
+#define DEBUG_DGUSLCD // Uncomment for debug messages
 #define DEBUG_OUT ENABLED(DEBUG_DGUSLCD)
 #include "../../../core/debug_out.h"
 
-#define Swap16(val) ((uint16_t)(((uint16_t)(val) >> 8) |\
-                                ((uint16_t)(val) << 8)))
+// New endianness swap for 32bit mcu (tested with STM32G0B1RE)
+#define BE16_P(V) ( ((uint8_t*)(V))[0] << 8U | ((uint8_t*)(V))[1] )
+#define BE32_P(V) ( ((uint8_t*)(V))[0] << 24U | ((uint8_t*)(V))[1] << 16U | ((uint8_t*)(V))[2] << 8U | ((uint8_t*)(V))[3] )
 
 // Low-Level access to the display.
 class DGUSDisplay {
diff --git a/Marlin/src/lcd/extui/dgus_reloaded/DGUSRxHandler.cpp b/Marlin/src/lcd/extui/dgus_reloaded/DGUSRxHandler.cpp
index 88fe30a027..ce03ab6b83 100644
--- a/Marlin/src/lcd/extui/dgus_reloaded/DGUSRxHandler.cpp
+++ b/Marlin/src/lcd/extui/dgus_reloaded/DGUSRxHandler.cpp
@@ -215,7 +215,7 @@ void DGUSRxHandler::PrintResume(DGUS_VP &vp, void *data_ptr) {
 void DGUSRxHandler::Feedrate(DGUS_VP &vp, void *data_ptr) {
   UNUSED(vp);
 
-  const int16_t feedrate = Swap16(*(int16_t*)data_ptr);
+  const int16_t feedrate = BE16_P(data_ptr);
 
   ExtUI::setFeedrate_percent(feedrate);
 
@@ -223,7 +223,7 @@ void DGUSRxHandler::Feedrate(DGUS_VP &vp, void *data_ptr) {
 }
 
 void DGUSRxHandler::Flowrate(DGUS_VP &vp, void *data_ptr) {
-  const int16_t flowrate = Swap16(*(int16_t*)data_ptr);
+  const int16_t flowrate = BE16_P(data_ptr);
 
   switch (vp.addr) {
     default: return;
@@ -246,7 +246,7 @@ void DGUSRxHandler::Flowrate(DGUS_VP &vp, void *data_ptr) {
 void DGUSRxHandler::BabystepSet(DGUS_VP &vp, void *data_ptr) {
   UNUSED(vp);
 
-  const int16_t data = Swap16(*(int16_t*)data_ptr);
+  const int16_t data = BE16_P(data_ptr);
   const float offset = dgus_display.FromFixedPoint<int16_t, float, 2>(data);
 
   const int16_t steps = ExtUI::mmToWholeSteps(offset - ExtUI::getZOffset_mm(), ExtUI::Z);
@@ -315,7 +315,7 @@ void DGUSRxHandler::TempPreset(DGUS_VP &vp, void *data_ptr) {
 }
 
 void DGUSRxHandler::TempTarget(DGUS_VP &vp, void *data_ptr) {
-  const int16_t temp = Swap16(*(int16_t*)data_ptr);
+  const int16_t temp = BE16_P(data_ptr);
 
   switch (vp.addr) {
     default: return;
@@ -338,7 +338,7 @@ void DGUSRxHandler::TempTarget(DGUS_VP &vp, void *data_ptr) {
 void DGUSRxHandler::TempCool(DGUS_VP &vp, void *data_ptr) {
   UNUSED(vp);
 
-  const DGUS_Data::Heater heater = (DGUS_Data::Heater)Swap16(*(uint16_t*)data_ptr);
+  const DGUS_Data::Heater heater = (DGUS_Data::Heater)BE16_P(data_ptr);
 
   switch (heater) {
     default: return;
@@ -397,7 +397,7 @@ void DGUSRxHandler::ZOffset(DGUS_VP &vp, void *data_ptr) {
     return;
   }
 
-  const int16_t data = Swap16(*(int16_t*)data_ptr);
+  const int16_t data = BE16_P(data_ptr);
   const float offset = dgus_display.FromFixedPoint<int16_t, float, 2>(data);
 
   const int16_t steps = ExtUI::mmToWholeSteps(offset - ExtUI::getZOffset_mm(), ExtUI::Z);
@@ -546,7 +546,7 @@ void DGUSRxHandler::DisableABL(DGUS_VP &vp, void *data_ptr) {
 void DGUSRxHandler::FilamentSelect(DGUS_VP &vp, void *data_ptr) {
   UNUSED(vp);
 
-  const DGUS_Data::Extruder extruder = (DGUS_Data::Extruder)Swap16(*(uint16_t*)data_ptr);
+  const DGUS_Data::Extruder extruder = (DGUS_Data::Extruder)BE16_P(data_ptr);
 
   switch (extruder) {
     default: return;
@@ -563,7 +563,7 @@ void DGUSRxHandler::FilamentSelect(DGUS_VP &vp, void *data_ptr) {
 void DGUSRxHandler::FilamentLength(DGUS_VP &vp, void *data_ptr) {
   UNUSED(vp);
 
-  const uint16_t length = Swap16(*(uint16_t*)data_ptr);
+  const uint16_t length = BE16_P(data_ptr);
 
   dgus_screen_handler.filament_length = constrain(length, 0, EXTRUDE_MAXLENGTH);
 
@@ -644,7 +644,7 @@ void DGUSRxHandler::Home(DGUS_VP &vp, void *data_ptr) {
 }
 
 void DGUSRxHandler::Move(DGUS_VP &vp, void *data_ptr) {
-  const int16_t data = Swap16(*(int16_t*)data_ptr);
+  const int16_t data = BE16_P(data_ptr);
   const float position = dgus_display.FromFixedPoint<int16_t, float, 1>(data);
   ExtUI::axis_t axis;
 
@@ -816,7 +816,7 @@ void DGUSRxHandler::SettingsExtra(DGUS_VP &vp, void *data_ptr) {
 void DGUSRxHandler::PIDSelect(DGUS_VP &vp, void *data_ptr) {
   UNUSED(vp);
 
-  const DGUS_Data::Heater heater = (DGUS_Data::Heater)Swap16(*(uint16_t*)data_ptr);
+  const DGUS_Data::Heater heater = (DGUS_Data::Heater)BE16_P(data_ptr);
 
   switch (heater) {
     default: return;
@@ -846,7 +846,7 @@ void DGUSRxHandler::PIDSetTemp(DGUS_VP &vp, void *data_ptr) {
     return;
   }
 
-  uint16_t temp = Swap16(*(uint16_t*)data_ptr);
+  uint16_t temp = BE16_P(data_ptr);
 
   switch (dgus_screen_handler.pid_heater) {
     default: return;
diff --git a/Marlin/src/lcd/extui/dgus_reloaded/DGUSRxHandler.h b/Marlin/src/lcd/extui/dgus_reloaded/DGUSRxHandler.h
index c2e6e4308e..4cad11fc0b 100644
--- a/Marlin/src/lcd/extui/dgus_reloaded/DGUSRxHandler.h
+++ b/Marlin/src/lcd/extui/dgus_reloaded/DGUSRxHandler.h
@@ -107,7 +107,7 @@ namespace DGUSRxHandler {
         break;
       }
       case 2: {
-        const uint16_t data = Swap16(*(uint16_t*)data_ptr);
+        const uint16_t data = BE16_P(data_ptr);
         *(T*)vp.extra = (T)data;
         break;
       }
diff --git a/Marlin/src/lcd/extui/dgus_reloaded/DGUSTxHandler.h b/Marlin/src/lcd/extui/dgus_reloaded/DGUSTxHandler.h
index 94632fe385..7d1b46773b 100644
--- a/Marlin/src/lcd/extui/dgus_reloaded/DGUSTxHandler.h
+++ b/Marlin/src/lcd/extui/dgus_reloaded/DGUSTxHandler.h
@@ -24,6 +24,8 @@
 #include "DGUSDisplay.h"
 #include "definition/DGUS_VP.h"
 
+#define Swap16(val) ((uint16_t)(((uint16_t)(val) >> 8) | ((uint16_t)(val) << 8)))
+
 namespace DGUSTxHandler {
 
   #if ENABLED(SDSUPPORT)