src_marte/test_fosa_ads.c

   1 #include "fosa_app_def_sched.h"
   2 #include "fosa_threads_and_signals.h"
   3 #include "fosa_mutexes_and_condvars.h"
   4 #include "fosa_clocks_and_timers.h"
   5 #include <stdio.h>  // for printf
   6 #include <string.h> // for strlen, strncpy
   7 #include <unistd.h> // for sleep
   8 #include <time.h> // for nanosleep
   9 #include <misc/load.h> // for adjust, eat
  10
  11 // TODO: use #include <misc/assert.h> when it works for all architectures
  12 #include <stdlib.h> // for exit in assert
  13
  14 static void inline assert(expression)
  15 {
  16    if (!expression) {
  17       printe(__FILE__":%u: failed assertion.\n", __LINE__);
  18       exit (-1);
  19    }
  20 }
  21
  22 ///////////////////////////////////////////////////////
  23 //  0) Function prototypes
  24 ///////////////////////////////////////////////////////
  25
  26 static void *thread_code(void *arg);
  27
  28 static void frsh_callback_init
  29                 (void *sched_data, void *arg);
  30
  31 static void frsh_callback_new_thread
  32                 (void *sched_data,
  33                  frsh_thread_id_t thread,
  34                  fosa_ads_actions_t *actions,
  35                  struct timespec *current_time);
  36
  37 static void frsh_callback_thread_terminate
  38                 (void *sched_data,
  39                  frsh_thread_id_t thread,
  40                  fosa_ads_actions_t *actions,
  41                  struct timespec *current_time);
  42
  43 static void frsh_callback_thread_ready
  44                 (void *sched_data,
  45                  frsh_thread_id_t thread,
  46                  fosa_ads_actions_t *actions,
  47                  struct timespec *current_time);
  48
  49 static void frsh_callback_thread_block
  50                 (void *sched_data,
  51                  frsh_thread_id_t thread,
  52                  fosa_ads_actions_t *actions,
  53                  struct timespec *current_time);
  54
  55 static void frsh_callback_change_sched_param_thread
  56                 (void *sched_data,
  57                  frsh_thread_id_t thread,
  58                  fosa_ads_actions_t *actions,
  59                  struct timespec *current_time);
  60
  61 static void frsh_callback_explicit_call_with_data
  62                 (void *sched_data,
  63                  frsh_thread_id_t thread,
  64                  const void * msg,
  65                  size_t msg_size,
  66                  void *reply,
  67                  size_t *reply_size,
  68                  fosa_ads_actions_t *actions,
  69                  struct timespec *current_time);
  70
  71 static void frsh_callback_notification_for_thread
  72                 (void *sched_data,
  73                  frsh_thread_id_t thread,
  74                  fosa_clock_id_t clock,
  75                  fosa_ads_actions_t *actions,
  76                  struct timespec *current_time);
  77
  78 static void frsh_callback_timeout
  79                 (void *sched_data,
  80                  fosa_ads_actions_t *actions,
  81                  struct timespec *current_time);
  82
  83 static void frsh_callback_signal
  84                 (void *sched_data,
  85                  frsh_signal_t signal,
  86                  frsh_signal_info_t siginfo,
  87                  fosa_ads_actions_t *actions,
  88                  struct timespec *current_time);
  89
  90 static void frsh_callback_appsched_error
  91                 (void *sched_data,
  92                  frsh_thread_id_t thread,
  93                  fosa_ads_error_cause_t cause,
  94                  fosa_ads_actions_t *actions);
  95
  96 ///////////////////////////////////////////////////////
  97 //  1) Types, Module variables and Constants
  98 ///////////////////////////////////////////////////////
  99
 100 static fosa_ads_scheduler_ops_t scheduler_ops = {
 101     .init = frsh_callback_init,
 102     .new_thread = frsh_callback_new_thread,
 103     .thread_terminate = frsh_callback_thread_terminate,
 104     .thread_ready = frsh_callback_thread_ready,
 105     .thread_block = frsh_callback_thread_block,
 106     .change_sched_param_thread = frsh_callback_change_sched_param_thread,
 107     .explicit_call_with_data = frsh_callback_explicit_call_with_data,
 108     .notification_for_thread = frsh_callback_notification_for_thread,
 109     .timeout = frsh_callback_timeout,
 110     .signal = frsh_callback_signal,
 111     .appsched_error = frsh_callback_appsched_error,
 112 };
 113
 114 static frsh_mutex_t lock;
 115
 116 const unsigned int MX_INIT_ARGS_STRING = 30;
 117 const unsigned int MX_PRIO = 20;
 118 #define MX_THREADS  2
 119
 120 // general control structure for the scheduler
 121 typedef struct {
 122     fosa_ads_urgency_t array_prio[MX_THREADS];
 123     int array_status[MX_THREADS];
 124 } frsh_sched_data_t;
 125
 126 typedef struct {
 127     fosa_ads_urgency_t prio;
 128 } appsched_params_t;
 129
 130 typedef enum {
 131     ACTIVATE,
 132     SUSPEND
 133 } command_t;
 134
 135 typedef struct {
 136     frsh_thread_id_t thread_id;
 137     command_t command;
 138 } signal_action_t;
 139
 140 ///////////////////////////////////////////////////////
 141 //  2) Main program
 142 ///////////////////////////////////////////////////////
 143
 144 int main () {
 145
 146     int err;
 147     char init_args_string[MX_INIT_ARGS_STRING + 1];
 148     frsh_thread_id_t tid1, tid2, tid3;
 149     frsh_thread_attr_t th_attr;
 150     bool is_appsched;
 151     appsched_params_t params;
 152     frsh_signal_info_t timer_info;
 153     fosa_timer_id_t timerid;
 154     struct timespec timerval;
 155     signal_action_t signal_action;
 156
 157     adjust(); // for function eat
 158
 159     err = fosa_mutex_init(&lock, MX_PRIO);
 160     printf("mutex initialized with ceiling %d. err=%d\n", MX_PRIO, err);
 161     assert(err == 0);
 162
 163     //////////////////////////////////
 164     //  2a) Create the scheduler
 165     //////////////////////////////////
 166
 167     printf("--------------------------------------------------\n");
 168     printf("2a) Create the scheduler\n");
 169
 170     strncpy(init_args_string, "Hello Scheduler!!", MX_INIT_ARGS_STRING);
 171     init_args_string[MX_INIT_ARGS_STRING] = '\0';
 172
 173     err = fosa_ads_scheduler_create
 174             (&scheduler_ops,
 175              sizeof(frsh_sched_data_t),
 176              init_args_string, // INIT IS CALLED INMEDIATELY??!!!
 177              strlen(init_args_string) + 1);
 178     printf("scheduler created, err=%d\n", err);
 179     assert(err == 0);
 180
 181     //////////////////////////////////////////////////////////////
 182     //  2b) Create two application scheduled threads
 183     //////////////////////////////////////////////////////////////
 184
 185     printf("--------------------------------------------------\n");
 186     printf("2b) Create two application scheduled threads\n");
 187
 188     err = frsh_thread_attr_init(&th_attr);
 189     printf("thread attributes object initialized, err=%d\n", err);
 190     assert(err == 0);
 191
 192     err = frsh_thread_attr_set_stacksize(&th_attr,40000);
 193     printf("thread attr set stack size to 40000, err=%d\n", err);
 194     assert(err == 0);
 195
 196     err = fosa_thread_attr_set_prio(&th_attr, MX_PRIO - 1);
 197     printf("thread attr set prio to 10, err=%d\n",err);
 198     assert(err == 0);
 199
 200     err = fosa_thread_attr_set_appscheduled(&th_attr, true);
 201     printf("thread attr set appscheduled, err=%d\n", err);
 202     assert(err == 0);
 203
 204     err = fosa_thread_attr_get_appscheduled(&th_attr, &is_appsched);
 205     printf("thread attr get appscheduled=%d, err=%d\n",is_appsched, err);
 206     assert(err == 0);
 207
 208     params.prio = 10;
 209     err = fosa_thread_attr_set_appsched_params
 210             (&th_attr, (void *)&params, sizeof(params));
 211     printf("thread attr set params prio=%d, err=%d\n", params.prio, err);
 212     assert(err == 0);
 213
 214     // create thread 1
 215     err = fosa_thread_create (&tid1, &th_attr, thread_code, (void *)1);
 216     printf("creating thread 1 with prio=%d, err=%d\n", params.prio, err);
 217     assert(err == 0);
 218
 219     params.prio = 15;
 220     err = fosa_thread_attr_set_appsched_params
 221             (&th_attr, (void *)&params, sizeof(params));
 222     printf("thread attr set params prio=%d, err=%d\n", params.prio, err);
 223     assert(err == 0);
 224
 225     // create thread 2
 226     err = fosa_thread_create (&tid2, &th_attr, thread_code, (void *)2);
 227     printf("creating thread 2 with prio=%d, err=%d\n",params.prio, err);
 228     assert(err == 0);
 229
 230     err = frsh_thread_attr_destroy(&th_attr);
 231     printf("thread attributes object destroyed, err=%d\n",err);
 232     assert(err == 0);
 233
 234     // create thread 3 - should be rejected
 235     err = fosa_thread_create (&tid3, &th_attr, thread_code, (void *)3);
 236     printf("creating thread 3 (rejected=%d), err=%d\n",
 237             (err == FOSA_EREJECT), err);
 238
 239     err = frsh_thread_attr_destroy(&th_attr);
 240     printf("thread attributes object destroyed, err=%d\n",err);
 241     assert(err == 0);
 242
 243     sleep (5);
 244
 245     //////////////////////////////////////////////////////////////
 246     //  2c) Change appsched params
 247     //////////////////////////////////////////////////////////////
 248     printf("--------------------------------------------------\n");
 249     printf("2c) Change appsched params\n");
 250
 251     err = fosa_thread_set_prio(tid1,14);
 252     printf("priority of thread 1 set to 14, err=%d\n", err);
 253     assert(err == 0);
 254
 255     sleep (1);
 256
 257     params.prio = MX_PRIO - 5;
 258     err = fosa_ads_set_appsched_params(tid2, (void *)&params, sizeof(params));
 259     printf("urgency of thread 2 set to %d, err=%d\n", params.prio, err);
 260     assert(err == 0);
 261
 262     sleep (3);
 263
 264     //////////////////////////////////////////////////////////////
 265     //  2d) Dettach from application scheduler
 266     //////////////////////////////////////////////////////////////
 267     printf("--------------------------------------------------\n");
 268     printf("2d) Dettach from application scheduler\n");
 269
 270     err = fosa_ads_set_appscheduled (tid1, false);
 271     printf("dettach thread 1, err=%d\n", err);
 272     assert(err == 0);
 273
 274     err = fosa_ads_set_appscheduled (tid2, false);
 275     printf("dettach thread 2, err=%d\n", err);
 276     assert(err == 0);
 277
 278     err = fosa_ads_get_appscheduled (tid1, &is_appsched);
 279     printf("thread 1 is dettached? %d, err=%d\n", is_appsched, err);
 280     assert(err == 0);
 281
 282     err = fosa_ads_get_appscheduled (tid2, &is_appsched);
 283     printf("thread 2 is dettached? %d, err=%d\n", is_appsched, err);
 284     assert(err == 0);
 285
 286     signal_action.command = SUSPEND;
 287     signal_action.thread_id = tid1;
 288     timer_info.sival_ptr = (void *)&signal_action;
 289
 290     err = fosa_timer_create
 291             (FOSA_CLOCK_REALTIME, FOSA_SIGNAL_MAX, timer_info, &timerid);
 292     printf("timer created, err=%d\n", err);
 293     assert(err == 0);
 294
 295     timerval.tv_sec = 0;
 296     timerval.tv_nsec = 500000000;
 297
 298     err=fosa_timer_arm(timerid, false, &timerval);
 299     printf("suspend thread 1 in 0.5 secs, err=%d\n", err);
 300     assert(err == 0);
 301
 302     sleep (3);
 303
 304     signal_action.command = ACTIVATE;
 305     err=fosa_timer_arm(timerid, false, &timerval);
 306     printf("activate thread 1 in 0.5 secs, err=%d\n", err);
 307     assert(err == 0);
 308
 309     fosa_timer_delete(timerid);
 310
 311     sleep(1000);
 312     return 0;
 313 }
 314
 315 ///////////////////////////////////////////////////////
 316 // 3) Threads Code
 317 ///////////////////////////////////////////////////////
 318
 319 static void *thread_code (void * arg)
 320 {
 321     struct timespec ts = {2, 0};
 322     int err, i;
 323     char whoami[] = "who am i?";
 324     int reply;
 325     size_t reply_size;
 326
 327     printf("Thread %d starts\n", (int)arg);
 328     for (i=1;i<50;i++) {
 329         nanosleep (&ts, NULL);
 330
 331         eat (2.0);
 332
 333         err=fosa_mutex_lock(&lock);
 334         printf("mutex locked by %d. err=%d\n", (int)arg, err);
 335         assert(err == 0);
 336
 337         eat (1.0);
 338
 339         err=fosa_mutex_unlock(&lock);
 340         printf("mutex unlocked %d. err=%d\n", (int)arg, err);
 341         assert(err == 0);
 342
 343         err = fosa_ads_invoke_withdata
 344                 (whoami, sizeof(whoami), (void *)&reply, &reply_size);
 345         printf("who am I, correct?: %d, err=%d\n", reply == (int)arg, err);
 346         assert(err == 0);
 347
 348         eat (1.0);
 349     }
 350     return NULL;
 351 }
 352
 353 ///////////////////////////////////////////////////////
 354 // 4) Implementation of Application Defined Callbacks
 355 ///////////////////////////////////////////////////////
 356 static void frsh_callback_init
 357         (void *sched_data, void *arg)
 358 {
 359     frsh_sched_data_t *frsh = (frsh_sched_data_t *)sched_data;
 360     int i, err;
 361     frsh_signal_t sched_signal = FOSA_SIGNAL_MAX; // HOW TO PUT SIGNALS IF IS OPAQUE??
 362
 363     printf ("CALLBACK: %s \n", __func__);
 364     printf ("Init args string received: %s \n", (char *)arg);
 365
 366     err = fosa_ads_set_handled_signal_set(&sched_signal, 1);
 367     printf("set signal set, err=%d\n", err);
 368     assert(err == 0);
 369
 370     for (i=0; i<MX_THREADS; i++) {
 371         frsh->array_prio[i] = 0;
 372         frsh->array_status[i] = 0;
 373     }
 374 }
 375
 376 static void frsh_callback_new_thread
 377         (void *sched_data,
 378          frsh_thread_id_t thread,
 379          fosa_ads_actions_t *actions,
 380          struct timespec *current_time)
 381 {
 382     int err;
 383     unsigned int i, index;
 384     frsh_sched_data_t *frsh = (frsh_sched_data_t *)sched_data;
 385     appsched_params_t params;
 386     size_t size;
 387
 388     printf ("CALLBACK: %s \n", __func__);
 389
 390     index = MX_THREADS;
 391     for (i=0; i<MX_THREADS; i++) {
 392         if (frsh->array_status[i] == 0) {
 393             index = i;
 394             break;
 395         }
 396     }
 397
 398     if (index == MX_THREADS) {
 399         err = fosa_adsactions_add_reject(actions, thread);
 400         assert(err == 0);
 401     } else {
 402         // Get the priority for the new thread
 403         size = sizeof(params);
 404         err = fosa_ads_get_appsched_params(thread, (void *)&params, &size);
 405         assert(err == 0);
 406
 407         frsh->array_prio[index] = params.prio;
 408         frsh->array_status[index] = 1;
 409     }
 410 }
 411
 412 static void frsh_callback_thread_terminate
 413         (void *sched_data,
 414          frsh_thread_id_t thread,
 415          fosa_ads_actions_t *actions,
 416          struct timespec *current_time)
 417 {
 418     struct timespec at_time;
 419     int err;
 420
 421     printf ("CALLBACK: %s \n", __func__);
 422
 423     fosa_clock_get_time(FOSA_CLOCK_REALTIME, &at_time);
 424     printf("current time sec=%d nsec=%d\n", at_time.tv_sec, at_time.tv_nsec);
 425     at_time.tv_sec = at_time.tv_sec + 1;
 426
 427     err = fosa_adsactions_add_timeout(actions, FOSA_CLOCK_REALTIME, &at_time);
 428     printf("put a timeout of one second, err=%d\n", err);
 429     assert(err == 0);
 430 }
 431
 432 static void frsh_callback_thread_ready
 433         (void *sched_data,
 434          frsh_thread_id_t thread,
 435          fosa_ads_actions_t *actions,
 436          struct timespec *current_time)
 437 {
 438     printf ("CALLBACK: %s \n", __func__);
 439 }
 440
 441 static void frsh_callback_thread_block
 442         (void *sched_data,
 443          frsh_thread_id_t thread,
 444          fosa_ads_actions_t *actions,
 445          struct timespec *current_time)
 446 {
 447     int err;
 448     struct timespec at_time;
 449
 450     printf ("CALLBACK: %s \n", __func__);
 451
 452     fosa_clock_get_time(FOSA_CLOCK_REALTIME, &at_time);
 453     printf("current time sec=%d nsec=%d\n", at_time.tv_sec, at_time.tv_nsec);
 454     at_time.tv_sec = at_time.tv_sec + 1;
 455
 456     err = fosa_adsactions_add_timeout(actions, FOSA_CLOCK_REALTIME, &at_time);
 457     printf("put a timeout of one second, err=%d\n", err);
 458     assert(err == 0);
 459 }
 460
 461 static void frsh_callback_change_sched_param_thread
 462         (void *sched_data,
 463          frsh_thread_id_t thread,
 464          fosa_ads_actions_t *actions,
 465          struct timespec *current_time)
 466 {
 467     printf ("CALLBACK: %s \n", __func__);
 468     // HOW DO I KNOW THE REASON IS SETPRIO OR SETSCHEDPARAMS????
 469 }
 470
 471 static void frsh_callback_explicit_call_with_data
 472         (void *sched_data,
 473          frsh_thread_id_t thread,
 474          const void * msg,
 475          size_t msg_size,
 476          void *reply,
 477          size_t *reply_size,
 478          fosa_ads_actions_t *actions,
 479          struct timespec *current_time)
 480 {
 481     printf ("CALLBACK: %s \n", __func__);
 482     printf ("received command: %s \n", (char *)msg);
 483     *((int *)reply) = (int)thread;
 484 }
 485
 486 static void frsh_callback_notification_for_thread
 487         (void *sched_data,
 488          frsh_thread_id_t thread,
 489          fosa_clock_id_t clock,
 490          fosa_ads_actions_t *actions,
 491          struct timespec *current_time)
 492 {
 493     printf ("CALLBACK: %s \n", __func__);
 494 }
 495
 496 static void frsh_callback_timeout // WHY NOT WATCHDOG????
 497         (void *sched_data,
 498          fosa_ads_actions_t *actions,
 499          struct timespec *current_time) // WHY IS IN OUT and not IN???
 500 {
 501     printf ("CALLBACK: %s \n", __func__);
 502     printf("current time sec=%d nsec=%d\n",
 503             current_time->tv_sec, current_time->tv_nsec);
 504 }
 505
 506 static void frsh_callback_signal
 507         (void *sched_data,
 508          frsh_signal_t signal,
 509          frsh_signal_info_t siginfo, // HOW DO I KNOW THE ORIGIN (see signal_info type)
 510          fosa_ads_actions_t *actions,
 511          struct timespec *current_time)
 512 {
 513     int err;
 514     signal_action_t *signal_action = (signal_action_t *)siginfo.sival_ptr;
 515     frsh_sched_data_t *frsh = (frsh_sched_data_t *)sched_data;
 516
 517     printf ("CALLBACK: %s \n", __func__);
 518
 519     if (signal_action->command == SUSPEND) {
 520         err = fosa_adsactions_add_suspend(actions, signal_action->thread_id);
 521         printf("suspending thread 1 , err=%d\n", err);
 522         assert(err == 0);
 523     } else {
 524         err = fosa_adsactions_add_activate
 525                 (actions, signal_action->thread_id, frsh->array_prio[0]);
 526         printf("activating thread 1, err=%d\n", err);
 527         assert(err == 0);
 528     }
 529 }
 530
 531 static void frsh_callback_appsched_error
 532         (void *sched_data,
 533          frsh_thread_id_t thread,
 534          fosa_ads_error_cause_t cause,
 535          fosa_ads_actions_t *actions)
 536 {
 537     printf ("CALLBACK: %s \n", __func__);
 538 }