FOSA-PaRTiKle: fixes in long jumps

[frescor/fosa.git] / src_partikle / fosa_long_jump.c

diff --git a/src_partikle/fosa_long_jump.c b/src_partikle/fosa_long_jump.c
index e59a69ff5d4a3dabaf2d03932179370bb749e4dd..0868ec76182ad6fac072f66ab05d3ec241491f9c 100644 (file)
--- a/src_partikle/fosa_long_jump.c
+++ b/src_partikle/fosa_long_jump.c
@@ -69,7 +69,7 @@
 #include <stdio.h>
 
 #define LONGJMP_MAGIC 0x01234567
-int jmp_used_signals [LONGJMP_NSIG] = {[0 ... (LONGJMP_NSIG - 1)] = 0};
+pthread_t jmp_used_signals [LONGJMP_NSIG] = {[0 ... (LONGJMP_NSIG - 1)] = NULL};
 pthread_mutex_t signal_pool_m = PTHREAD_MUTEX_INITIALIZER;
 
 extern void fosa_longjmp (fosa_long_jump_context_t ctx, unsigned long magic);
@@ -80,7 +80,6 @@ int fosa_long_jump_was_performed
   if (!context && !jumped)
     return FOSA_EINVAL;
 
-//   printf ("context [6]=%ld\n", (*(context))[6]);
   *jumped = ((*(context))[6] == LONGJMP_MAGIC);
   return 0;
 }
@@ -90,11 +89,14 @@ void jmp_handler (int signo, siginfo_t *info, void *context)
 {
   fosa_long_jump_context_t *jmp_info = (fosa_long_jump_context_t *) info -> si_value.sival_ptr;
 
-#ifdef CONFIG_LONGJUMP_FREE_SIGNAL
-  sigset_t set;
+  sigset_t s;
 
   // Restore the signal mask
-//   pthread_sigmask (SIG_SETMASK, &(jmp_info -> jmp_sigmask), NULL);
+  s.sig = (*jmp_info)[7];
+  pthread_sigmask (SIG_SETMASK, &s, NULL);
+
+#ifdef CONFIG_LONGJUMP_FREE_SIGNAL
+  sigset_t set;
 
   // Free this signal
   sigemptyset (&set);
@@ -102,11 +104,10 @@ void jmp_handler (int signo, siginfo_t *info, void *context)
   pthread_sigmask (SIG_BLOCK, &sigmask, NULL);
 
   pthread_mutex_lock (&signal_pool_m);
-  jmp_used_signals [signo] = 0;
+  jmp_used_signals [signo] = NULL;
   pthread_mutex_unlock (&signal_pool_m);
 #endif
 
-//   printf ("\t\t\t\t>>>  RESTORE CONTEXT  <<<\n\n");
   // Restore the saved context
   fosa_longjmp (*jmp_info, LONGJMP_MAGIC);
   return;
@@ -120,6 +121,15 @@ int fosa_long_jump_install_handler
   struct sigaction sa;
   sigset_t sigmask;
 
+  // Check if current thread has a handler associated
+  for (i = 0; i < LONGJMP_NSIG; i ++)
+    if (jmp_used_signals [i] == pthread_self ()) {
+      sigemptyset (&sigmask);
+      sigaddset (&sigmask, LONGJMP_FIRSTSIG + i);
+      pthread_sigmask (SIG_UNBLOCK, &sigmask, NULL);
+      return 0;
+    }
+
   // Find the first usable signal
   pthread_mutex_lock (&signal_pool_m);
   for (i = 0; i < LONGJMP_NSIG && jmp_used_signals [i]; i++);
@@ -128,7 +138,7 @@ int fosa_long_jump_install_handler
     return FOSA_ENOMEM;
   }
 
-  jmp_used_signals [i] = 1;
+  jmp_used_signals [i] = pthread_self ();
   pthread_mutex_unlock (&signal_pool_m);
 
   *signal = LONGJMP_FIRSTSIG + i;