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