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 <fosa_time.h>
  64
  65 #include <sched.h>
  66 #include <signal.h>
  67 #include <unistd.h>
  68 #include <stdio.h>
  69 #include <string.h>
  70
  71 #define REACHS(str) printf ("%s: %s: %d: %s\n", __FILE__, __FUNCTION__, __LINE__, str)
  72
  73 static pthread_t fosa_scheduler_th;                                     // Scheduler thread
  74 static fosa_ads_scheduler_ops_t fosa_scheduler_ops;                     // Scheduler operations
  75 static void * fosa_scheduler_data;                                      // Scheduler parameters
  76 static size_t fosa_scheduler_data_size;
  77 static void * fosa_init_args;                                           // Scheduler initialisation args
  78 static size_t fosa_init_args_size;
  79 static sigset_t fosa_handled_signals;                                   // Signals handled by the scheduler
  80
  81 // Reply info for 'fosa_ads_invoke_withdata ()'
  82 struct reply_info{
  83         void * msg;
  84         size_t * size_ptr;
  85 };
  86 static int fosa_reply_key;
  87
  88 void clear_actions (fosa_ads_actions_t *act)
  89 {
  90         posix_appsched_actions_destroy (&(act -> actions));
  91         posix_appsched_actions_init (&(act -> actions));
  92         act -> activated = false;
  93         act -> suspended = false;
  94         act -> rejected = false;
  95         act -> timeout_ptr = NULL;
  96 }
  97
  98 void *fosa_scheduler_main (void * args)
  99 {
 100         fosa_ads_actions_t actions;
 101         struct posix_appsched_event event;
 102         struct timespec current_time;
 103         posix_appsched_eventset_t accepted_events;
 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                 if (posix_appsched_execute_actions(&(actions.actions), &fosa_handled_signals, actions.timeout_ptr,
 149                     &current_time, &event))
 150                 {
 151                   fosa_scheduler_ops.appsched_error
 152                       (fosa_scheduler_data, 0x0, FOSA_ADS_THREAD_NOT_ATTACHED, &actions);
 153                   continue;
 154                 }
 155
 156                 clear_actions (&actions);
 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                                         clear_actions (&actions);
 168
 169                                         // alloc memory for reply info of 'fosa_ads_invoke_with_data ()'
 170                                         struct reply_info *reply_mem = malloc (sizeof (struct reply_info));
 171                                         if (!reply_mem) {
 172                                           posix_appsched_actions_addreject (&actions.actions, event.thread);
 173                                         } else {
 174                                            posix_appsched_actions_addaccept (&actions.actions, event.thread);
 175                                            pthread_setspecific_for (fosa_reply_key, event.thread, reply_mem);
 176                                         }
 177                                 }
 178                                 break;
 179                         case POSIX_APPSCHED_TERMINATE:
 180                                 fosa_scheduler_ops.thread_terminate
 181                                                 (fosa_scheduler_data,
 182                                                  event.thread,
 183                                                  &actions,
 184                                                  &current_time);
 185                                 break;
 186                         case POSIX_APPSCHED_READY:
 187                                 fosa_scheduler_ops.thread_ready
 188                                                 (fosa_scheduler_data,
 189                                                  event.thread,
 190                                                  &actions,
 191                                                  &current_time);
 192                                 break;
 193                         case POSIX_APPSCHED_BLOCK:
 194                                 fosa_scheduler_ops.thread_block
 195                                                 (fosa_scheduler_data,
 196                                                  event.thread,
 197                                                  &actions,
 198                                                  &current_time);
 199                                 break;
 200                         case POSIX_APPSCHED_CHANGE_SCHED_PARAM:
 201                                 fosa_scheduler_ops.change_sched_param_thread
 202                                                 (fosa_scheduler_data,
 203                                                  event.thread,
 204                                                  &actions,
 205                                                  &current_time);
 206                                 break;
 207                         case POSIX_APPSCHED_EXPLICIT_CALL_WITH_DATA:
 208                         {
 209                                 struct reply_info *fosa_reply;
 210
 211                                 // Get reply memory pointer
 212                                 pthread_getspecific_from (fosa_reply_key, event.thread, (void **) &fosa_reply);
 213                                 fosa_scheduler_ops.explicit_call_with_data
 214                                                 (fosa_scheduler_data,
 215                                                  event.thread,
 216                                                  event.event_info.info,
 217                                                  event.info_size,
 218                                                  fosa_reply -> msg,
 219                                                  fosa_reply -> size_ptr,
 220                                                  &actions,
 221                                                  &current_time);
 222
 223                                // activate the thread unless suspended or already activated
 224                                 if (!actions.suspended && !actions.activated) {
 225                                         posix_appsched_actions_addactivate
 226                                                         (&actions.actions, event.thread);
 227                                 }
 228
 229                         } break;
 230                         case  POSIX_APPSCHED_TIMEOUT:
 231                                 fosa_scheduler_ops.timeout
 232                                                 (fosa_scheduler_data,
 233                                                  &actions,
 234                                                  &current_time);
 235                                 break;
 236                         case POSIX_APPSCHED_SIGNAL:
 237                                 fosa_scheduler_ops.signal
 238                                                 (fosa_scheduler_data,
 239                                                  event.event_info.siginfo.si_signo,
 240                                                  (fosa_signal_info_t)event.event_info.siginfo.si_value.sival_ptr,
 241                                                  &actions,
 242                                                  &current_time);
 243                                 break;
 244 /*                              - NOT IMPLEMENTED
 245
 246                         case POSIX_APPSCHED_THREAD_NOTIFICATION:
 247                                 fosa_scheduler_ops.notification_for_thread
 248                                                 (fosa_scheduler_data,
 249                                                  event.thread,(fosa_clock_id_t) event.event_info.info,
 250                                                  &actions,
 251                                                  &current_time);
 252                                 break;
 253
 254                         case POSIX_APPSCHED_ERROR: // Implemented using the usual error handling mechanism
 255                                 fosa_scheduler_ops.appsched_error
 256                                         (fosa_scheduler_data,
 257                                          event.thread,
 258                                          0,
 259                                          &actions);
 260                                 break;
 261
 262 */
 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 }