1  #include "config.h"
  2  #include "rtapi.h"
  3  #include "rtapi_uspace.hh"
  4  #pragma GCC diagnostic push
  5  #pragma GCC diagnostic ignored "-Wnarrowing"
  6  #include <rtai_lxrt.h>
  7  #pragma GCC diagnostic pop
  8  #include <atomic>
  9  #ifdef HAVE_SYS_IO_H
 10  #include <sys/io.h>
 11  #endif
 12  
 13  namespace
 14  {
 15  RtapiApp *app;
 16  
 17  struct RtaiTask : rtapi_task {
 18      RtaiTask() : rtapi_task{}, cancel{}, rt_task{}, thr{} {}
 19      std::atomic<int> cancel;
 20      RT_TASK *rt_task;
 21      pthread_t thr;
 22  };
 23  
 24  template<class T=rtapi_task>
 25  T *get_task(int task_id) {
 26      return static_cast<T*>(RtapiApp::get_task(task_id));
 27  }
 28  
 29  
 30  struct RtaiApp : RtapiApp {
 31      RtaiApp() : RtapiApp(SCHED_FIFO) {
 32          pthread_once(&key_once, init_key);
 33      }
 34  
 35      RtaiTask *do_task_new() {
 36          return new RtaiTask;
 37      }
 38  
 39      int task_delete(int id) {
 40          auto task = ::get_task<RtaiTask>(id);
 41          if(!task) return -EINVAL;
 42  
 43          task->cancel = 1;
 44          pthread_join(task->thr, nullptr);
 45          task->magic = 0;
 46          task_array[id] = 0;
 47          delete task;
 48          return 0;
 49      }
 50  
 51      int task_start(int task_id, unsigned long period_nsec) {
 52          auto task = ::get_task<RtaiTask>(task_id);
 53          if(!task) return -EINVAL;
 54  
 55          task->period = period_nsec;
 56          struct sched_param param;
 57          memset(&param, 0, sizeof(param));
 58          param.sched_priority = task->prio;
 59  
 60          // PLL functions not supported
 61          task->pll_correction_limit = 0;
 62          task->pll_correction = 0;
 63  
 64          pthread_attr_t attr;
 65          if(pthread_attr_init(&attr) < 0)
 66              return -errno;
 67          if(pthread_attr_setstacksize(&attr, task->stacksize) < 0)
 68              return -errno;
 69          if(pthread_attr_setschedpolicy(&attr, policy) < 0)
 70              return -errno;
 71          if(pthread_attr_setschedparam(&attr, &param) < 0)
 72              return -errno;
 73          if(pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) < 0)
 74              return -errno;
 75          if(pthread_create(&task->thr, &attr, &wrapper, reinterpret_cast<void*>(task)) < 0)
 76              return -errno;
 77  
 78          return 0;
 79      }
 80  
 81      static void *wrapper(void *arg) {
 82          auto task = reinterpret_cast<RtaiTask*>(arg);
 83          pthread_setspecific(key, arg);
 84          task->rt_task = rt_task_init(task->id, task->prio, 0, 0);
 85          rt_set_periodic_mode();
 86          start_rt_timer(nano2count(task->period));
 87          if(task->uses_fp) rt_task_use_fpu(task->rt_task, 1);
 88          // assumes processor numbers are contiguous
 89          int nprocs = sysconf( _SC_NPROCESSORS_ONLN );
 90          rt_set_runnable_on_cpus(task->rt_task, 1u << (nprocs - 1));
 91          rt_make_hard_real_time();
 92          rt_task_make_periodic_relative_ns(task->rt_task, task->period, task->period);
 93          (task->taskcode) (task->arg);
 94  
 95          rtapi_print("ERROR: reached end of wrapper for task %d\n", task->id);
 96          rt_make_soft_real_time();
 97          return nullptr;
 98      }
 99  
100      int task_pause(int task_id) {
101          auto task = ::get_task<RtaiTask>(task_id);
102          if(!task) return -EINVAL;
103          return rt_task_suspend(task->rt_task);
104      }
105  
106      int task_resume(int task_id) {
107          auto task = ::get_task<RtaiTask>(task_id);
108          if(!task) return -EINVAL;
109          return rt_task_resume(task->rt_task);
110      }
111  
112      long long task_pll_get_reference(void) {
113          // PLL functions not supported
114          return 0;
115      }
116  
117      int task_pll_set_correction(long value) {
118          // PLL functions not supported
119          return -EINVAL;
120      }
121  
122      void wait() {
123          int task_id = task_self();
124          auto task = ::get_task<RtaiTask>(task_id);
125          if(task->cancel) {
126              rt_make_soft_real_time();
127              pthread_exit(nullptr);
128          }
129          if(rt_task_wait_period() < 0) unexpected_realtime_delay(task);
130      }
131  
132      unsigned char do_inb(unsigned int port) {
133  #ifdef HAVE_SYS_IO_H
134          return inb(port);
135  #endif
136      }
137  
138      void do_outb(unsigned char val, unsigned int port) {
139  #ifdef HAVE_SYS_IO_H
140          return outb(val, port);
141  #endif
142      }
143  
144      int run_threads(int fd, int (*callback)(int fd)) {
145          while(callback(fd)) { /* nothing */ }
146          return 0;
147      }
148  
149      int task_self() {
150          struct rtapi_task *task = reinterpret_cast<rtapi_task*>(pthread_getspecific(key));
151          if(!task) return -EINVAL;
152          return task->id;
153      }
154  
155      static pthread_once_t key_once;
156      static pthread_key_t key;
157      static void init_key(void) {
158          pthread_key_create(&key, NULL);
159      }
160  
161      long long do_get_time() {
162          return rt_get_cpu_time_ns();
163      }
164  
165      void do_delay(long ns) {
166          rt_sleep(nano2count(ns));
167      }
168  };
169  
170  pthread_once_t RtaiApp::key_once;
171  pthread_key_t RtaiApp::key;
172  }
173  
174  extern "C" RtapiApp *make();
175  
176  RtapiApp *make() {
177      rtapi_print_msg(RTAPI_MSG_ERR, "Note: Using LXRT realtime\n");
178      return app = new RtaiApp;
179  }