src_partikle/fosa_app_def_sched.c

   1 // -----------------------------------------------------------------------
   2 //  Copyright (C) 2006 - 2007 FRESCOR consortium partners:
   3 //
   4 //    Universidad de Cantabria,              SPAIN
   5 //    University of York,                    UK
   6 //    Scuola Superiore Sant'Anna,            ITALY
   7 //    Kaiserslautern University,             GERMANY
   8 //    Univ. Politecnica  Valencia,           SPAIN
   9 //    Czech Technical University in Prague,  CZECH REPUBLIC
  10 //    ENEA                                   SWEDEN
  11 //    Thales Communication S.A.              FRANCE
  12 //    Visual Tools S.A.                      SPAIN
  13 //    Rapita Systems Ltd                     UK
  14 //    Evidence                               ITALY
  15 //
  16 //    See http://www.frescor.org for a link to partners' websites
  17 //
  18 //           FRESCOR project (FP6/2005/IST/5-034026) is funded
  19 //        in part by the European Union Sixth Framework Programme
  20 //        The European Union is not liable of any use that may be
  21 //        made of this code.
  22 //
  23 //  This file is part of the FRSH implementation
  24 //
  25 //  FRSH is free software; you can  redistribute it and/or  modify
  26 //  it under the terms of  the GNU General Public License as published by
  27 //  the Free Software Foundation;  either  version 2, or (at  your option)
  28 //  any later version.
  29 //
  30 //  FRSH  is distributed  in  the hope  that  it  will  be useful,  but
  31 //  WITHOUT  ANY  WARRANTY;     without  even the   implied   warranty  of
  32 //  MERCHANTABILITY  or  FITNESS FOR  A  PARTICULAR PURPOSE. See  the  GNU
  33 //  General Public License for more details.
  34 //
  35 //  You should have  received a  copy of  the  GNU  General Public License
  36 //  distributed  with  FRSH;  see file COPYING.   If not,  write to the
  37 //  Free Software  Foundation,  59 Temple Place  -  Suite 330,  Boston, MA
  38 //  02111-1307, USA.
  39 //
  40 //  As a special exception, if you include this header file into source
  41 //  files to be compiled, this header file does not by itself cause
  42 //  the resulting executable to be covered by the GNU General Public
  43 //  License.  This exception does not however invalidate any other
  44 //  reasons why the executable file might be covered by the GNU General
  45 //  Public License.
  46 // -----------------------------------------------------------------------
  47 //==============================================
  48 //  ********  ******    ********  **********
  49 //  **///// /**    **  **//////  /**     /**
  50 //  **      /**    ** /**        /**     /**
  51 //  ******* /**    ** /********* /**********
  52 //  **////  /**    ** ////////** /**//////**
  53 //  **      /**    **        /** /**     /**
  54 //  **      /**    **  ********  /**     /**
  55 //  //       /******/  ////////   //      //
  56 //
  57 // FOSA(Frescor Operating System Adaptation layer)
  58 //================================================
  59
  60 #include <fosa_configuration_parameters.h>
  61 #include <fosa_app_def_sched.h>
  62 #include <sched.h>
  63 #include <signal.h>
  64 #include <unistd.h>
  65 #include <stdio.h>
  66 #include <string.h>
  67 // #include <fosa_configuration_parameters.h>
  68
  69 #define REACHS(str) printf ("%s: %s: %d: %s\n", __FILE__, __FUNCTION__, __LINE__, str)
  70
  71 static pthread_t fosa_scheduler_th;                                     // Scheduler thread
  72 static fosa_ads_scheduler_ops_t fosa_scheduler_ops;                     // Scheduler operations
  73 static void * fosa_scheduler_data;                                      // Scheduler parameters
  74 static size_t fosa_scheduler_data_size;
  75 static void * fosa_init_args;                                           // Scheduler initialisation args
  76 static size_t fosa_init_args_size;
  77 static sigset_t fosa_handled_signals;                                   // Signals handled by the scheduler
  78
  79 // Reply info for 'fosa_ads_invoke_withdata ()'
  80 struct reply_info{
  81         void * msg;
  82         size_t * size_ptr;
  83 };
  84 static int fosa_reply_key;
  85
  86 void clear_actions (fosa_ads_actions_t *act)
  87 {
  88         posix_appsched_actions_destroy (&(act -> actions));
  89         posix_appsched_actions_init (&(act -> actions));
  90         act -> activated = false;
  91         act -> suspended = false;
  92         act -> rejected = false;
  93         act -> timeout_ptr = NULL;
  94 }
  95
  96 void *fosa_scheduler_main (void * args)
  97 {
  98         fosa_ads_actions_t actions;
  99         struct posix_appsched_event event;
 100         struct timespec current_time;
 101         posix_appsched_eventset_t accepted_events;
 102         bool active;
 103
 104
 105         // accept events which have a callback associated
 106         posix_appsched_fillset (&accepted_events);
 107         if (fosa_scheduler_ops.new_thread)
 108                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_NEW);
 109
 110         if (fosa_scheduler_ops.thread_terminate)
 111                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_TERMINATE);
 112
 113         if (fosa_scheduler_ops.thread_ready)
 114                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_READY);
 115
 116         if (fosa_scheduler_ops.thread_block)
 117                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_BLOCK);
 118
 119         if (fosa_scheduler_ops.change_sched_param_thread)
 120                 posix_appsched_delset (&accepted_events, POSIX_APPSCHED_CHANGE_SCHED_PARAM);
 121
 122         if (fosa_scheduler_ops.explicit_call_with_data)
 123                 posix_appsched_delset (&accepted_events, POSIX_APPSCHED_EXPLICIT_CALL_WITH_DATA);
 124
 125 //      if (fosa_scheduler_ops.notification_for_thread)
 126 //              posix_appsched_delset (&accepted_events,POSIX_APPSCHED_TASK_NOTIFICATION);
 127
 128         if (fosa_scheduler_ops.timeout)
 129                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_TIMEOUT);
 130
 131         if (fosa_scheduler_ops.signal)
 132                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_SIGNAL);
 133
 134 //      if (fosa_scheduler_ops.signal)
 135 //              posix_appsched_delset (&accepted_events,POSIX_APPSCHED_ERRROR);
 136
 137         posix_appschedattr_seteventmask(&accepted_events);
 138
 139         // Set the clock (and its) flags used by the scheduler
 140         posix_appschedattr_setclock (FOSA_CLOCK_REALTIME);
 141         posix_appschedattr_setflags (POSIX_APPSCHED_ABSTIMEOUT);
 142
 143         // init scheduler
 144         fosa_scheduler_ops.init (fosa_scheduler_data, fosa_init_args);
 145         clear_actions (&actions);
 146
 147         while (1) {     // scheduler loop
 148 //              printf ("ACTIONS: timeout(0x%x), handled_signals=0x%x\n", actions.timeout_ptr, fosa_handled_signals.sig);
 149                 if (posix_appsched_execute_actions(&(actions.actions), &fosa_handled_signals, actions.timeout_ptr,
 150                     &current_time, &event))
 151                         perror ("posix_appsched_execute_actions");
 152
 153 //              printf ("\nReceived event=%d. current_time={%ld,%ld}\n", event.event_code, current_time.tv_sec, current_time.tv_nsec);
 154                 clear_actions (&actions);
 155
 156                 switch (event.event_code) {
 157                         case POSIX_APPSCHED_NEW:
 158                                 fosa_scheduler_ops.new_thread
 159                                                 (fosa_scheduler_data,
 160                                                  event.thread,
 161                                                  &actions,
 162                                                  &current_time);
 163
 164                                 if (!actions.rejected) {
 165
 166 /*                                      active = actions.activated;
 167                                         int suspend = actions.suspended; */
 168 #ifdef CONFIG_URGENCY
 169         #error "Urgency not supported. Disable this feature in the Makefile"
 170 #else
 171                                         clear_actions (&actions);
 172 //                                         printf ("NEW: activate (%d), suspend (%d)\n", actions.activated, actions.suspended);
 173 #endif
 174
 175                                         posix_appsched_actions_addaccept (&actions.actions, event.thread);
 176
 177 //                                      printf ("activated\n");
 178 //                                      if (active)
 179 //                                              fosa_adsactions_add_activate (&actions, event.thread, 0);
 180 //                                      if (suspend)
 181 //                                              fosa_adsactions_add_suspend (&actions, event.thread);
 182
 183
 184                                         // alloc memory for reply info of 'fosa_ads_invoke_with_data ()'
 185                                         struct reply_info *reply_mem = malloc (sizeof (struct reply_info));
 186                                         if (!reply_mem) {
 187                                            printf ("BUG: not enougth dynamic memory\n");
 188                                            exit (-20);
 189                                         }
 190                                         pthread_setspecific_for (fosa_reply_key, event.thread, reply_mem);
 191                                 }
 192                                 break;
 193                         case POSIX_APPSCHED_TERMINATE:
 194                                 fosa_scheduler_ops.thread_terminate
 195                                                 (fosa_scheduler_data,
 196                                                  event.thread,
 197                                                  &actions,
 198                                                  &current_time);
 199                                 break;
 200                         case POSIX_APPSCHED_READY:
 201                                 fosa_scheduler_ops.thread_ready
 202                                                 (fosa_scheduler_data,
 203                                                  event.thread,
 204                                                  &actions,
 205                                                  &current_time);
 206                                 break;
 207                         case POSIX_APPSCHED_BLOCK:
 208                                 fosa_scheduler_ops.thread_block
 209                                                 (fosa_scheduler_data,
 210                                                  event.thread,
 211                                                  &actions,
 212                                                  &current_time);
 213                                 break;
 214                         case POSIX_APPSCHED_CHANGE_SCHED_PARAM:
 215                                 fosa_scheduler_ops.change_sched_param_thread
 216                                                 (fosa_scheduler_data,
 217                                                  event.thread,
 218                                                  &actions,
 219                                                  &current_time);
 220                                 break;
 221                         case POSIX_APPSCHED_EXPLICIT_CALL_WITH_DATA:
 222                         {
 223                                 struct reply_info *fosa_reply;
 224
 225                                 // Get reply memory pointer
 226 //                                 printf ("CALL_WITH_DATA: caller 0x%x\n", event.thread);
 227                                 pthread_getspecific_from (fosa_reply_key, event.thread, (void **) &fosa_reply);
 228 //                              printf ("reply_size (0x%x)=%d\n", fosa_reply -> size_ptr, (fosa_reply -> size_ptr)? *(fosa_reply -> size_ptr):-1);
 229                                 fosa_scheduler_ops.explicit_call_with_data
 230                                                 (fosa_scheduler_data,
 231                                                  event.thread,
 232                                                  event.event_info.info,
 233                                                  event.info_size,
 234                                                  fosa_reply -> msg,
 235                                                  fosa_reply -> size_ptr,
 236                                                  &actions,
 237                                                  &current_time);
 238
 239 //                                 printf ("CALL_WITH_DATA: activate (%d), suspend (%d)\n", actions.activated, actions.suspended);
 240                                // activate the thread unless suspended or already activated
 241                                 if (!actions.suspended && !actions.activated) {
 242                                         posix_appsched_actions_addactivate
 243                                                         (&actions.actions, event.thread);
 244                                 }
 245
 246                         } break;
 247                         case  POSIX_APPSCHED_TIMEOUT:
 248                                 fosa_scheduler_ops.timeout
 249                                                 (fosa_scheduler_data,
 250                                                  &actions,
 251                                                  &current_time);
 252                                 break;
 253                         case POSIX_APPSCHED_SIGNAL:
 254                                 fosa_scheduler_ops.signal
 255                                                 (fosa_scheduler_data,
 256                                                  event.event_info.siginfo.si_signo,
 257                                                  (fosa_signal_info_t)event.event_info.siginfo.si_value.sival_ptr,
 258                                                  &actions,
 259                                                  &current_time);
 260                                 break;
 261 /*                              - NOT IMPLEMENTED
 262
 263                         case POSIX_APPSCHED_THREAD_NOTIFICATION:
 264                                 fosa_scheduler_ops.notification_for_thread
 265                                                 (fosa_scheduler_data, event.thread,(fosa_clock_id_t) event.event_info.info, &actions, &current_time);
 266                                 break;
 267
 268                         case POSIX_APPSCHED_ERROR:
 269                                 fosa_scheduler_ops.appsched_error
 270                                         (fosa_scheduler_data,
 271                                          event.thread,
 272                                          0,
 273                                          &actions);
 274                                 break;
 275
 276                 */
 277                 }
 278 //              printf ("ENd LOOP\n");
 279         }
 280 }
 281
 282 /********************************
 283  * Application-defined scheduling
 284  ********************************/
 285 int fosa_ads_scheduler_create
 286                 (const fosa_ads_scheduler_ops_t * scheduler_ops,
 287                  size_t scheduler_data_size,
 288                  void * init_args,
 289                  size_t init_args_size)
 290 {
 291         pthread_attr_t attr;
 292         struct sched_param sp;
 293
 294         if (!scheduler_ops)
 295                 return EINVAL;
 296         fosa_scheduler_ops = *scheduler_ops;
 297
 298         // Alloc memory for init args and scheduler data
 299         fosa_init_args_size = init_args_size;
 300         fosa_init_args = malloc (init_args_size);
 301         if (!init_args)
 302                 return EAGAIN;
 303
 304         memcpy (fosa_init_args, init_args, init_args_size);
 305
 306         fosa_scheduler_data_size = scheduler_data_size;
 307         fosa_scheduler_data = malloc (scheduler_data_size);
 308         if (!fosa_scheduler_data) {
 309                 free (init_args);
 310                 return  EAGAIN;
 311         }
 312         memset (fosa_scheduler_data, 0, scheduler_data_size);
 313
 314         // Key for reply information index of 'fosa_reply' for each thread
 315         pthread_key_create (&fosa_reply_key, NULL);
 316
 317         // Set scheduler thread parameters
 318         pthread_attr_init (&attr);
 319         sp.sched_priority = sched_get_priority_max (SCHED_FIFO) + FOSA_ADS_SCHEDULER_PRIO_DIFF;
 320         pthread_attr_setschedparam (&attr, &sp);
 321         pthread_attr_setschedpolicy (&attr, SCHED_FIFO);
 322         pthread_attr_setappschedulerstate (&attr, PTHREAD_APPSCHEDULER);
 323         pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_DETACHED);
 324         pthread_attr_setinheritsched(&attr,PTHREAD_EXPLICIT_SCHED);
 325
 326         if (pthread_create (&fosa_scheduler_th, &attr, fosa_scheduler_main, NULL))
 327                 return errno;
 328         else
 329                 return 0;
 330 }
 331
 332 int fosa_thread_attr_set_appscheduled
 333                 (fosa_thread_attr_t *attr,
 334                  bool appscheduled)
 335 {
 336         int err;
 337
 338         if (err = pthread_attr_setappscheduler (attr, fosa_scheduler_th))
 339                 return err;
 340
 341         if (appscheduled)
 342                 return pthread_attr_setschedpolicy (attr, SCHED_APP);
 343         else
 344                 return pthread_attr_setschedpolicy (attr, SCHED_FIFO);
 345 }
 346
 347
 348 int fosa_thread_attr_get_appscheduled
 349                 (const fosa_thread_attr_t *attr,
 350                  bool *appscheduled)
 351 {
 352         int policy, err;
 353
 354         if (err = pthread_attr_getschedpolicy (attr, &policy))
 355                 return err;
 356
 357         *appscheduled = (policy == SCHED_APP);
 358         return 0;
 359 }
 360
 361
 362 int fosa_thread_attr_set_appsched_params
 363                 (fosa_thread_attr_t *attr,
 364                  const void *param,
 365                  size_t paramsize)
 366 {
 367 #if FOSA_ADS_SCHEDPARAM_MAX > POSIX_APPSCHEDPARAM_MAX
 368 #error  FOSA_ADS_SCHEDPARAM_MAX > POSIX_APPSCHEDPARAM_MAX
 369 #endif
 370         if (paramsize > FOSA_ADS_SCHEDPARAM_MAX)
 371                 return FOSA_EINVAL;
 372
 373         return pthread_attr_setappschedparam (attr, param, paramsize);
 374 }
 375
 376
 377 int fosa_thread_attr_get_appsched_params
 378                 (const fosa_thread_attr_t *attr,
 379                  void *param,
 380                 size_t *paramsize)
 381 {
 382         return pthread_attr_getappschedparam (attr, param, paramsize);
 383 }
 384
 385
 386 int fosa_ads_set_appscheduled
 387                 (fosa_thread_id_t thread,
 388                  bool appscheduled)
 389 {
 390         struct sched_param sp;
 391         int policy_old, policy_new, err;
 392
 393         if (err = pthread_getschedparam (thread, &policy_old, &sp))
 394                 return err;
 395
 396         if (appscheduled) {
 397                 if (err = pthread_setappscheduler (thread, fosa_scheduler_th))
 398                         return err;
 399
 400                 policy_new = SCHED_APP;
 401         } else {
 402                 policy_new = SCHED_FIFO;
 403         }
 404
 405         if (policy_new != policy_old)
 406                 return pthread_setschedparam (thread, policy_new, &sp);
 407
 408         return 0;
 409 }
 410
 411
 412 int fosa_ads_get_appscheduled
 413                 (fosa_thread_id_t thread,
 414                  bool *appscheduled)
 415 {
 416         struct sched_param sp;
 417         int policy, err;
 418
 419         if (err = pthread_getschedparam (thread, &policy, &sp))
 420                 return err;
 421
 422         *appscheduled = (policy == SCHED_APP);
 423         return 0;
 424 }
 425
 426
 427 int fosa_ads_set_appsched_params
 428                 (fosa_thread_id_t thread,
 429                  const void *param,
 430                  size_t paramsize)
 431 {
 432         return pthread_setappschedparam (thread, param, paramsize);
 433 }
 434
 435
 436 int fosa_ads_get_appsched_params
 437                 (fosa_thread_id_t thread,
 438                  void *param,
 439                  size_t *paramsize)
 440 {
 441         return pthread_getappschedparam (thread, param, paramsize);
 442 }
 443
 444
 445
 446 /*********************************
 447  * ADS actions
 448  *********************************/
 449 int fosa_adsactions_add_reject
 450                 (fosa_ads_actions_t *sched_actions,
 451                  fosa_thread_id_t thread)
 452 {
 453         sched_actions -> rejected = true;
 454         return posix_appsched_actions_addreject (&(sched_actions -> actions), thread);
 455 }
 456
 457 int fosa_adsactions_add_activate
 458                 (fosa_ads_actions_t *sched_actions,
 459                  fosa_thread_id_t thread,
 460                  fosa_ads_urgency_t urgency)
 461 {
 462         sched_actions -> activated = true;
 463 #ifdef CONFIG_URGENCY
 464         #error "Urgency not supported. Disable this feature in the Makefile"
 465 #endif
 466 //         printf ("0x%x: Activate thread 0x%x\n", pthread_self (), thread);
 467         return posix_appsched_actions_addactivate (&(sched_actions -> actions), thread);
 468 }
 469
 470 int fosa_adsactions_add_suspend
 471                 (fosa_ads_actions_t *sched_actions,
 472                  fosa_thread_id_t thread)
 473 {
 474 //         printf ("0x%x: Suspend thread 0x%x\n", pthread_self (), thread);
 475         sched_actions -> suspended = true;
 476         return posix_appsched_actions_addsuspend (&(sched_actions -> actions), thread);
 477 }
 478
 479 int fosa_adsactions_add_timeout
 480                 (fosa_ads_actions_t *sched_actions,
 481                  fosa_clock_id_t clock_id,
 482                  const struct timespec *at_time)
 483 {
 484         sched_actions -> timeout = *at_time;
 485         sched_actions -> timeout_ptr = &(sched_actions -> timeout);
 486         return 0;
 487 }
 488
 489 int fosa_adsactions_add_thread_notification
 490                 (fosa_ads_actions_t *sched_actions,
 491                  fosa_thread_id_t thread,
 492                  fosa_clock_id_t clock_id,
 493                  const struct timespec *at_time)
 494 {
 495         printf ("BUG: fosa_adsactions_add_thread_notification: Not implemented\n");
 496         exit (-21);
 497         return 0;
 498 }
 499
 500
 501 int fosa_ads_set_handled_signal_set (fosa_signal_t set[], int size)
 502 {
 503         int i;
 504
 505         if (!pthread_equal (pthread_self (), fosa_scheduler_th))
 506                 return EPOLICY;
 507
 508         printf ("\n\nHANDLED\n");
 509         sigemptyset (&fosa_handled_signals);
 510         for (i = 0; i < size; i++) {
 511                 if (FOSA_SIGNAL_MIN > set [i] || set [i] > FOSA_SIGNAL_MAX)
 512                         return EINVAL;
 513                 sigaddset (&fosa_handled_signals, set[i]);
 514         }
 515         return fosa_set_accepted_signals (set, size);
 516 }
 517
 518
 519 int fosa_signal_queue_scheduler (fosa_signal_t signal, fosa_signal_info_t info)
 520 {
 521         return fosa_signal_queue (signal, info, 0);
 522 }
 523
 524
 525 int fosa_ads_invoke_withdata
 526                 (const void *msg, size_t msg_size, void *reply, size_t *reply_size)
 527 {
 528         struct reply_info *fosa_reply;
 529
 530 #if FOSA_ADS_SCHEDINFO_MAX > POSIX_APPSCHEDINFO_MAX
 531 #error  FOSA_ADS_SCHEDINFO_MAX > POSIX_APPSCHEDINFO_MAX
 532 #endif
 533
 534         if (msg_size > FOSA_ADS_SCHEDINFO_MAX)
 535                 return EINVAL;
 536
 537         fosa_reply = (struct reply_info *) pthread_getspecific (fosa_reply_key);
 538         fosa_reply -> msg = reply;
 539         fosa_reply -> size_ptr = reply_size;
 540
 541         return posix_appsched_invoke_withdata (msg, msg_size, NULL, NULL);
 542 }