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