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