src_partikle/fosa_app_def_sched.c

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