AP_Common: add Cortex-M-specific working setjmp implementation

Tools/ardupilotwaf/chibios.py

@@ -723,7 +723,8 @@ def build(bld):
                  'fopen', 'fflush', 'fwrite', 'fread', 'fputs', 'fgets',
                  'clearerr', 'fseek', 'ferror', 'fclose', 'tmpfile', 'getc', 'ungetc', 'feof',
                 'ftell', 'freopen', 'remove', 'vfprintf', 'vfprintf_r', 'fscanf',
-                '_gettimeofday', '_times', '_times_r', '_gettimeofday_r', 'time', 'clock']
+                '_gettimeofday', '_times', '_times_r', '_gettimeofday_r', 'time', 'clock',
+                'setjmp']
 
     # these functions use global state that is not thread safe
     blacklist += ['gmtime']

libraries/AP_Common/ap_setjmp.c

@@ -0,0 +1,29 @@
+#include <AP_Common/ap_setjmp.h>
+
+#if AP_SETJMP_CORTEX_ENABLE
+
+int __attribute__((naked, noinline)) ap_setjmp(ap_jmp_buf env) {
+    __asm__(
+        "mov ip, sp\n\t" // ip must be free on a function call; can't store sp
+        "stmia r0!, {r4-r10, fp, ip, lr}\n\t" // store 10 regular registers
+        "vstm r0, {d8-d15}\n\t" // store 16 FP registers. use 8 byte store for
+                                // speed as our buffer is 8 byte aligned
+        "movs r0, #0\n\t" // return 0
+        "bx lr\n\t" // do return
+    );
+}
+
+void __attribute__((noreturn, naked, noinline)) ap_longjmp(ap_jmp_buf env, int val) {
+    __asm__(
+        "ldmia r0!, {r4-r10, fp, ip, lr}\n\t" // load 10 regular registers
+        "mov sp, ip\n\t" // restore stack pointer from scratch register
+        "vldm r0, {d8-d15}\n\t" // load 16 FP registers. use 8 byte load for
+                                // speed as our buffer is 8 byte aligned
+        "movs r0, r1\n\t" // prepare return of value
+        "it eq\n\t" // but if value is zero
+        "moveq r0, #1\n\t" // return one
+        "bx lr\n\t" // do return
+    );
+}
+
+#endif

libraries/AP_Common/ap_setjmp.h

@@ -0,0 +1,53 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <setjmp.h>
+
+// setjmp with platform-specific implementations.
+
+// older newlib (in particular that shipped with ARM GCC 10.3.1) does not save
+// the callee-saved floating point registers. if we detect a situation where we
+// may be using such a newlib and such registers are available, use our own
+// implementation which actually saves them. these cannot be wrappers as we
+// cannot safely use the stack and have to expand the jmp_buf.
+#if (defined(_NEWLIB_VERSION) && (defined(__ARM_FP) || defined(__ARM_FEATURE_MVE)))
+#define AP_SETJMP_CORTEX_ENABLE 1
+#else
+#define AP_SETJMP_CORTEX_ENABLE 0
+#endif
+
+#if AP_SETJMP_CORTEX_ENABLE
+
+#include <inttypes.h>
+
+// 20 8-byte words is defined by
+// https://github.com/ARM-software/abi-aa/blob/c51addc3dc03e73a016a1e4edf25440bcac76431/clibabi32/clibabi32.rst#setjmp-h
+// but we only need 10 regular registers and 16 FP registers meaning 13 8 byte
+// words so we can save a bit of memory. switching back to the official setjmp
+// implementation will re-consume it though. Lua keeps these on the C stack.
+
+struct _ap_jmp_buf {
+    int64_t data[13];
+};
+
+// hack to allow ap_jmp_buf to decay to a pointer
+typedef struct _ap_jmp_buf ap_jmp_buf[1];
+
+int __attribute__((naked, noinline)) ap_setjmp(ap_jmp_buf env);
+void __attribute__((noreturn, naked, noinline)) ap_longjmp(ap_jmp_buf env, int val);
+
+#else
+
+#define ap_setjmp(env) (setjmp(env))
+#define ap_longjmp(env, val) (longjmp(env, val))
+
+typedef jmp_buf ap_jmp_buf;
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif

libraries/AP_Scripting/AP_Scripting.cpp

@@ -294,15 +294,6 @@ MAV_RESULT AP_Scripting::handle_command_int_packet(const mavlink_command_int_t &
 }
 #endif
 
-/*
-  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
@@ -398,7 +389,6 @@ void AP_Scripting::thread(void) {
         }
     }
 }
-#pragma GCC pop_options
 
 void AP_Scripting::handle_mission_command(const AP_Mission::Mission_Command& cmd_in)
 {

libraries/AP_Scripting/lua/src/ldo.c

@@ -10,7 +10,7 @@
 #include "lprefix.h"
 
 
-#include <setjmp.h>
+#include <AP_Common/ap_setjmp.h>
 #include <stdlib.h>
 #include <string.h>
 
@@ -71,9 +71,9 @@
 #else							/* }{ */
 
 /* ISO C handling with long jumps */
-#define LUAI_THROW(L,c)		longjmp((c)->b, 1)
-#define LUAI_TRY(L,c,a)		if (setjmp((c)->b) == 0) { a }
-#define luai_jmpbuf		jmp_buf
+#define LUAI_THROW(L,c)		ap_longjmp((c)->b, 1)
+#define LUAI_TRY(L,c,a)		if (ap_setjmp((c)->b) == 0) { a }
+#define luai_jmpbuf		ap_jmp_buf
 
 #endif							/* } */
 
@@ -133,29 +133,21 @@ 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;
   lj.status = LUA_OK;
   lj.previous = L->errorJmp;  /* chain new error handler */
   L->errorJmp = &lj;
-#ifdef ARM_MATH_CM7
-    __asm__("vpush {s16-s31}");
-#endif
   LUAI_TRY(L, &lj,
     (*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
+
 /* }====================================================== */
 
 

libraries/AP_Scripting/lua_scripts.cpp

@@ -30,7 +30,7 @@ extern const AP_HAL::HAL& hal;
 #define ENABLE_DEBUG_MODULE 0
 
 bool lua_scripts::overtime;
-jmp_buf lua_scripts::panic_jmp;
+ap_jmp_buf lua_scripts::panic_jmp;
 char *lua_scripts::error_msg_buf;
 HAL_Semaphore lua_scripts::error_msg_buf_sem;
 uint8_t lua_scripts::print_error_count;
@@ -132,7 +132,7 @@ void lua_scripts::set_and_print_new_error_message(MAV_SEVERITY severity, const c
 
 int lua_scripts::atpanic(lua_State *L) {
     set_and_print_new_error_message(MAV_SEVERITY_CRITICAL, "Panic: %s", get_error_object_message(L));
-    longjmp(panic_jmp, 1);
+    ap_longjmp(panic_jmp, 1);
     return 0;
 }
 
@@ -480,7 +480,7 @@ void lua_scripts::run(void) {
     }
 
     // panic should be hooked first
-    if (setjmp(panic_jmp)) {
+    if (ap_setjmp(panic_jmp)) {
         if (!succeeded_initial_load) {
             return;
         }

libraries/AP_Scripting/lua_scripts.h

@@ -20,7 +20,7 @@
 
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
-#include <setjmp.h>
+#include <AP_Common/ap_setjmp.h>
 
 #include <AP_Filesystem/posix_compat.h>
 #include <AP_Scripting/AP_Scripting.h>
@@ -82,7 +82,7 @@ class lua_scripts
 
     // lua panic handler, will jump back to the start of run
     static int atpanic(lua_State *L);
-    static jmp_buf panic_jmp;
+    static ap_jmp_buf panic_jmp;
 
     lua_State *lua_state;