Bela
Real-time, ultra-low-latency audio and sensor processing system for BeagleBone Black
 All Classes Files Functions Variables Typedefs Macros Groups
xenomai_wraps.h
1 #ifndef XENOMAI_WRAPS_H
2 #define XENOMAI_WRAPS_H
3 
4 #include "Bela.h"
5 #if !defined(XENOMAI_MAJOR)
6 #error Xenomai version not set
7 #endif
8 #if !defined(XENOMAI_SKIN_native) && !defined(XENOMAI_SKIN_posix)
9 #error Xenomai skin unsupported or not defined
10 #endif
11 
12 #include <time.h>
13 #include <stdio.h>
14 #include <string.h>
15 
16 #ifdef __cplusplus
17 extern "C" {
18 #endif
19 #ifdef XENOMAI_SKIN_posix
20 #include <pthread.h>
21 #include <mqueue.h>
22 #include <sys/socket.h>
23 
24 // Forward declare __wrap_ versions of POSIX calls.
25 // At link time, Xenomai will provide implementations for these
26 int __wrap_nanosleep(const struct timespec *req, struct timespec *rem);
27 int __wrap_pthread_create(pthread_t *thread, const pthread_attr_t *attr, void *(*start_routine) (void *), void *arg);
28 int __wrap_pthread_setschedparam(pthread_t thread, int policy, const struct sched_param *param);
29 int __wrap_pthread_getschedparam(pthread_t thread, int *policy, struct sched_param *param);
30 int __wrap_pthread_yield(void);
31 
32 int __wrap_pthread_mutex_init(pthread_mutex_t *mutex, const pthread_mutexattr_t *attr);
33 int __wrap_pthread_mutex_destroy(pthread_mutex_t *mutex);
34 int __wrap_pthread_mutex_lock(pthread_mutex_t *mutex);
35 int __wrap_pthread_mutex_trylock(pthread_mutex_t *mutex);
36 int __wrap_pthread_mutex_unlock(pthread_mutex_t *mutex);
37 
38 int __wrap_pthread_cond_destroy(pthread_cond_t *cond);
39 int __wrap_pthread_cond_init(pthread_cond_t *cond, const pthread_condattr_t *attr);
40 int __wrap_pthread_cond_signal(pthread_cond_t *cond);
41 int __wrap_pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex);
42 
43 int __wrap_socket(int protocol_family, int socket_type, int protocol);
44 int __wrap_setsockopt(int fd, int level, int optname, const void *optval, socklen_t optlen);
45 int __wrap_bind(int fd, const struct sockaddr *my_addr, socklen_t addrlen);
46 ssize_t __wrap_sendto(int fd, const void *buf, size_t len, int flags, const struct sockaddr *to, socklen_t tolen);
47 
48 mqd_t __wrap_mq_open(const char *name, int oflags, ...);
49 int __wrap_mq_close(mqd_t mqdes);
50 ssize_t __wrap_mq_receive(mqd_t mqdes, char *msg_ptr, size_t msg_len, unsigned *msg_prio);
51 int __wrap_mq_send(mqd_t mqdes, const char *msg_ptr, size_t msg_len, unsigned msg_prio);
52 int __wrap_mq_unlink(const char *name);
53 
54 // Handle difference between posix API of Xenomai 2.6 and Xenomai 3
55 // Some functions are not wrapped by Xenomai 2.6, so we redefine the __wrap
56 // to the actual POSIX service for Xenomai 2.6 while we simply forward declare
57 // the __wrap_ version for Xenomai 3
58 #if XENOMAI_MAJOR == 2
59 #define __wrap_pthread_join(a,b) pthread_join(a,b) // NOWRAP
60 #define __wrap_pthread_attr_init(a) pthread_attr_init(a) // NOWRAP
61 #define __wrap_sched_get_priority_max(a) sched_get_priority_max(a) // NOWRAP
62 #endif
63 #if XENOMAI_MAJOR == 3
64 int __wrap_pthread_join(pthread_t thread, void **retval);
65 int __wrap_pthread_attr_init(pthread_attr_t *attr);
66 int __wrap_sched_get_priority_max(int policy);
67 #endif
68 #endif /* XENOMAI_SKIN_posix */
69 
70 #ifdef XENOMAI_SKIN_native
71 #include <native/task.h>
72 typedef RTIME time_ns_t;
73 #endif
74 #ifdef XENOMAI_SKIN_posix
75 #if XENOMAI_MAJOR == 3
76 #include <rtdm/ipc.h>
77 #else
78 #include <rtdm/rtipc.h>
79 #endif
80 typedef long long int time_ns_t;
81 typedef void *(pthread_callback_t)(void *);
82 #endif
83 
84 inline int task_sleep_ns(long long int timens)
85 {
86 #ifdef XENOMAI_SKIN_native
87  return rt_task_sleep((RTIME) timens);
88 #endif
89 #ifdef XENOMAI_SKIN_posix
90  struct timespec req;
91  req.tv_sec = timens/1000000000;
92  req.tv_nsec = timens - req.tv_sec * 1000000000;
93  return __wrap_nanosleep(&req, NULL);
94 #endif
95 }
96 
97 #ifdef XENOMAI_SKIN_posix
98 #include <error.h>
99 //void error(int exitCode, int errno, char* message)
100 //{
101  //fprintf(stderr, "Error during `%s`: %d %s\n", message, errno, strerror(errno));
102  //exit(exitCode);
103 //}
104 
105 // got this from xenomai-3/testsuite/latency/latency.c
106 inline void setup_sched_parameters(pthread_attr_t *attr, int prio)
107 {
108  struct sched_param p;
109  int ret;
110  ret = pthread_attr_setinheritsched(attr, PTHREAD_EXPLICIT_SCHED);
111  if (ret)
112  error(1, ret, "pthread_attr_setinheritsched()");
113 
114  ret = pthread_attr_setschedpolicy(attr, prio ? SCHED_FIFO : SCHED_OTHER);
115  if (ret)
116  error(1, ret, "pthread_attr_setschedpolicy()");
117 
118  p.sched_priority = prio;
119  ret = pthread_attr_setschedparam(attr, &p);
120  if (ret)
121  error(1, ret, "pthread_attr_setschedparam()");
122 }
123 
124 inline int set_thread_stack_and_priority(pthread_attr_t *attr, int stackSize, int prio)
125 {
126  if(pthread_attr_setdetachstate(attr, PTHREAD_CREATE_JOINABLE))
127  {
128  fprintf(stderr, "Error: unable to set detachstate\n");
129  return -1;
130  }
131  if(stackSize <= 0)
132  {
133  stackSize = 65536;
134  }
135  if(pthread_attr_setstacksize(attr, stackSize))
136  {
137  fprintf(stderr, "Error: unable to set stack size to %d\n", stackSize);
138  return -1;
139  }
140  setup_sched_parameters(attr, prio);
141  return 0;
142 }
143 inline int create_and_start_thread(pthread_t* task, const char* taskName, int priority, int stackSize, pthread_callback_t* callback, void* arg)
144 {
145  pthread_attr_t attr;
146  if(__wrap_pthread_attr_init(&attr))
147  {
148  fprintf(stderr, "Error: unable to init thread attributes\n");
149  return -1;
150  }
151  if(int ret = set_thread_stack_and_priority(&attr, stackSize, priority))
152  {
153  return ret;
154  }
155  if(int ret = __wrap_pthread_create(task, &attr, callback, arg))
156  {
157  return ret;
158  }
159 #if XENOMAI_MAJOR == 2
160  // note the different spelling. Worst thing is that
161  // pthread_setname_np would still compile and run (because it is a POSIX
162  // extension provided by Linux), but would not have the desired effect.
163  pthread_set_name_np(*task, taskName);
164 #endif
165 #if XENOMAI_MAJOR == 3
166  __wrap_pthread_setname_np(*task, taskName);
167 #endif
168  // check that effective parameters match the ones we requested
169  //pthread_attr_t actualAttr;
170  //pthread_getattr_np(*task, &actualAttr);
171  //size_t stk;
172  //pthread_attr_getstacksize(&actualAttr, &stk);
173  //printf("measured stack: %d, requested stack: %d\n", stk, stackSize);
174 
175  pthread_attr_destroy(&attr);
176  return 0;
177 }
178 // from xenomai-3/demo/posix/cobalt/xddp-echo.c
179 inline int createXenomaiPipe(const char* portName, int poolsz)
180 {
181  /*
182  * Get a datagram socket to bind to the RT endpoint. Each
183  * endpoint is represented by a port number within the XDDP
184  * protocol namespace.
185  */
186  int s = __wrap_socket(AF_RTIPC, SOCK_DGRAM, IPCPROTO_XDDP);
187  if (s < 0) {
188  fprintf(stderr, "Failed call to socket\n");
189  return -1;
190  }
191 
192  /*
193  * Set a port label. This name will be registered when
194  * binding
195  */
196  struct rtipc_port_label plabel;
197  strcpy(plabel.label, portName);
198  int ret = __wrap_setsockopt(s, SOL_XDDP, XDDP_LABEL,
199  &plabel, sizeof(plabel));
200  /*
201  * Set a local pool for the RT endpoint. Memory needed to
202  * convey datagrams will be pulled from this pool, instead of
203  * Xenomai's system pool.
204  */
205  if(poolsz == 0)
206  poolsz = 16384; /* bytes */
207  ret = __wrap_setsockopt(s, SOL_XDDP, XDDP_POOLSZ,
208  &poolsz, sizeof(poolsz));
209  if (ret)
210  {
211  fprintf(stderr, "Failed call to __wrap_setsockopt\n");
212  return -1;
213  }
214 
215  /*
216  * Bind the socket to the port, to setup a proxy to channel
217  * traffic to/from the Linux domain.
218  */
219  struct sockaddr_ipc saddr;
220  memset(&saddr, 0, sizeof(saddr));
221  saddr.sipc_family = AF_RTIPC;
222  saddr.sipc_port = -1; // automatically assign port number
223  ret = __wrap_bind(s, (struct sockaddr *)&saddr, sizeof(saddr));
224  if (ret)
225  {
226  fprintf(stderr, "Failed call to __wrap_bind\n");
227  return -1;
228  }
229  return s;
230 }
231 #endif /* XENOMAI_SKIN_posix */
232 
233 #ifdef __cplusplus
234 }
235 #endif
236 
237 #endif /* XENOMAI_WRAPS_H */
Main Bela public API.