From a121f665f1755512c1e108275640b5be8bd18b39 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Thu, 6 Jul 2023 00:54:52 +1000
Subject: [PATCH] AP_Scripting: stash and restore FPU register context in
 LUAI_TRY

this fixes an issue where a lua library function triggers an exception
after it does a math operation which changes the floating point
registers on M7 MCUs (such as STM32H7). An example is math.random()
which calls math_random(), which pre-calculates a double value before
checking if the arguments to the call are valid. When it then checks
and finds invalid values the exception longjmp does not restore the
floating point registers.
---
 libraries/AP_Scripting/AP_Scripting.cpp | 10 ++++++++++
 libraries/AP_Scripting/lua/src/ldo.c    | 12 ++++++++++--
 2 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp
index 51484e8153..3400af3085 100644
--- a/libraries/AP_Scripting/AP_Scripting.cpp
+++ b/libraries/AP_Scripting/AP_Scripting.cpp
@@ -218,6 +218,15 @@ void AP_Scripting::repl_stop(void) {
     // can't do any more cleanup here, closing the open FD's is the REPL's responsibility
 }
 
+/*
+  avoid optimisation of the thread function. This avoids nasty traps
+  where setjmp/longjmp does not properly handle save/restore of
+  floating point registers on exceptions. This is an extra protection
+  over the top of the fix in luaD_rawrunprotected() for the same issue
+ */
+#pragma GCC push_options
+#pragma GCC optimize ("O0")
+
 void AP_Scripting::thread(void) {
     while (true) {
         // reset flags
@@ -275,6 +284,7 @@ void AP_Scripting::thread(void) {
         }
     }
 }
+#pragma GCC pop_options
 
 void AP_Scripting::handle_mission_command(const AP_Mission::Mission_Command& cmd_in)
 {
diff --git a/libraries/AP_Scripting/lua/src/ldo.c b/libraries/AP_Scripting/lua/src/ldo.c
index 328a497f7b..88b9d921dd 100644
--- a/libraries/AP_Scripting/lua/src/ldo.c
+++ b/libraries/AP_Scripting/lua/src/ldo.c
@@ -133,7 +133,9 @@ l_noret luaD_throw (lua_State *L, int errcode) {
   }
 }
 
-
+// remove optimization
+#pragma GCC push_options
+#pragma GCC optimize ("O0")
 int luaD_rawrunprotected (lua_State *L, Pfunc f, void *ud) {
   unsigned short oldnCcalls = L->nCcalls;
   struct lua_longjmp lj;
@@ -141,13 +143,19 @@ int luaD_rawrunprotected (lua_State *L, Pfunc f, void *ud) {
   lj.previous = L->errorJmp;  /* chain new error handler */
   L->errorJmp = &lj;
   LUAI_TRY(L, &lj,
+#ifdef ARM_MATH_CM7
+    __asm__("vpush {s16-s31}");
+#endif
     (*f)(L, ud);
   );
+#ifdef ARM_MATH_CM7
+  __asm__("vpop {s16-s31}");
+#endif
   L->errorJmp = lj.previous;  /* restore old error handler */
   L->nCcalls = oldnCcalls;
   return lj.status;
 }
-
+#pragma GCC pop_options
 /* }====================================================== */