src/app/rpi-pmsm-test1/appl_utils.c

   1 #include <stdlib.h>
   2 #include <stdio.h>
   3 #include <string.h>
   4 #include <sys/types.h>
   5 #include <sys/stat.h>
   6 #include <fcntl.h>
   7 #include <stdint.h>
   8 #include <unistd.h>
   9 #include <sys/mman.h>  /* this provides mlockall() */
  10 #include <pthread.h>
  11 #include <signal.h>
  12 #include <time.h>
  13
  14 #include "appl_utils.h"
  15
  16 int appl_base_task_prio;
  17
  18 int create_rt_task(pthread_t *thread, int prio, void *(*start_routine) (void *), void *arg)
  19 {
  20   int ret ;
  21
  22   pthread_attr_t attr;
  23   struct sched_param schparam;
  24
  25   if (pthread_attr_init(&attr) != 0) {
  26     fprintf(stderr, "pthread_attr_init failed\n");
  27     return -1;
  28   }
  29
  30   if (pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) != 0) {
  31     fprintf(stderr, "pthread_attr_setinheritsched failed\n");
  32     return -1;
  33   }
  34
  35   if (pthread_attr_setschedpolicy(&attr, SCHED_FIFO) != 0) {
  36     fprintf(stderr, "pthread_attr_setschedpolicy SCHED_FIFO failed\n");
  37     return -1;
  38   }
  39
  40   schparam.sched_priority = prio;
  41
  42   if (pthread_attr_setschedparam(&attr, &schparam) != 0) {
  43     fprintf(stderr, "pthread_attr_setschedparam failed\n");
  44     return -1;
  45   }
  46
  47   ret = pthread_create(thread, &attr, start_routine, arg);
  48
  49   pthread_attr_destroy(&attr);
  50
  51   return ret;
  52 }
  53
  54 int sample_period_setup(sample_period_t *sper, unsigned long pernsec)
  55 {
  56   sper->period_nsec = pernsec;
  57   clock_gettime(CLOCK_MONOTONIC, &sper->period_time);
  58   return 0;
  59 }
  60
  61 int sample_period_wait_next(sample_period_t *sper)
  62 {
  63   sper->period_time.tv_nsec += sper->period_nsec;
  64   if (sper->period_time.tv_nsec > 1000*1000*1000) {
  65     sper->period_time.tv_nsec -= 1000*1000*1000;
  66     sper->period_time.tv_sec += 1;
  67   }
  68   return clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &sper->period_time, NULL);
  69 }
  70
  71 void appl_sig_handler(int sig)
  72 {
  73     appl_stop();
  74     exit(1);
  75 }
  76
  77 int appl_setup_environment(const char *argv0)
  78 {
  79   struct sigaction sigact;
  80   int fifo_min_prio = sched_get_priority_min(SCHED_FIFO);
  81   int fifo_max_prio = sched_get_priority_max(SCHED_FIFO);
  82
  83   appl_base_task_prio = fifo_max_prio - 20;
  84   if (appl_base_task_prio < fifo_min_prio)
  85     appl_base_task_prio = fifo_min_prio;
  86
  87
  88   if (mlockall(MCL_FUTURE | MCL_CURRENT) < 0) {
  89     fprintf(stderr, "%s: mlockall failed - cannot lock application in memory\n", argv0);
  90     exit(1);
  91   }
  92
  93   atexit(appl_stop);
  94
  95   memset(&sigact, 0, sizeof(sigact));
  96   sigact.sa_handler = appl_sig_handler;
  97   sigaction(SIGINT, &sigact, NULL);
  98   sigaction(SIGTERM, &sigact, NULL);
  99
 100   return 0;
 101 }