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 <fosa_threads_and_signals.h>
  63 #include <sched.h>
  64 #include <signal.h>
  65 #include <unistd.h>
  66 #include <stdio.h>
  67 #include <string.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         printf ("SCHEDULER thread=0x%lx\n\n", (unsigned long) pthread_self ());
 105
 106         // accept events which have a callback associated
 107         posix_appsched_fillset (&accepted_events);
 108         if (fosa_scheduler_ops.new_thread)
 109                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_NEW);
 110
 111         if (fosa_scheduler_ops.thread_terminate)
 112                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_TERMINATE);
 113
 114         if (fosa_scheduler_ops.thread_ready)
 115                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_READY);
 116
 117         if (fosa_scheduler_ops.thread_block)
 118                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_BLOCK);
 119
 120         if (fosa_scheduler_ops.change_sched_param_thread)
 121                 posix_appsched_delset (&accepted_events, POSIX_APPSCHED_CHANGE_SCHED_PARAM);
 122
 123         if (fosa_scheduler_ops.explicit_call_with_data)
 124                 posix_appsched_delset (&accepted_events, POSIX_APPSCHED_EXPLICIT_CALL_WITH_DATA);
 125
 126 //      if (fosa_scheduler_ops.notification_for_thread)
 127 //              posix_appsched_delset (&accepted_events,POSIX_APPSCHED_TASK_NOTIFICATION);
 128
 129         if (fosa_scheduler_ops.timeout)
 130                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_TIMEOUT);
 131
 132         if (fosa_scheduler_ops.signal)
 133                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_SIGNAL);
 134
 135 //      if (fosa_scheduler_ops.signal)
 136 //              posix_appsched_delset (&accepted_events,POSIX_APPSCHED_ERRROR);
 137
 138         posix_appschedattr_seteventmask(&accepted_events);
 139
 140         // Set the clock (and its) flags used by the scheduler
 141         posix_appschedattr_setclock (FOSA_CLOCK_REALTIME);
 142         posix_appschedattr_setflags (POSIX_APPSCHED_ABSTIMEOUT);
 143
 144         // init scheduler
 145         fosa_scheduler_ops.init (fosa_scheduler_data, fosa_init_args);
 146         clear_actions (&actions);
 147
 148         while (1) {     // scheduler loop
 149 //              printf ("ACTIONS: timeout(0x%x), handled_signals=0x%x\n", actions.timeout_ptr, fosa_handled_signals.sig);
 150                 if (posix_appsched_execute_actions(&(actions.actions), &fosa_handled_signals, actions.timeout_ptr,
 151                     &current_time, &event))
 152                         perror ("posix_appsched_execute_actions");
 153
 154 //              printf ("\nReceived event=%d. current_time={%ld,%ld}\n", event.event_code, current_time.tv_sec, current_time.tv_nsec);
 155                 clear_actions (&actions);
 156
 157                 switch (event.event_code) {
 158                         case POSIX_APPSCHED_NEW:
 159                                 fosa_scheduler_ops.new_thread
 160                                                 (fosa_scheduler_data,
 161                                                  event.thread,
 162                                                  &actions,
 163                                                  &current_time);
 164
 165                                 if (!actions.rejected) {
 166
 167 /*                                      active = actions.activated;
 168                                         int suspend = actions.suspended; */
 169 // #ifndef CONFIG_URGENCY
 170                                         clear_actions (&actions);
 171 //                                         printf ("NEW: activate (%d), suspend (%d)\n", actions.activated, actions.suspended);
 172 // // #endif
 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                         case POSIX_APPSCHED_ERROR:
 260                                 fosa_scheduler_ops.appsched_error
 261                                         (fosa_scheduler_data,
 262                                          event.thread,
 263                                          0,
 264                                          &actions);
 265                                 break;
 266
 267 */
 268                 }
 269         }
 270 }
 271
 272 /********************************
 273  * Application-defined scheduling
 274  ********************************/
 275 int fosa_ads_scheduler_create
 276                 (const fosa_ads_scheduler_ops_t * scheduler_ops,
 277                  size_t scheduler_data_size,
 278                  void * init_args,
 279                  size_t init_args_size)
 280 {
 281         pthread_attr_t attr;
 282         struct sched_param sp;
 283
 284
 285         printf ("\nMAIN thread=0x%lx\n\n", (unsigned long)pthread_self ());
 286
 287
 288         if (!scheduler_ops)
 289                 return EINVAL;
 290         fosa_scheduler_ops = *scheduler_ops;
 291
 292         // Alloc memory for init args and scheduler data
 293         fosa_init_args_size = init_args_size;
 294         fosa_init_args = malloc (init_args_size);
 295         if (!init_args)
 296                 return EAGAIN;
 297
 298         memcpy (fosa_init_args, init_args, init_args_size);
 299
 300         fosa_scheduler_data_size = scheduler_data_size;
 301         fosa_scheduler_data = malloc (scheduler_data_size);
 302         if (!fosa_scheduler_data) {
 303                 free (init_args);
 304                 return  EAGAIN;
 305         }
 306         memset (fosa_scheduler_data, 0, scheduler_data_size);
 307
 308         // Key for reply information index of 'fosa_reply' for each thread
 309         pthread_key_create (&fosa_reply_key, NULL);
 310
 311         // Set scheduler thread parameters
 312         pthread_attr_init (&attr);
 313         sp.sched_priority = sched_get_priority_max (SCHED_FIFO) + FOSA_ADS_SCHEDULER_PRIO_DIFF;
 314         pthread_attr_setschedparam (&attr, &sp);
 315         pthread_attr_setschedpolicy (&attr, SCHED_FIFO);
 316         pthread_attr_setappschedulerstate (&attr, PTHREAD_APPSCHEDULER);
 317         pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_DETACHED);
 318         pthread_attr_setinheritsched(&attr,PTHREAD_EXPLICIT_SCHED);
 319
 320         if (pthread_create (&fosa_scheduler_th, &attr, fosa_scheduler_main, NULL))
 321                 return errno;
 322         else
 323                 return 0;
 324 }
 325
 326 int fosa_thread_attr_set_appscheduled
 327                 (fosa_thread_attr_t *attr,
 328                  bool appscheduled)
 329 {
 330         int err;
 331
 332         err = pthread_attr_setappscheduler (attr, fosa_scheduler_th);
 333         if (err)
 334                 return err;
 335
 336         if (appscheduled)
 337                 return pthread_attr_setschedpolicy (attr, SCHED_APP);
 338         else
 339                 return pthread_attr_setschedpolicy (attr, SCHED_FIFO);
 340 }
 341
 342
 343 int fosa_thread_attr_get_appscheduled
 344                 (const fosa_thread_attr_t *attr,
 345                  bool *appscheduled)
 346 {
 347         int policy, err;
 348
 349         err = pthread_attr_getschedpolicy (attr, &policy);
 350         if (err)
 351                 return err;
 352
 353         *appscheduled = (policy == SCHED_APP);
 354         return 0;
 355 }
 356
 357
 358 int fosa_thread_attr_set_appsched_params
 359                 (fosa_thread_attr_t *attr,
 360                  const void *param,
 361                  size_t paramsize)
 362 {
 363 #if FOSA_ADS_SCHEDPARAM_MAX > POSIX_APPSCHEDPARAM_MAX
 364 #error  FOSA_ADS_SCHEDPARAM_MAX > POSIX_APPSCHEDPARAM_MAX
 365 #endif
 366         if (paramsize > FOSA_ADS_SCHEDPARAM_MAX)
 367                 return FOSA_EINVAL;
 368
 369         return pthread_attr_setappschedparam (attr, param, paramsize);
 370 }
 371
 372
 373 int fosa_thread_attr_get_appsched_params
 374                 (const fosa_thread_attr_t *attr,
 375                  void *param,
 376                 size_t *paramsize)
 377 {
 378         return pthread_attr_getappschedparam (attr, param, paramsize);
 379 }
 380
 381
 382 int fosa_ads_set_appscheduled
 383                 (fosa_thread_id_t thread,
 384                  bool appscheduled)
 385 {
 386         struct sched_param sp;
 387         int policy_old, policy_new, err;
 388
 389         err = pthread_getschedparam (thread, &policy_old, &sp);
 390         if (err)
 391                 return err;
 392
 393         if (appscheduled) {
 394                 err = pthread_setappscheduler (thread, fosa_scheduler_th);
 395                 if (err)
 396                         return err;
 397
 398                 policy_new = SCHED_APP;
 399         } else {
 400                 policy_new = SCHED_FIFO;
 401         }
 402
 403         if (policy_new != policy_old)
 404                 return pthread_setschedparam (thread, policy_new, &sp);
 405
 406         return 0;
 407 }
 408
 409
 410 int fosa_ads_get_appscheduled
 411                 (fosa_thread_id_t thread,
 412                  bool *appscheduled)
 413 {
 414         struct sched_param sp;
 415         int policy, err;
 416
 417         err = pthread_getschedparam (thread, &policy, &sp);
 418         if (err)
 419                 return err;
 420
 421         *appscheduled = (policy == SCHED_APP);
 422         return 0;
 423 }
 424
 425
 426 int fosa_ads_set_appsched_params
 427                 (fosa_thread_id_t thread,
 428                  const void *param,
 429                  size_t paramsize)
 430 {
 431         return pthread_setappschedparam (thread, param, paramsize);
 432 }
 433
 434
 435 int fosa_ads_get_appsched_params
 436                 (fosa_thread_id_t thread,
 437                  void *param,
 438                  size_t *paramsize)
 439 {
 440         return pthread_getappschedparam (thread, param, paramsize);
 441 }
 442
 443
 444
 445 /*********************************
 446  * ADS actions
 447  *********************************/
 448 int fosa_adsactions_add_reject
 449                 (fosa_ads_actions_t *sched_actions,
 450                  fosa_thread_id_t thread)
 451 {
 452         sched_actions -> rejected = true;
 453         return posix_appsched_actions_addreject (&(sched_actions -> actions), thread);
 454 }
 455
 456 extern inline int fosa2prtk (int prio, int urg);
 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
 464 #ifdef CONFIG_URGENCY
 465         #error "Urgency not supported. Disable this feature in the Makefile"
 466         int fosaprio, policy;
 467         struct sched_param sp;
 468
 469         pthread_getschedparam (thread, &policy, &sp);
 470         fosa_thread_get_prio (thread, &fosaprio);
 471         sp.sched_priority =  fosa2prtk (fosaprio, urgency);
 472         printf ("policy=%d, prio=%d, urg=%d, new_prio=%d\n", policy);
 473         pthread_setschedparam (thread, policy, &sp);
 474 #endif
 475         return posix_appsched_actions_addactivate (&(sched_actions -> actions), thread);
 476 }
 477
 478 int fosa_adsactions_add_suspend
 479                 (fosa_ads_actions_t *sched_actions,
 480                  fosa_thread_id_t thread)
 481 {
 482         sched_actions -> suspended = true;
 483         return posix_appsched_actions_addsuspend (&(sched_actions -> actions), thread);
 484 }
 485
 486 int fosa_adsactions_add_timeout
 487                 (fosa_ads_actions_t *sched_actions,
 488                  fosa_clock_id_t clock_id,
 489                  const struct timespec *at_time)
 490 {
 491         sched_actions -> timeout = *at_time;
 492         sched_actions -> timeout_ptr = &(sched_actions -> timeout);
 493         return 0;
 494 }
 495
 496 int fosa_adsactions_add_thread_notification
 497                 (fosa_ads_actions_t *sched_actions,
 498                  fosa_thread_id_t thread,
 499                  fosa_clock_id_t clock_id,
 500                  const struct timespec *at_time)
 501 {
 502         printf ("BUG: fosa_adsactions_add_thread_notification: Not implemented\n");
 503         exit (-21);
 504         return 0;
 505 }
 506
 507
 508 int fosa_ads_set_handled_signal_set (fosa_signal_t set[], int size)
 509 {
 510         int i;
 511
 512         if (!pthread_equal (pthread_self (), fosa_scheduler_th))
 513                 return EPOLICY;
 514
 515         sigemptyset (&fosa_handled_signals);
 516         for (i = 0; i < size; i++) {
 517                 if (FOSA_SIGNAL_MIN > set [i] || set [i] > FOSA_SIGNAL_MAX)
 518                         return EINVAL;
 519                 sigaddset (&fosa_handled_signals, set[i]);
 520         }
 521         return fosa_set_accepted_signals (set, size);
 522 }
 523
 524
 525 int fosa_signal_queue_scheduler (fosa_signal_t signal, fosa_signal_info_t info)
 526 {
 527         return fosa_signal_queue (signal, info, 0);
 528 }
 529
 530
 531 int fosa_ads_invoke_withdata
 532                 (const void *msg, size_t msg_size, void *reply, size_t *reply_size)
 533 {
 534         struct reply_info *fosa_reply;
 535
 536 #if FOSA_ADS_SCHEDINFO_MAX > POSIX_APPSCHEDINFO_MAX
 537 #error  FOSA_ADS_SCHEDINFO_MAX > POSIX_APPSCHEDINFO_MAX
 538 #endif
 539
 540         if (msg_size > FOSA_ADS_SCHEDINFO_MAX)
 541                 return EINVAL;
 542
 543         fosa_reply = (struct reply_info *) pthread_getspecific (fosa_reply_key);
 544         fosa_reply -> msg = reply;
 545         fosa_reply -> size_ptr = reply_size;
 546
 547         return posix_appsched_invoke_withdata (msg, msg_size, NULL, NULL);
 548 }