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
 103         // accept events which have a callback associated
 104         posix_appsched_fillset (&accepted_events);
 105         if (fosa_scheduler_ops.new_thread)
 106                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_NEW);
 107
 108         if (fosa_scheduler_ops.thread_terminate)
 109                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_TERMINATE);
 110
 111         if (fosa_scheduler_ops.thread_ready)
 112                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_READY);
 113
 114         if (fosa_scheduler_ops.thread_block)
 115                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_BLOCK);
 116
 117         if (fosa_scheduler_ops.change_sched_param_thread)
 118                 posix_appsched_delset (&accepted_events, POSIX_APPSCHED_CHANGE_SCHED_PARAM);
 119
 120         if (fosa_scheduler_ops.explicit_call_with_data)
 121                 posix_appsched_delset (&accepted_events, POSIX_APPSCHED_EXPLICIT_CALL_WITH_DATA);
 122
 123 //      if (fosa_scheduler_ops.notification_for_thread)
 124 //              posix_appsched_delset (&accepted_events,POSIX_APPSCHED_TASK_NOTIFICATION);
 125
 126         if (fosa_scheduler_ops.timeout)
 127                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_TIMEOUT);
 128
 129         if (fosa_scheduler_ops.signal)
 130                 posix_appsched_delset (&accepted_events,POSIX_APPSCHED_SIGNAL);
 131
 132 //      if (fosa_scheduler_ops.signal)
 133 //              posix_appsched_delset (&accepted_events,POSIX_APPSCHED_ERRROR);
 134
 135         posix_appschedattr_seteventmask(&accepted_events);
 136
 137         // Set the clock (and its) flags used by the scheduler
 138         posix_appschedattr_setclock (FOSA_CLOCK_REALTIME);
 139         posix_appschedattr_setflags (POSIX_APPSCHED_ABSTIMEOUT);
 140
 141         // init scheduler
 142         fosa_scheduler_ops.init (fosa_scheduler_data, fosa_init_args);
 143         clear_actions (&actions);
 144
 145         while (1) {     // scheduler loop
 146                 if (posix_appsched_execute_actions(&(actions.actions), &fosa_handled_signals, actions.timeout_ptr,
 147                     &current_time, &event))
 148                 {
 149                   fosa_scheduler_ops.appsched_error
 150                       (fosa_scheduler_data, 0x0, FOSA_ADS_THREAD_NOT_ATTACHED, &actions);
 151                   continue;
 152                 }
 153
 154                 clear_actions (&actions);
 155                 switch (event.event_code) {
 156                         case POSIX_APPSCHED_NEW:
 157                                 fosa_scheduler_ops.new_thread
 158                                                 (fosa_scheduler_data,
 159                                                  event.thread,
 160                                                  &actions,
 161                                                  &current_time);
 162
 163                                 if (!actions.rejected) {
 164
 165                                         clear_actions (&actions);
 166
 167                                         // alloc memory for reply info of 'fosa_ads_invoke_with_data ()'
 168                                         struct reply_info *reply_mem = malloc (sizeof (struct reply_info));
 169                                         if (!reply_mem) {
 170                                           posix_appsched_actions_addreject (&actions.actions, event.thread);
 171                                         } else {
 172                                            posix_appsched_actions_addaccept (&actions.actions, event.thread);
 173                                            pthread_setspecific_for (fosa_reply_key, event.thread, reply_mem);
 174                                         }
 175                                 }
 176                                 break;
 177                         case POSIX_APPSCHED_TERMINATE:
 178                                 fosa_scheduler_ops.thread_terminate
 179                                                 (fosa_scheduler_data,
 180                                                  event.thread,
 181                                                  &actions,
 182                                                  &current_time);
 183                                 break;
 184                         case POSIX_APPSCHED_READY:
 185                                 fosa_scheduler_ops.thread_ready
 186                                                 (fosa_scheduler_data,
 187                                                  event.thread,
 188                                                  &actions,
 189                                                  &current_time);
 190                                 break;
 191                         case POSIX_APPSCHED_BLOCK:
 192                                 fosa_scheduler_ops.thread_block
 193                                                 (fosa_scheduler_data,
 194                                                  event.thread,
 195                                                  &actions,
 196                                                  &current_time);
 197                                 break;
 198                         case POSIX_APPSCHED_CHANGE_SCHED_PARAM:
 199                                 fosa_scheduler_ops.change_sched_param_thread
 200                                                 (fosa_scheduler_data,
 201                                                  event.thread,
 202                                                  &actions,
 203                                                  &current_time);
 204                                 break;
 205                         case POSIX_APPSCHED_EXPLICIT_CALL_WITH_DATA:
 206                         {
 207                                 struct reply_info *fosa_reply;
 208
 209                                 // Get reply memory pointer
 210                                 pthread_getspecific_from (fosa_reply_key, event.thread, (void **) &fosa_reply);
 211                                 fosa_scheduler_ops.explicit_call_with_data
 212                                                 (fosa_scheduler_data,
 213                                                  event.thread,
 214                                                  event.event_info.info,
 215                                                  event.info_size,
 216                                                  fosa_reply -> msg,
 217                                                  fosa_reply -> size_ptr,
 218                                                  &actions,
 219                                                  &current_time);
 220
 221                                // activate the thread unless suspended or already activated
 222                                 if (!actions.suspended && !actions.activated) {
 223                                         posix_appsched_actions_addactivate
 224                                                         (&actions.actions, event.thread);
 225                                 }
 226
 227                         } break;
 228                         case  POSIX_APPSCHED_TIMEOUT:
 229                                 fosa_scheduler_ops.timeout
 230                                                 (fosa_scheduler_data,
 231                                                  &actions,
 232                                                  &current_time);
 233                                 break;
 234                         case POSIX_APPSCHED_SIGNAL:
 235                                 fosa_scheduler_ops.signal
 236                                                 (fosa_scheduler_data,
 237                                                  event.event_info.siginfo.si_signo,
 238                                                  (fosa_signal_info_t)event.event_info.siginfo.si_value.sival_ptr,
 239                                                  &actions,
 240                                                  &current_time);
 241                                 break;
 242 /*                              - NOT IMPLEMENTED
 243
 244                         case POSIX_APPSCHED_THREAD_NOTIFICATION:
 245                                 fosa_scheduler_ops.notification_for_thread
 246                                                 (fosa_scheduler_data,
 247                                                  event.thread,(fosa_clock_id_t) event.event_info.info,
 248                                                  &actions,
 249                                                  &current_time);
 250                                 break;
 251
 252                         case POSIX_APPSCHED_ERROR: // Implemented using the usual error handling mechanism
 253                                 fosa_scheduler_ops.appsched_error
 254                                         (fosa_scheduler_data,
 255                                          event.thread,
 256                                          0,
 257                                          &actions);
 258                                 break;
 259
 260 */
 261                 }
 262         }
 263 }
 264
 265 /********************************
 266  * Application-defined scheduling
 267  ********************************/
 268 int fosa_ads_scheduler_create
 269                 (const fosa_ads_scheduler_ops_t * scheduler_ops,
 270                  size_t scheduler_data_size,
 271                  void * init_args,
 272                  size_t init_args_size)
 273 {
 274         pthread_attr_t attr;
 275         struct sched_param sp;
 276
 277         if (!scheduler_ops)
 278                 return EINVAL;
 279         fosa_scheduler_ops = *scheduler_ops;
 280
 281         // Alloc memory for init args and scheduler data
 282         fosa_init_args_size = init_args_size;
 283         fosa_init_args = malloc (init_args_size);
 284         if (!init_args)
 285                 return EAGAIN;
 286
 287         memcpy (fosa_init_args, init_args, init_args_size);
 288
 289         fosa_scheduler_data_size = scheduler_data_size;
 290         fosa_scheduler_data = malloc (scheduler_data_size);
 291         if (!fosa_scheduler_data) {
 292                 free (init_args);
 293                 return  EAGAIN;
 294         }
 295         memset (fosa_scheduler_data, 0, scheduler_data_size);
 296
 297         // Key for reply information index of 'fosa_reply' for each thread
 298         pthread_key_create (&fosa_reply_key, NULL);
 299
 300         // Set scheduler thread parameters
 301         pthread_attr_init (&attr);
 302         sp.sched_priority = sched_get_priority_max (SCHED_FIFO) + FOSA_ADS_SCHEDULER_PRIO_DIFF;
 303         pthread_attr_setschedparam (&attr, &sp);
 304         pthread_attr_setschedpolicy (&attr, SCHED_FIFO);
 305         pthread_attr_setappschedulerstate (&attr, PTHREAD_APPSCHEDULER);
 306         pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_DETACHED);
 307         pthread_attr_setinheritsched(&attr,PTHREAD_EXPLICIT_SCHED);
 308
 309         if (pthread_create (&fosa_scheduler_th, &attr, fosa_scheduler_main, NULL))
 310                 return errno;
 311         else
 312                 return 0;
 313 }
 314
 315 int fosa_thread_attr_set_appscheduled
 316                 (fosa_thread_attr_t *attr,
 317                  bool appscheduled)
 318 {
 319         int err;
 320
 321         err = pthread_attr_setappscheduler (attr, fosa_scheduler_th);
 322         if (err)
 323                 return err;
 324
 325         if (appscheduled)
 326                 return pthread_attr_setschedpolicy (attr, SCHED_APP);
 327         else
 328                 return pthread_attr_setschedpolicy (attr, SCHED_FIFO);
 329 }
 330
 331
 332 int fosa_thread_attr_get_appscheduled
 333                 (const fosa_thread_attr_t *attr,
 334                  bool *appscheduled)
 335 {
 336         int policy, err;
 337
 338         err = pthread_attr_getschedpolicy (attr, &policy);
 339         if (err)
 340                 return err;
 341
 342         *appscheduled = (policy == SCHED_APP);
 343         return 0;
 344 }
 345
 346
 347 int fosa_thread_attr_set_appsched_params
 348                 (fosa_thread_attr_t *attr,
 349                  const void *param,
 350                  size_t paramsize)
 351 {
 352 #if FOSA_ADS_SCHEDPARAM_MAX > POSIX_APPSCHEDPARAM_MAX
 353 #error  FOSA_ADS_SCHEDPARAM_MAX > POSIX_APPSCHEDPARAM_MAX
 354 #endif
 355         if (paramsize > FOSA_ADS_SCHEDPARAM_MAX)
 356                 return FOSA_EINVAL;
 357
 358         return pthread_attr_setappschedparam (attr, param, paramsize);
 359 }
 360
 361
 362 int fosa_thread_attr_get_appsched_params
 363                 (const fosa_thread_attr_t *attr,
 364                  void *param,
 365                 size_t *paramsize)
 366 {
 367         return pthread_attr_getappschedparam (attr, param, paramsize);
 368 }
 369
 370
 371 int fosa_ads_set_appscheduled
 372                 (fosa_thread_id_t thread,
 373                  bool appscheduled)
 374 {
 375         struct sched_param sp;
 376         int policy_old, policy_new, err;
 377
 378         err = pthread_getschedparam (thread, &policy_old, &sp);
 379         if (err)
 380                 return err;
 381
 382         if (appscheduled) {
 383                 err = pthread_setappscheduler (thread, fosa_scheduler_th);
 384                 if (err)
 385                         return err;
 386
 387                 policy_new = SCHED_APP;
 388         } else {
 389                 policy_new = SCHED_FIFO;
 390         }
 391
 392         if (policy_new != policy_old)
 393                 return pthread_setschedparam (thread, policy_new, &sp);
 394
 395         return 0;
 396 }
 397
 398
 399 int fosa_ads_get_appscheduled
 400                 (fosa_thread_id_t thread,
 401                  bool *appscheduled)
 402 {
 403         struct sched_param sp;
 404         int policy, err;
 405
 406         err = pthread_getschedparam (thread, &policy, &sp);
 407         if (err)
 408                 return err;
 409
 410         *appscheduled = (policy == SCHED_APP);
 411         return 0;
 412 }
 413
 414
 415 int fosa_ads_set_appsched_params
 416                 (fosa_thread_id_t thread,
 417                  const void *param,
 418                  size_t paramsize)
 419 {
 420         return pthread_setappschedparam (thread, param, paramsize);
 421 }
 422
 423
 424 int fosa_ads_get_appsched_params
 425                 (fosa_thread_id_t thread,
 426                  void *param,
 427                  size_t *paramsize)
 428 {
 429         return pthread_getappschedparam (thread, param, paramsize);
 430 }
 431
 432
 433
 434 /*********************************
 435  * ADS actions
 436  *********************************/
 437 int fosa_adsactions_add_reject
 438                 (fosa_ads_actions_t *sched_actions,
 439                  fosa_thread_id_t thread)
 440 {
 441         sched_actions -> rejected = true;
 442         return posix_appsched_actions_addreject (&(sched_actions -> actions), thread);
 443 }
 444
 445 extern inline int fosa2prtk (int prio, int urg);
 446 int fosa_adsactions_add_activate
 447                 (fosa_ads_actions_t *sched_actions,
 448                  fosa_thread_id_t thread,
 449                  fosa_ads_urgency_t urgency)
 450 {
 451         sched_actions -> activated = true;
 452
 453 #ifdef CONFIG_URGENCY
 454         #error "Urgency not supported. Disable this feature in the Makefile"
 455         int fosaprio, policy;
 456         struct sched_param sp;
 457
 458         pthread_getschedparam (thread, &policy, &sp);
 459         fosa_thread_get_prio (thread, &fosaprio);
 460         sp.sched_priority =  fosa2prtk (fosaprio, urgency);
 461 //      printf ("policy=%d, prio=%d, urg=%d, new_prio=%d\n", policy);
 462         pthread_setschedparam (thread, policy, &sp);
 463 #endif
 464         return posix_appsched_actions_addactivate (&(sched_actions -> actions), thread);
 465 }
 466
 467 int fosa_adsactions_add_suspend
 468                 (fosa_ads_actions_t *sched_actions,
 469                  fosa_thread_id_t thread)
 470 {
 471         sched_actions -> suspended = true;
 472         return posix_appsched_actions_addsuspend (&(sched_actions -> actions), thread);
 473 }
 474
 475 int fosa_adsactions_add_timeout
 476                 (fosa_ads_actions_t *sched_actions,
 477                  fosa_clock_id_t clock_id,
 478                  const struct timespec *at_time)
 479 {
 480         sched_actions -> timeout = *at_time;
 481         sched_actions -> timeout_ptr = &(sched_actions -> timeout);
 482         return 0;
 483 }
 484
 485 int fosa_adsactions_add_thread_notification
 486                 (fosa_ads_actions_t *sched_actions,
 487                  fosa_thread_id_t thread,
 488                  fosa_clock_id_t clock_id,
 489                  const struct timespec *at_time)
 490 {
 491         printf ("BUG: fosa_adsactions_add_thread_notification: Not implemented\n");
 492         exit (-21);
 493         return 0;
 494 }
 495
 496
 497 int fosa_ads_set_handled_signal_set (fosa_signal_t set[], int size)
 498 {
 499         int i;
 500
 501         if (!pthread_equal (pthread_self (), fosa_scheduler_th))
 502                 return EPOLICY;
 503
 504         sigemptyset (&fosa_handled_signals);
 505         for (i = 0; i < size; i++) {
 506                 if (FOSA_SIGNAL_MIN > set [i] || set [i] > FOSA_SIGNAL_MAX)
 507                         return EINVAL;
 508                 sigaddset (&fosa_handled_signals, set[i]);
 509         }
 510         return fosa_set_accepted_signals (set, size);
 511 }
 512
 513
 514 int fosa_signal_queue_scheduler (fosa_signal_t signal, fosa_signal_info_t info)
 515 {
 516         return fosa_signal_queue (signal, info, 0);
 517 }
 518
 519
 520 int fosa_ads_invoke_withdata
 521                 (const void *msg, size_t msg_size, void *reply, size_t *reply_size)
 522 {
 523         struct reply_info *fosa_reply;
 524
 525 #if FOSA_ADS_SCHEDINFO_MAX > POSIX_APPSCHEDINFO_MAX
 526 #error  FOSA_ADS_SCHEDINFO_MAX > POSIX_APPSCHEDINFO_MAX
 527 #endif
 528
 529         if (msg_size > FOSA_ADS_SCHEDINFO_MAX)
 530                 return EINVAL;
 531
 532         fosa_reply = (struct reply_info *) pthread_getspecific (fosa_reply_key);
 533         fosa_reply -> msg = reply;
 534         fosa_reply -> size_ptr = reply_size;
 535
 536         return posix_appsched_invoke_withdata (msg, msg_size, NULL, NULL);
 537 }