FOSA-PaRTiKle: some clean up

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

diff --git a/src_partikle/fosa_app_def_sched.c b/src_partikle/fosa_app_def_sched.c
index 0c2d83ffa761a7c7e9a6095a4eb7611f2716788e..057e65606dc2755144e2543cd489caf891c451b2 100644 (file)
--- a/src_partikle/fosa_app_def_sched.c
+++ b/src_partikle/fosa_app_def_sched.c
@@ -59,12 +59,12 @@
 
 #include <fosa_configuration_parameters.h>
 #include <fosa_app_def_sched.h>
+#include <fosa_threads_and_signals.h>
 #include <sched.h>
 #include <signal.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <string.h>
-// #include <fosa_configuration_parameters.h>
 
 #define REACHS(str) printf ("%s: %s: %d: %s\n", __FILE__, __FUNCTION__, __LINE__, str)
 
@@ -99,8 +99,9 @@ void *fosa_scheduler_main (void * args)
        struct posix_appsched_event event;
        struct timespec current_time;
        posix_appsched_eventset_t accepted_events;
-       bool active;
+//     bool active;
        
+       printf ("SCHEDULER thread=0x%lx\n\n", (unsigned long) pthread_self ());
        
        // accept events which have a callback associated
        posix_appsched_fillset (&accepted_events);
@@ -165,29 +166,19 @@ void *fosa_scheduler_main (void * args)
                                        
 /*                                     active = actions.activated;
                                        int suspend = actions.suspended; */
-#ifdef CONFIG_URGENCY
-        #error "Urgency not supported. Disable this feature in the Makefile"
-#else
+// #ifndef CONFIG_URGENCY  
                                        clear_actions (&actions);
 //                                         printf ("NEW: activate (%d), suspend (%d)\n", actions.activated, actions.suspended);
-#endif
-
-                                       posix_appsched_actions_addaccept (&actions.actions, event.thread);
-                                       
-//                                     printf ("activated\n");
-//                                     if (active)
-//                                             fosa_adsactions_add_activate (&actions, event.thread, 0);
-//                                     if (suspend) 
-//                                             fosa_adsactions_add_suspend (&actions, event.thread);
-
-                                                                               
+// // #endif
+  
                                        // alloc memory for reply info of 'fosa_ads_invoke_with_data ()'
                                        struct reply_info *reply_mem = malloc (sizeof (struct reply_info));
                                        if (!reply_mem) {
-                                          printf ("BUG: not enougth dynamic memory\n");
-                                          exit (-20);
+                                         posix_appsched_actions_addreject (&actions.actions, event.thread);
+                                       } else {
+                                          posix_appsched_actions_addaccept (&actions.actions, event.thread);
+                                          pthread_setspecific_for (fosa_reply_key, event.thread, reply_mem);
                                        }
-                                       pthread_setspecific_for (fosa_reply_key, event.thread, reply_mem);
                                }
                                break;
                        case POSIX_APPSCHED_TERMINATE:
@@ -223,9 +214,7 @@ void *fosa_scheduler_main (void * args)
                                struct reply_info *fosa_reply;
                                
                                // Get reply memory pointer
-//                                 printf ("CALL_WITH_DATA: caller 0x%x\n", event.thread);
                                pthread_getspecific_from (fosa_reply_key, event.thread, (void **) &fosa_reply);
-//                             printf ("reply_size (0x%x)=%d\n", fosa_reply -> size_ptr, (fosa_reply -> size_ptr)? *(fosa_reply -> size_ptr):-1);
                                fosa_scheduler_ops.explicit_call_with_data
                                                (fosa_scheduler_data,
                                                 event.thread,
@@ -236,7 +225,6 @@ void *fosa_scheduler_main (void * args)
                                                 &actions,
                                                 &current_time);
 
-//                                 printf ("CALL_WITH_DATA: activate (%d), suspend (%d)\n", actions.activated, actions.suspended);
                               // activate the thread unless suspended or already activated
                                if (!actions.suspended && !actions.activated) {
                                        posix_appsched_actions_addactivate
@@ -262,7 +250,10 @@ void *fosa_scheduler_main (void * args)
 
                        case POSIX_APPSCHED_THREAD_NOTIFICATION:
                                fosa_scheduler_ops.notification_for_thread
-                                               (fosa_scheduler_data, event.thread,(fosa_clock_id_t) event.event_info.info, &actions, &current_time);
+                                               (fosa_scheduler_data, 
+                                                event.thread,(fosa_clock_id_t) event.event_info.info, 
+                                                &actions, 
+                                                &current_time);
                                break;
 
                        case POSIX_APPSCHED_ERROR:
@@ -273,9 +264,8 @@ void *fosa_scheduler_main (void * args)
                                         &actions);
                                break;
 
-               */
+*/
                } 
-//             printf ("ENd LOOP\n");
        }
 }
 
@@ -291,6 +281,10 @@ int fosa_ads_scheduler_create
        pthread_attr_t attr;
        struct sched_param sp;
        
+       
+       printf ("\nMAIN thread=0x%lx\n\n", (unsigned long)pthread_self ());
+       
+       
        if (!scheduler_ops)
                return EINVAL;
        fosa_scheduler_ops = *scheduler_ops;
@@ -335,7 +329,8 @@ int fosa_thread_attr_set_appscheduled
 {
        int err;
        
-       if (err = pthread_attr_setappscheduler (attr, fosa_scheduler_th))
+       err = pthread_attr_setappscheduler (attr, fosa_scheduler_th);
+       if (err)
                return err;
        
        if (appscheduled)
@@ -351,7 +346,8 @@ int fosa_thread_attr_get_appscheduled
 {
        int policy, err;
        
-       if (err = pthread_attr_getschedpolicy (attr, &policy))
+       err = pthread_attr_getschedpolicy (attr, &policy);
+       if (err)
                return err;
        
        *appscheduled = (policy == SCHED_APP);
@@ -390,11 +386,13 @@ int fosa_ads_set_appscheduled
        struct sched_param sp; 
        int policy_old, policy_new, err;
        
-       if (err = pthread_getschedparam (thread, &policy_old, &sp))
+       err = pthread_getschedparam (thread, &policy_old, &sp);
+       if (err)
                return err;
        
        if (appscheduled) {
-               if (err = pthread_setappscheduler (thread, fosa_scheduler_th))
+               err = pthread_setappscheduler (thread, fosa_scheduler_th);
+               if (err)
                        return err;
        
                policy_new = SCHED_APP;
@@ -416,7 +414,8 @@ int fosa_ads_get_appscheduled
        struct sched_param sp;
        int policy, err;
        
-       if (err = pthread_getschedparam (thread, &policy, &sp))
+       err = pthread_getschedparam (thread, &policy, &sp);
+       if (err)
                return err;
        
        *appscheduled = (policy == SCHED_APP);
@@ -454,16 +453,25 @@ int fosa_adsactions_add_reject
        return posix_appsched_actions_addreject (&(sched_actions -> actions), thread);
 }
 
+extern inline int fosa2prtk (int prio, int urg);
 int fosa_adsactions_add_activate
                (fosa_ads_actions_t *sched_actions,
                 fosa_thread_id_t thread,
                 fosa_ads_urgency_t urgency)
 {
        sched_actions -> activated = true;
+
 #ifdef CONFIG_URGENCY
         #error "Urgency not supported. Disable this feature in the Makefile"
+       int fosaprio, policy;
+       struct sched_param sp;
+       
+       pthread_getschedparam (thread, &policy, &sp);
+       fosa_thread_get_prio (thread, &fosaprio);
+       sp.sched_priority =  fosa2prtk (fosaprio, urgency);
+       printf ("policy=%d, prio=%d, urg=%d, new_prio=%d\n", policy);
+       pthread_setschedparam (thread, policy, &sp);
 #endif
-//         printf ("0x%x: Activate thread 0x%x\n", pthread_self (), thread);
        return posix_appsched_actions_addactivate (&(sched_actions -> actions), thread);
 }
 
@@ -471,7 +479,6 @@ int fosa_adsactions_add_suspend
                (fosa_ads_actions_t *sched_actions,
                 fosa_thread_id_t thread)
 {
-//         printf ("0x%x: Suspend thread 0x%x\n", pthread_self (), thread);
        sched_actions -> suspended = true;
        return posix_appsched_actions_addsuspend (&(sched_actions -> actions), thread);
 }
@@ -505,7 +512,6 @@ int fosa_ads_set_handled_signal_set (fosa_signal_t set[], int size)
        if (!pthread_equal (pthread_self (), fosa_scheduler_th))
                return EPOLICY;
        
-       printf ("\n\nHANDLED\n");
        sigemptyset (&fosa_handled_signals);
        for (i = 0; i < size; i++) {
                if (FOSA_SIGNAL_MIN > set [i] || set [i] > FOSA_SIGNAL_MAX)