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