diff --git a/src/rtapi/Submakefile b/src/rtapi/Submakefile index a51231dc1af..883f9486fc8 100644 --- a/src/rtapi/Submakefile +++ b/src/rtapi/Submakefile @@ -32,6 +32,7 @@ $(patsubst ./rtapi/%,../include/%,$(RTAPIINCS)): ../include/%.h: ./rtapi/%.h ifeq ($(BUILD_SYS),uspace) RTAPI_APP_SRCS := \ + rtapi/uspace_rtapi_main.cc \ rtapi/uspace_rtapi_app.cc \ rtapi/uspace_rtapi_parport.cc \ rtapi/uspace_rtapi_string.c \ @@ -43,10 +44,19 @@ $(call TOOBJSDEPS, $(RTAPI_APP_SRCS)): EXTRAFLAGS += -DSIM \ -UULAPI -DRTAPI -pthread ../bin/rtapi_app: $(call TOOBJS, $(RTAPI_APP_SRCS)) $(ECHO) Linking $(notdir $@) - $(Q)$(CXX) -rdynamic -o $@ $^ $(LIBDL) -pthread -lrt $(LIBUDEV_LIBS) -ldl $(LDFLAGS) + $(Q)$(CXX) -rdynamic -o $@ $^ $(LIBDL) -pthread -lrt -lfmt $(LIBUDEV_LIBS) -ldl $(LDFLAGS) TARGETS += ../bin/rtapi_app endif +USPACE_POSIX_SRCS := rtapi/uspace_posix.cc +USERSRCS += $(USPACE_POSIX_SRCS) +$(call TOOBJSDEPS, $(USPACE_POSIX_SRCS)): EXTRAFLAGS += -pthread -fPIC +../lib/libuspace-posix.so.0: $(call TOOBJS, $(USPACE_POSIX_SRCS)) + $(ECHO) Linking $(notdir $@) + $(Q)$(CXX) -shared $(LDFLAGS) -o $@ $^ -Wl,-soname,$(notdir $@) +TARGETS += ../lib/libuspace-posix.so.0 +TARGETS += ../lib/libuspace-posix.so + ifeq ($(CONFIG_USPACE_RTAI),y) USPACE_RTAI_SRCS := rtapi/uspace_rtai.cc USERSRCS += $(USPACE_RTAI_SRCS) diff --git a/src/rtapi/rtapi_pci.cc b/src/rtapi/rtapi_pci.cc index f3e71844515..cfbf05ca885 100644 --- a/src/rtapi/rtapi_pci.cc +++ b/src/rtapi/rtapi_pci.cc @@ -34,7 +34,7 @@ #include #include #include -#include "rtapi_uspace.hh" +#include "uspace_rtapi_app.hh" #include #include diff --git a/src/rtapi/uspace_common.h b/src/rtapi/uspace_common.h index 9c82c3a397e..ffc7376b9a9 100644 --- a/src/rtapi/uspace_common.h +++ b/src/rtapi/uspace_common.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -39,7 +40,7 @@ static msg_level_t msg_level = RTAPI_MSG_ERR; /* message printing level */ #include "config.h" #ifdef RTAPI -#include "rtapi_uspace.hh" +#include "uspace_rtapi_app.hh" #endif typedef struct { diff --git a/src/rtapi/uspace_posix.cc b/src/rtapi/uspace_posix.cc new file mode 100644 index 00000000000..3d4d41f4288 --- /dev/null +++ b/src/rtapi/uspace_posix.cc @@ -0,0 +1,268 @@ +/* Copyright (C) 2006-2014 Jeff Epler + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +#include "config.h" +#include "rtapi.h" +#include "uspace_rtapi_app.hh" +#include +#include +#include +#include +#ifdef HAVE_SYS_IO_H +#include +#endif + +namespace { +struct PosixTask : rtapi_task { + PosixTask() : rtapi_task{}, thr{} { + } + + pthread_t thr; /* thread's context */ +}; + +struct PosixApp : RtapiApp { + PosixApp(int policy = SCHED_FIFO) + : RtapiApp(policy), do_thread_lock(policy != SCHED_FIFO) { + pthread_once(&key_once, init_key); + if (do_thread_lock) { + pthread_once(&lock_once, init_lock); + } + } + + struct rtapi_task *do_task_new() { + return new PosixTask; + } + + int task_delete(int id) { + auto task = ::rtapi_get_task(id); + if (!task) + return -EINVAL; + + pthread_cancel(task->thr); + pthread_join(task->thr, 0); + task->magic = 0; + task_array[id] = 0; + delete task; + return 0; + } + + int task_start(int task_id, unsigned long period_nsec) { + auto task = ::rtapi_get_task(task_id); + if (!task) + return -EINVAL; + + task->period = period_nsec; + struct sched_param param; + memset(¶m, 0, sizeof(param)); + param.sched_priority = task->prio; + + // limit PLL correction values to +/-1% of cycle time + task->pll_correction_limit = period_nsec / 100; + task->pll_correction = 0; + + int nprocs = sysconf(_SC_NPROCESSORS_ONLN); + + pthread_attr_t attr; + int ret; + if ((ret = pthread_attr_init(&attr)) != 0) + return -ret; + if ((ret = pthread_attr_setstacksize(&attr, task->stacksize)) != 0) + return -ret; + if ((ret = pthread_attr_setschedpolicy(&attr, policy)) != 0) + return -ret; + if ((ret = pthread_attr_setschedparam(&attr, ¶m)) != 0) + return -ret; + if ((ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED)) != 0) + return -ret; + if (nprocs > 1) { + const static int rt_cpu_number = find_rt_cpu_number(); + rtapi_print_msg( + RTAPI_MSG_INFO, "rt_cpu_number = %i\n", rt_cpu_number + ); + if (rt_cpu_number != -1) { +#ifdef __FreeBSD__ + cpuset_t cpuset; +#else + cpu_set_t cpuset; +#endif + CPU_ZERO(&cpuset); + CPU_SET(rt_cpu_number, &cpuset); + if ((ret = pthread_attr_setaffinity_np(&attr, sizeof(cpuset), &cpuset)) != 0) + return -ret; + } + } + if (do_thread_lock) + pthread_mutex_lock(&thread_lock); + if ((ret = pthread_create(&task->thr, &attr, &wrapper, reinterpret_cast(task))) != 0) + return -ret; + + return 0; + } + + static void *wrapper(void *arg) { + auto task = reinterpret_cast(arg); + + pthread_setspecific(key, arg); + set_namef("rtapi_app:T#%d", task->id); + + struct timespec now; + clock_gettime(CLOCK_MONOTONIC, &now); + rtapi_timespec_advance( + task->nextstart, now, task->period + task->pll_correction + ); + + /* call the task function with the task argument */ + (task->taskcode)(task->arg); + + rtapi_print("ERROR: reached end of wrapper for task %d\n", task->id); + return NULL; + } + + int task_pause(int task_id) { + (void)task_id; + return -ENOSYS; + } + + int task_resume(int task_id) { + (void)task_id; + return -ENOSYS; + } + + long long task_pll_get_reference(void) { + struct rtapi_task *task = + reinterpret_cast(pthread_getspecific(key)); + if (!task) + return 0; + return task->nextstart.tv_sec * 1000000000LL + task->nextstart.tv_nsec; + } + + int task_pll_set_correction(long value) { + struct rtapi_task *task = + reinterpret_cast(pthread_getspecific(key)); + if (!task) + return -EINVAL; + if (value > task->pll_correction_limit) + value = task->pll_correction_limit; + if (value < -(task->pll_correction_limit)) + value = -(task->pll_correction_limit); + task->pll_correction = value; + return 0; + } + + void wait() { + if (do_thread_lock) + pthread_mutex_unlock(&thread_lock); + pthread_testcancel(); + struct rtapi_task *task = + reinterpret_cast(pthread_getspecific(key)); + rtapi_timespec_advance( + task->nextstart, + task->nextstart, + task->period + task->pll_correction + ); + struct timespec now; + clock_gettime(CLOCK_MONOTONIC, &now); + if (rtapi_timespec_less(task->nextstart, now)) { + if (policy == SCHED_FIFO) + unexpected_realtime_delay(task); + } else { + int res = clock_nanosleep( + CLOCK_MONOTONIC, TIMER_ABSTIME, &task->nextstart, nullptr + ); + if (res < 0) + perror("clock_nanosleep"); + } + if (do_thread_lock) + pthread_mutex_lock(&thread_lock); + } + + unsigned char do_inb(unsigned int port) { +#ifdef HAVE_SYS_IO_H + return inb(port); +#else + (void)port; + return 0; +#endif + } + + void do_outb(unsigned char val, unsigned int port) { +#ifdef HAVE_SYS_IO_H + return outb(val, port); +#else + (void)val; + (void)port; +#endif + } + + int run_threads(int fd, int (*callback)(int fd)) { + while (callback(fd)) { + /* nothing */ + } + return 0; + } + + int task_self() { + struct rtapi_task *task = + reinterpret_cast(pthread_getspecific(key)); + if (!task) + return -EINVAL; + return task->id; + } + + bool do_thread_lock; + + static pthread_once_t key_once; + static pthread_key_t key; + static void init_key(void) { + pthread_key_create(&key, NULL); + } + + static pthread_once_t lock_once; + static pthread_mutex_t thread_lock; + static void init_lock(void) { + pthread_mutex_init(&thread_lock, NULL); + } + + long long do_get_time() { + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return ts.tv_sec * 1000000000LL + ts.tv_nsec; + } + + void do_delay(long ns) { + struct timespec ts = {0, ns}; + clock_nanosleep(CLOCK_MONOTONIC, 0, &ts, nullptr); + } +}; + +pthread_once_t PosixApp::key_once = PTHREAD_ONCE_INIT; +pthread_once_t PosixApp::lock_once = PTHREAD_ONCE_INIT; +pthread_key_t PosixApp::key; +pthread_mutex_t PosixApp::thread_lock; + +} // namespace + +extern "C" RtapiApp *make(int policy); + +RtapiApp *make(int policy) { + if (policy == SCHED_OTHER) { + rtapi_print_msg(RTAPI_MSG_ERR, "Note: Using POSIX non-realtime\n"); + } else { + rtapi_print_msg(RTAPI_MSG_ERR, "Note: Using POSIX realtime\n"); + } + return new PosixApp(policy); +} diff --git a/src/rtapi/uspace_rtai.cc b/src/rtapi/uspace_rtai.cc index ff5f9d784ed..209da69cea6 100644 --- a/src/rtapi/uspace_rtai.cc +++ b/src/rtapi/uspace_rtai.cc @@ -1,28 +1,44 @@ +/* Copyright (C) 2016 Jeff Epler + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ #include "config.h" #include "rtapi.h" -#include "rtapi_uspace.hh" +#include "uspace_rtapi_app.hh" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wnarrowing" #include #pragma GCC diagnostic pop +#include #ifdef HAVE_SYS_IO_H #include #endif -namespace -{ +namespace { RtapiApp *app; struct RtaiTask : rtapi_task { - RtaiTask() : rtapi_task{}, cancel{}, rt_task{}, thr{} {} + RtaiTask() : rtapi_task{}, cancel{}, rt_task{}, thr{} { + } std::atomic_int cancel; RT_TASK *rt_task; pthread_t thr; }; -template -T *get_task(int task_id) { - return static_cast(RtapiApp::get_task(task_id)); +template T *get_task(int task_id) { + return static_cast(RtapiApp::get_task(task_id)); } @@ -31,13 +47,14 @@ struct RtaiApp : RtapiApp { pthread_once(&key_once, init_key); } - RtaiTask *do_task_new() { + struct rtapi_task *do_task_new() { return new RtaiTask; } int task_delete(int id) { auto task = ::get_task(id); - if(!task) return -EINVAL; + if (!task) + return -EINVAL; task->cancel = 1; pthread_join(task->thr, nullptr); @@ -49,7 +66,8 @@ struct RtaiApp : RtapiApp { int task_start(int task_id, unsigned long period_nsec) { auto task = ::get_task(task_id); - if(!task) return -EINVAL; + if (!task) + return -EINVAL; task->period = period_nsec; struct sched_param param; @@ -62,40 +80,47 @@ struct RtaiApp : RtapiApp { int ret; pthread_attr_t attr; - if((ret = pthread_attr_init(&attr)) != 0) + if ((ret = pthread_attr_init(&attr)) != 0) return -ret; - if((ret = pthread_attr_setstacksize(&attr, task->stacksize)) != 0) + if ((ret = pthread_attr_setstacksize(&attr, task->stacksize)) != 0) return -ret; - if((ret = pthread_attr_setschedpolicy(&attr, policy)) != 0) + if ((ret = pthread_attr_setschedpolicy(&attr, policy)) != 0) return -ret; - if((ret = pthread_attr_setschedparam(&attr, ¶m)) != 0) + if ((ret = pthread_attr_setschedparam(&attr, ¶m)) != 0) return -ret; - if((ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED)) != 0) + if ((ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED)) != 0) return -ret; - if((ret = pthread_create(&task->thr, &attr, &wrapper, reinterpret_cast(task))) != 0) + if ((ret = pthread_create(&task->thr, &attr, &wrapper, reinterpret_cast(task))) != 0) return -ret; return 0; } static void *wrapper(void *arg) { - auto task = reinterpret_cast(arg); + auto task = reinterpret_cast(arg); pthread_setspecific(key, arg); - - int nprocs = sysconf( _SC_NPROCESSORS_ONLN ); - int cpus_allowed = 1 << (nprocs-1); //Use last CPU as default + + int nprocs = sysconf(_SC_NPROCESSORS_ONLN); + int cpus_allowed = 1 << (nprocs - 1); //Use last CPU as default const static int rt_cpu_number = find_rt_cpu_number(); - if(rt_cpu_number != -1) { - rtapi_print_msg(RTAPI_MSG_INFO, "rt_cpu_number = %i\n", rt_cpu_number); + if (rt_cpu_number != -1) { + rtapi_print_msg( + RTAPI_MSG_INFO, "rt_cpu_number = %i\n", rt_cpu_number + ); cpus_allowed = 1 << rt_cpu_number; } - task->rt_task = rt_task_init_schmod(task->id, task->prio, 0, 0, SCHED_FIFO, cpus_allowed); + task->rt_task = rt_task_init_schmod( + task->id, task->prio, 0, 0, SCHED_FIFO, cpus_allowed + ); rt_set_periodic_mode(); start_rt_timer(nano2count(task->period)); - if(task->uses_fp) rt_task_use_fpu(task->rt_task, 1); + if (task->uses_fp) + rt_task_use_fpu(task->rt_task, 1); rt_make_hard_real_time(); - rt_task_make_periodic_relative_ns(task->rt_task, task->period, task->period); - (task->taskcode) (task->arg); + rt_task_make_periodic_relative_ns( + task->rt_task, task->period, task->period + ); + (task->taskcode)(task->arg); rtapi_print("ERROR: reached end of wrapper for task %d\n", task->id); rt_make_soft_real_time(); @@ -104,13 +129,15 @@ struct RtaiApp : RtapiApp { int task_pause(int task_id) { auto task = ::get_task(task_id); - if(!task) return -EINVAL; + if (!task) + return -EINVAL; return rt_task_suspend(task->rt_task); } int task_resume(int task_id) { auto task = ::get_task(task_id); - if(!task) return -EINVAL; + if (!task) + return -EINVAL; return rt_task_resume(task->rt_task); } @@ -128,11 +155,12 @@ struct RtaiApp : RtapiApp { void wait() { int task_id = task_self(); auto task = ::get_task(task_id); - if(task->cancel) { + if (task->cancel) { rt_make_soft_real_time(); pthread_exit(nullptr); } - if(rt_task_wait_period() < 0) unexpected_realtime_delay(task); + if (rt_task_wait_period() < 0) + unexpected_realtime_delay(task); } unsigned char do_inb(unsigned int port) { @@ -148,19 +176,23 @@ struct RtaiApp : RtapiApp { #ifdef HAVE_SYS_IO_H return outb(val, port); #else + (void)val; (void)port; - return 0; #endif } int run_threads(int fd, int (*callback)(int fd)) { - while(callback(fd)) { /* nothing */ } + while (callback(fd)) { + /* nothing */ + } return 0; } int task_self() { - struct rtapi_task *task = reinterpret_cast(pthread_getspecific(key)); - if(!task) return -EINVAL; + struct rtapi_task *task = + reinterpret_cast(pthread_getspecific(key)); + if (!task) + return -EINVAL; return task->id; } @@ -189,11 +221,14 @@ struct RtaiApp : RtapiApp { pthread_once_t RtaiApp::key_once; pthread_key_t RtaiApp::key; -} +} // namespace -extern "C" RtapiApp *make(); +extern "C" RtapiApp *make(int policy); -RtapiApp *make() { +RtapiApp *make(int policy) { + if (policy != SCHED_FIFO) { + throw std::invalid_argument("Only SCHED_FIFO allowed"); + } rtapi_print_msg(RTAPI_MSG_ERR, "Note: Using LXRT realtime\n"); - return app = new RtaiApp; + return app = new RtaiApp(); } diff --git a/src/rtapi/uspace_rtapi_app.cc b/src/rtapi/uspace_rtapi_app.cc index 30a159dc983..2bdebc162ee 100644 --- a/src/rtapi/uspace_rtapi_app.cc +++ b/src/rtapi/uspace_rtapi_app.cc @@ -15,56 +15,18 @@ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ -#include "config.h" -#include +#include "rtapi.h" +#include "uspace_rtapi_app.hh" -#ifdef __linux__ -#include -#endif -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include -#ifdef HAVE_SYS_IO_H -#include -#endif -#include -#include -#ifdef __linux__ -#include -#include -#endif -#ifdef __FreeBSD__ -#include -#endif - -#include - -#include "rtapi.h" -#include -#include "hal/hal_priv.h" -#include "rtapi_uspace.hh" +#include +#include std::atomic_int WithRoot::level; -static uid_t euid, ruid; - -#include "rtapi/uspace_common.h" +uid_t euid, ruid; WithRoot::WithRoot() { - if(!level++) { + if (!level++) { #ifdef __linux__ setfsuid(euid); #endif @@ -72,842 +34,39 @@ WithRoot::WithRoot() { } WithRoot::~WithRoot() { - if(!--level) { + if (!--level) { #ifdef __linux__ setfsuid(ruid); #endif } } -namespace -{ -RtapiApp &App(); - -struct message_t { - msg_level_t level; - char msg[1024-sizeof(level)]; -}; - -boost::lockfree::queue> -rtapi_msg_queue; - -static void set_namef(const char *fmt, ...) { - char *buf = NULL; - va_list ap; - - va_start(ap, fmt); - if (vasprintf(&buf, fmt, ap) < 0) { - va_end(ap); - return; - } - va_end(ap); - - int res = pthread_setname_np(pthread_self(), buf); - if (res) { - fprintf(stderr, "pthread_setname_np() failed for %s: %d\n", buf, res); - } - free(buf); -} - -pthread_t queue_thread; -void *queue_function(void * /*arg*/) { - set_namef("rtapi_app:mesg"); - // note: can't use anything in this function that requires App() to exist - // but it's OK to use functions that aren't safe for realtime (that's the - // point of running this in a thread) - while(1) { - pthread_testcancel(); - pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, nullptr); - rtapi_msg_queue.consume_all([](const message_t &m) { - fputs(m.msg, m.level == RTAPI_MSG_ALL ? stdout : stderr); - }); - pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, nullptr); - struct timespec ts = {0, 10000000}; - rtapi_clock_nanosleep(CLOCK_MONOTONIC, 0, &ts, NULL, NULL); - } - return nullptr; -} -} - -static int sim_rtapi_run_threads(int fd, int (*callback)(int fd)); - -using namespace std; - -template T DLSYM(void *handle, const string &name) { - return (T)(dlsym(handle, name.c_str())); -} - -template T DLSYM(void *handle, const char *name) { - return (T)(dlsym(handle, name)); -} - -static std::map modules; - -static int instance_count = 0; -static int force_exit = 0; - -static int do_newinst_cmd(const string& type, const string& name, const string& arg) { - void *module = modules["hal_lib"]; - if(!module) { - rtapi_print_msg(RTAPI_MSG_ERR, - "newinst: hal_lib is required, but not loaded\n"); - return -1; - } - - hal_comp_t *(*find_comp_by_name)(char*) = - DLSYM(module, "halpr_find_comp_by_name"); - if(!find_comp_by_name) { - rtapi_print_msg(RTAPI_MSG_ERR, - "newinst: halpr_find_comp_by_name not found\n"); - return -1; - } - - hal_comp_t *comp = find_comp_by_name((char*)type.c_str()); - if(!comp) { - rtapi_print_msg(RTAPI_MSG_ERR, - "newinst: component %s not found\n", type.c_str()); - return -1; - } - - return comp->make((char*)name.c_str(), (char*)arg.c_str()); -} - -static int do_one_item(char item_type_char, const string ¶m_name, const string ¶m_value, void *vitem, int idx=0) { - char *endp; - switch(item_type_char) { - case 'l': { - long *litem = *(long**) vitem; - litem[idx] = strtol(param_value.c_str(), &endp, 0); - if(*endp) { - rtapi_print_msg(RTAPI_MSG_ERR, - "`%s' invalid for parameter `%s'", - param_value.c_str(), param_name.c_str()); - return -1; - } - return 0; - } - case 'i': { - int *iitem = *(int**) vitem; - iitem[idx] = strtol(param_value.c_str(), &endp, 0); - if(*endp) { - rtapi_print_msg(RTAPI_MSG_ERR, - "`%s' invalid for parameter `%s'", - param_value.c_str(), param_name.c_str()); - return -1; - } - return 0; - } - case 's': { - char **sitem = *(char***) vitem; - sitem[idx] = strdup(param_value.c_str()); - return 0; - } - default: - rtapi_print_msg(RTAPI_MSG_ERR, - "%s: Invalid type character `%c'\n", - param_name.c_str(), item_type_char); - return -1; - } - return 0; -} - -void remove_quotes(string &s) { - s.erase(remove_copy(s.begin(), s.end(), s.begin(), '"'), s.end()); -} - -static int do_comp_args(void *module, vector args) { - for(unsigned i=1; i < args.size(); i++) { - string &s = args[i]; - remove_quotes(s); - size_t idx = s.find('='); - if(idx == string::npos) { - rtapi_print_msg(RTAPI_MSG_ERR, "Invalid parameter `%s'\n", - s.c_str()); - return -1; - } - string param_name(s, 0, idx); - string param_value(s, idx+1); - void *item=DLSYM(module, "rtapi_info_address_" + param_name); - if(!item) { - rtapi_print_msg(RTAPI_MSG_ERR, - "Unknown parameter `%s'\n", s.c_str()); - return -1; - } - char **item_type=DLSYM(module, "rtapi_info_type_" + param_name); - if(!item_type || !*item_type) { - rtapi_print_msg(RTAPI_MSG_ERR, - "Unknown parameter `%s' (type information missing)\n", - s.c_str()); - return -1; - } - - int*max_size_ptr=DLSYM(module, "rtapi_info_size_" + param_name); - - char item_type_char = **item_type; - if(max_size_ptr) { - int max_size = *max_size_ptr; - size_t idx = 0; - int i = 0; - while(idx != string::npos) { - if(i == max_size) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s: can only take %d arguments\n", - s.c_str(), max_size); - return -1; - } - size_t idx1 = param_value.find(",", idx); - string substr(param_value, idx, idx1 - idx); - int result = do_one_item(item_type_char, s, substr, item, i); - if(result != 0) return result; - i++; - idx = idx1 == string::npos ? idx1 : idx1 + 1; - } - } else { - int result = do_one_item(item_type_char, s, param_value, item); - if(result != 0) return result; - } - } - return 0; -} - -static int do_load_cmd(const string& name, const vector& args) { - void *w = modules[name]; - if(w == NULL) { - char what[LINELEN+1]; - snprintf(what, LINELEN, "%s/%s.so", EMC2_RTLIB_DIR, name.c_str()); - void *module = modules[name] = dlopen(what, RTLD_GLOBAL | RTLD_NOW); - if(!module) { - rtapi_print_msg(RTAPI_MSG_ERR, "%s: dlopen: %s\n", name.c_str(), dlerror()); - modules.erase(name); - return -1; - } - /// XXX handle arguments - int (*start)(void) = DLSYM(module, "rtapi_app_main"); - if(!start) { - rtapi_print_msg(RTAPI_MSG_ERR, "%s: dlsym: %s\n", name.c_str(), dlerror()); - dlclose(module); - modules.erase(name); - return -1; - } - int result; - - result = do_comp_args(module, args); - if(result < 0) { - dlclose(module); - modules.erase(name); - return -1; - } - - if ((result=start()) < 0) { - rtapi_print_msg(RTAPI_MSG_ERR, "%s: rtapi_app_main: %s (%d)\n", - name.c_str(), strerror(-result), result); - dlclose(module); - modules.erase(name); - return result; - } else { - instance_count ++; - return 0; - } - } else { - rtapi_print_msg(RTAPI_MSG_ERR, "%s: already exists\n", name.c_str()); - return -1; - } -} - -static int do_unload_cmd(const string& name) { - void *w = modules[name]; - if(w == NULL) { - rtapi_print_msg(RTAPI_MSG_ERR, "%s: not loaded\n", name.c_str()); - return -1; - } else { - int (*stop)(void) = DLSYM(w, "rtapi_app_exit"); - if(stop) stop(); - modules.erase(modules.find(name)); - dlclose(w); - instance_count --; - } - return 0; -} - -static int do_debug_cmd(const string& value) { - try{ - int new_level = stoi(value); - if (new_level < 0 || new_level > 5){ - rtapi_print_msg(RTAPI_MSG_ERR, "Debug level must be >=0 and <= 5\n"); - return -EINVAL; - } - return rtapi_set_msg_level(new_level); - }catch(invalid_argument &e){ - //stoi will throw an exception if parsing is not possible - rtapi_print_msg(RTAPI_MSG_ERR, "Debug level is not a number\n"); - return -EINVAL; - } -} - -struct ReadError : std::exception {}; -struct WriteError : std::exception {}; - -static int read_number(int fd) { - int r = 0, neg=1; - char ch; - - while(1) { - int res = read(fd, &ch, 1); - if(res != 1) return -1; - if(ch == '-') neg = -1; - else if(ch == ' ') return r * neg; - else r = 10 * r + ch - '0'; - } -} - -static string read_string(int fd) { - int len = read_number(fd); - if(len < 0) - throw ReadError(); - if(!len) - return string(); - string str(len, 0); - if(read(fd, str.data(), len) != len) - throw ReadError(); - return str; -} - -static vector read_strings(int fd) { - vector result; - int count = read_number(fd); - if(count < 0) - return result; - for(int i=0; i& strings) { - string buf; - write_number(buf, strings.size()); - for(unsigned int i=0; i args) { - if(args.size() == 0) { return 0; } - if(args.size() == 1 && args[0] == "exit") { - force_exit = 1; - return 0; - } else if(args.size() >= 2 && args[0] == "load") { - string name = args[1]; - args.erase(args.begin()); - return do_load_cmd(name, args); - } else if(args.size() == 2 && args[0] == "unload") { - return do_unload_cmd(args[1]); - } else if(args.size() == 3 && args[0] == "newinst") { - return do_newinst_cmd(args[1], args[2], ""); - } else if(args.size() == 4 && args[0] == "newinst") { - return do_newinst_cmd(args[1], args[2], args[3]); - } else if(args.size() == 2 && args[0] == "debug") { - return do_debug_cmd(args[1]); - } else { - rtapi_print_msg(RTAPI_MSG_ERR, - "Unrecognized command starting with %s\n", - args[0].c_str()); - return -1; - } -} - -static int slave(int fd, const vector& args) { - try { - write_strings(fd, args); - } - catch (WriteError &e) { - rtapi_print_msg(RTAPI_MSG_ERR, - "rtapi_app: failed to write to master: %s\n", strerror(errno)); - } - - int result = read_number(fd); - return result; -} - -static int callback(int fd) -{ - struct sockaddr_un client_addr; - memset(&client_addr, 0, sizeof(client_addr)); - socklen_t len = sizeof(client_addr); - int fd1 = accept(fd, (sockaddr*)&client_addr, &len); - if(fd1 < 0) { - rtapi_print_msg(RTAPI_MSG_ERR, - "rtapi_app: failed to accept connection from slave: %s\n", strerror(errno)); - return -1; - } else { - int result; - try { - result = handle_command(read_strings(fd1)); - } catch (ReadError &e) { - rtapi_print_msg(RTAPI_MSG_ERR, - "rtapi_app: failed to read from slave: %s\n", strerror(errno)); - close(fd1); - return -1; - } - string buf; - write_number(buf, result); - if(write(fd1, buf.data(), buf.size()) != (ssize_t)buf.size()) { - rtapi_print_msg(RTAPI_MSG_ERR, - "rtapi_app: failed to write to slave: %s\n", strerror(errno)); - }; - close(fd1); - } - return !force_exit && instance_count > 0; -} - -static pthread_t main_thread{}; - -static int master(int fd, const vector& args) { - main_thread = pthread_self(); - int result; - if((result = pthread_create(&queue_thread, nullptr, &queue_function, nullptr)) != 0) { - errno = result; - perror("pthread_create (queue function)"); - return -1; - } - do_load_cmd("hal_lib", vector()); - instance_count = 0; - App(); // force rtapi_app to be created - if(args.size()) { - result = handle_command(args); - if(result != 0) goto out; - if(force_exit || instance_count == 0) goto out; - } - sim_rtapi_run_threads(fd, callback); -out: - pthread_cancel(queue_thread); - pthread_join(queue_thread, nullptr); - rtapi_msg_queue.consume_all([](const message_t &m) { - fputs(m.msg, m.level == RTAPI_MSG_ALL ? stdout : stderr); - }); - return result; -} - -static std::string -_get_fifo_path() { - std::string s; - if(getenv("RTAPI_FIFO_PATH")) - s = getenv("RTAPI_FIFO_PATH"); - else if(getenv("HOME")) - s = std::string(getenv("HOME")) + "/.rtapi_fifo"; - else { - rtapi_print_msg(RTAPI_MSG_ERR, - "rtapi_app: RTAPI_FIFO_PATH and HOME are unset. rtapi fifo creation is unsafe."); - return NULL; - } - if(s.size() + 1 > sizeof(sockaddr_un::sun_path)) { - rtapi_print_msg(RTAPI_MSG_ERR, - "rtapi_app: rtapi fifo path is too long (arch limit %zd): %s", - sizeof(sockaddr_un::sun_path), s.c_str()); - return NULL; - } - return s; -} - -static const char * -get_fifo_path() { - static std::string path = _get_fifo_path(); - return path.c_str(); -} - -static int -get_fifo_path(char *buf, size_t bufsize) { - int len; - const char *s = get_fifo_path(); - if(!s) return -1; - len=snprintf(buf+1, bufsize-1, "%s", s); - return len; -} - -int main(int argc, char **argv) { - if(getuid() == 0) { - char *fallback_uid_str = getenv("RTAPI_UID"); - int fallback_uid = fallback_uid_str ? atoi(fallback_uid_str) : 0; - if(fallback_uid == 0) - { - // Cppcheck cannot see EMC2_BIN_DIR when RTAPI is defined, but that - // doesn't happen in uspace. - fprintf(stderr, - "Refusing to run as root without fallback UID specified\n" - "To run under a debugger with I/O, use e.g.,\n" - // cppcheck-suppress unknownMacro - " sudo env RTAPI_UID=`id -u` RTAPI_FIFO_PATH=$HOME/.rtapi_fifo gdb " EMC2_BIN_DIR "/rtapi_app\n"); - exit(1); - } - if (setreuid(fallback_uid, 0) != 0) { perror("setreuid"); abort(); } - fprintf(stderr, - "Running with fallback_uid. getuid()=%d geteuid()=%d\n", - getuid(), geteuid()); - } - ruid = getuid(); - euid = geteuid(); - if (setresuid(euid, euid, ruid) != 0) { perror("setresuid"); abort(); } -#ifdef __linux__ - setfsuid(ruid); -#endif - vector args; - for(int i=1; i(malloc(PRE_ALLOC_SIZE)); - if (buf == NULL) { - rtapi_print_msg(RTAPI_MSG_WARN, "malloc(PRE_ALLOC_SIZE) failed\n"); - return; - } - long pagesize = sysconf(_SC_PAGESIZE); - /* Touch each page in this piece of memory to get it mapped into RAM */ - for (size_t i = 0; i < PRE_ALLOC_SIZE; i += pagesize) { - /* Each write to this buffer will generate a pagefault. - * Once the pagefault is handled a page will be locked in - * memory and never given back to the system. */ - buf[i] = 0; - } - free((void *)buf); -} - -static int harden_rt() -{ - if(!rtapi_is_realtime()) return -EINVAL; - - WITH_ROOT; -#if defined(__linux__) && (defined(__x86_64__) || defined(__i386__)) - if (iopl(3) < 0) { - rtapi_print_msg(RTAPI_MSG_ERR, - "iopl() failed: %s\n" - "cannot gain I/O privileges - " - "forgot 'sudo make setuid' or using secure boot? -" - "parallel port access is not allowed\n", - strerror(errno)); - } -#endif - - struct sigaction sig_act = {}; -#ifdef __linux__ - // enable realtime - if (setrlimit(RLIMIT_RTPRIO, &unlimited) < 0) - { - rtapi_print_msg(RTAPI_MSG_WARN, - "setrlimit(RTLIMIT_RTPRIO): %s\n", - strerror(errno)); - return -errno; - } - - // enable core dumps - if (setrlimit(RLIMIT_CORE, &unlimited) < 0) - rtapi_print_msg(RTAPI_MSG_WARN, - "setrlimit: %s - core dumps may be truncated or non-existent\n", - strerror(errno)); - - // even when setuid root - if (prctl(PR_SET_DUMPABLE, 1) < 0) - rtapi_print_msg(RTAPI_MSG_WARN, - "prctl(PR_SET_DUMPABLE) failed: no core dumps will be created - %d - %s\n", - errno, strerror(errno)); -#endif /* __linux__ */ - - configure_memory(); - - sigemptyset( &sig_act.sa_mask ); - sig_act.sa_handler = SIG_IGN; - sig_act.sa_sigaction = NULL; - - // prevent stopping of RT threads by ^Z - sigaction(SIGTSTP, &sig_act, (struct sigaction *) NULL); - - sig_act.sa_sigaction = signal_handler; - sig_act.sa_flags = SA_SIGINFO; - - sigaction(SIGSEGV, &sig_act, (struct sigaction *) NULL); - sigaction(SIGILL, &sig_act, (struct sigaction *) NULL); - sigaction(SIGFPE, &sig_act, (struct sigaction *) NULL); - sigaction(SIGTERM, &sig_act, (struct sigaction *) NULL); - sigaction(SIGINT, &sig_act, (struct sigaction *) NULL); - -#ifdef __linux__ - int fd = open("/dev/cpu_dma_latency", O_WRONLY | O_CLOEXEC); - if (fd < 0) { - rtapi_print_msg(RTAPI_MSG_WARN, "failed to open /dev/cpu_dma_latency: %s\n", strerror(errno)); - } else { - int r; - r = write(fd, "\0\0\0\0", 4); - if (r != 4) { - rtapi_print_msg(RTAPI_MSG_WARN, "failed to write to /dev/cpu_dma_latency: %s\n", strerror(errno)); - } - // deliberately leak fd until program exit - } -#endif /* __linux__ */ - return 0; -} - - -static RtapiApp *makeApp() -{ - if(euid != 0 || harden_rt() < 0) - { - rtapi_print_msg(RTAPI_MSG_ERR, "Note: Using POSIX non-realtime\n"); - return new Posix(SCHED_OTHER); - } - WithRoot r; - void *dll = nullptr; - if(detect_xenomai_evl()) { - dll = dlopen(EMC2_HOME "/lib/libuspace-xenomai-evl.so.0", RTLD_NOW); - if(!dll) fprintf(stderr, "dlopen: %s\n", dlerror()); - }else if(detect_xenomai()) { - dll = dlopen(EMC2_HOME "/lib/libuspace-xenomai.so.0", RTLD_NOW); - if(!dll) fprintf(stderr, "dlopen: %s\n", dlerror()); - } else if(detect_rtai()) { - dll = dlopen(EMC2_HOME "/lib/libuspace-rtai.so.0", RTLD_NOW); - if(!dll) fprintf(stderr, "dlopen: %s\n", dlerror()); - } - if(dll) - { - auto fn = reinterpret_cast(dlsym(dll, "make")); - if(!fn) fprintf(stderr, "dlopen: %s\n", dlerror()); - auto result = fn ? fn() : nullptr; - if(result) { - return result; - } - } - rtapi_print_msg(RTAPI_MSG_ERR, "Note: Using POSIX realtime\n"); - return new Posix(SCHED_FIFO); -} -RtapiApp &App() -{ - static RtapiApp *app = makeApp(); - return *app; -} - -} -/* data for all tasks */ -struct rtapi_task *task_array[MAX_TASKS]; - /* Priority functions. Uspace uses POSIX task priorities. */ -int RtapiApp::prio_highest() const -{ +int RtapiApp::prio_highest() const { return sched_get_priority_max(policy); } -int RtapiApp::prio_lowest() const -{ - return sched_get_priority_min(policy); +int RtapiApp::prio_lowest() const { + return sched_get_priority_min(policy); } int RtapiApp::prio_higher_delta() const { - if(rtapi_prio_highest() > rtapi_prio_lowest()) { + if (rtapi_prio_highest() > rtapi_prio_lowest()) { return 1; } return -1; } int RtapiApp::prio_bound(int prio) const { - if(rtapi_prio_highest() > rtapi_prio_lowest()) { + if (rtapi_prio_highest() > rtapi_prio_lowest()) { if (prio >= rtapi_prio_highest()) return rtapi_prio_highest(); if (prio < rtapi_prio_lowest()) @@ -922,76 +81,82 @@ int RtapiApp::prio_bound(int prio) const { } bool RtapiApp::prio_check(int prio) const { - if(rtapi_prio_highest() > rtapi_prio_lowest()) { + if (rtapi_prio_highest() > rtapi_prio_lowest()) { return (prio <= rtapi_prio_highest()) && (prio >= rtapi_prio_lowest()); } else { return (prio <= rtapi_prio_lowest()) && (prio >= rtapi_prio_highest()); } } -int RtapiApp::prio_next_higher(int prio) const -{ +int RtapiApp::prio_next_higher(int prio) const { prio = prio_bound(prio); - if(prio != rtapi_prio_highest()) + if (prio != rtapi_prio_highest()) return prio + prio_higher_delta(); return prio; } -int RtapiApp::prio_next_lower(int prio) const -{ +int RtapiApp::prio_next_lower(int prio) const { prio = prio_bound(prio); - if(prio != rtapi_prio_lowest()) + if (prio != rtapi_prio_lowest()) return prio - prio_higher_delta(); return prio; } -int RtapiApp::allocate_task_id() -{ - for(int n=0; nid = n; - task->owner = owner; - task->uses_fp = uses_fp; - task->arg = arg; - task->stacksize = stacksize; - task->taskcode = taskcode; - task->prio = prio; - task->magic = TASK_MAGIC; - task_array[n] = task; + struct rtapi_task *task = do_task_new(); + if (stacksize < (1024 * 1024)) + stacksize = (1024 * 1024); + task->id = n; + task->owner = owner; + task->uses_fp = uses_fp; + task->arg = arg; + task->stacksize = stacksize; + task->taskcode = taskcode; + task->prio = prio; + task->magic = TASK_MAGIC; + task_array[n] = task; - /* and return handle to the caller */ + /* and return handle to the caller */ - return n; + return n; } rtapi_task *RtapiApp::get_task(int task_id) { - if(task_id < 0 || task_id >= MAX_TASKS) return NULL; + if (task_id < 0 || task_id >= MAX_TASKS) + return NULL; /* validate task handle */ rtapi_task *task = task_array[task_id]; - if(!task || task == TASK_MAGIC_INIT || task->magic != TASK_MAGIC) + if (!task || task == TASK_MAGIC_INIT || task->magic != TASK_MAGIC) return NULL; return task; @@ -999,34 +164,33 @@ rtapi_task *RtapiApp::get_task(int task_id) { void RtapiApp::unexpected_realtime_delay(rtapi_task *task, int /*nperiod*/) { static int printed = 0; - if(!printed) - { - rtapi_print_msg(RTAPI_MSG_ERR, - "Unexpected realtime delay on task %d with period %ld\n" - "This Message will only display once per session.\n" - "Run the Latency Test and resolve before continuing.\n", - task->id, task->period); + if (!printed) { + rtapi_print_msg( + RTAPI_MSG_ERR, + "Unexpected realtime delay on task %d with period %ld\n" + "This Message will only display once per session.\n" + "Run the Latency Test and resolve before continuing.\n", + task->id, + task->period + ); printed = 1; } } -int Posix::task_delete(int id) -{ - auto task = ::rtapi_get_task(id); - if(!task) return -EINVAL; - - pthread_cancel(task->thr); - pthread_join(task->thr, 0); - task->magic = 0; - task_array[id] = 0; - delete task; - return 0; +long RtapiApp::clock_set_period(long nsecs) { + if (nsecs == 0) + return period; + if (period != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "attempt to set period twice\n"); + return -EINVAL; + } + period = nsecs; + return period; } //parse_cpu_list from https://gitlab.com/Xenomai/xenomai4/libevl/-/blob/11e6a1fb183a315ae861762e7650fd5e10d83ff5/tests/helpers.c //License: MIT -static void parse_cpu_list(const char *path, cpu_set_t *cpuset) -{ +static void parse_cpu_list(const char *path, cpu_set_t *cpuset) { char *p, *range, *range_p = NULL, *id, *id_r; int start, end, cpu; char buf[BUFSIZ]; @@ -1065,29 +229,35 @@ static void parse_cpu_list(const char *path, cpu_set_t *cpuset) } int find_rt_cpu_number() { - if(getenv("RTAPI_CPU_NUMBER")) return atoi(getenv("RTAPI_CPU_NUMBER")); + if (getenv("RTAPI_CPU_NUMBER")) + return atoi(getenv("RTAPI_CPU_NUMBER")); #ifdef __linux__ - const char* isolated_file="/sys/devices/system/cpu/isolated"; + const char *isolated_file = "/sys/devices/system/cpu/isolated"; cpu_set_t cpuset; parse_cpu_list(isolated_file, &cpuset); //Print list rtapi_print_msg(RTAPI_MSG_INFO, "cpuset isolated "); - for(int i=0; i(task_id); - if(!task) return -EINVAL; - - if(period_nsec < (unsigned long)period) period_nsec = (unsigned long)period; - task->period = period_nsec; - task->ratio = period_nsec / period; - - struct sched_param param; - memset(¶m, 0, sizeof(param)); - param.sched_priority = task->prio; - - // limit PLL correction values to +/-1% of cycle time - task->pll_correction_limit = period_nsec / 100; - task->pll_correction = 0; - - int nprocs = sysconf( _SC_NPROCESSORS_ONLN ); - - pthread_attr_t attr; - int ret; - if((ret = pthread_attr_init(&attr)) != 0) - return -ret; - if((ret = pthread_attr_setstacksize(&attr, task->stacksize)) != 0) - return -ret; - if((ret = pthread_attr_setschedpolicy(&attr, policy)) != 0) - return -ret; - if((ret = pthread_attr_setschedparam(&attr, ¶m)) != 0) - return -ret; - if((ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED)) != 0) - return -ret; - if(nprocs > 1) { - const static int rt_cpu_number = find_rt_cpu_number(); - rtapi_print_msg(RTAPI_MSG_INFO, "rt_cpu_number = %i\n", rt_cpu_number); - if(rt_cpu_number != -1) { -#ifdef __FreeBSD__ - cpuset_t cpuset; -#else - cpu_set_t cpuset; -#endif - CPU_ZERO(&cpuset); - CPU_SET(rt_cpu_number, &cpuset); - if((ret = pthread_attr_setaffinity_np(&attr, sizeof(cpuset), &cpuset)) != 0) - return -ret; - } - } - if((ret = pthread_create(&task->thr, &attr, &wrapper, reinterpret_cast(task))) != 0) - return -ret; - - return 0; -} - -#define RTAPI_CLOCK (CLOCK_MONOTONIC) - -pthread_once_t Posix::key_once = PTHREAD_ONCE_INIT; -pthread_once_t Posix::lock_once = PTHREAD_ONCE_INIT; -pthread_key_t Posix::key; -pthread_mutex_t Posix::thread_lock; - -void *Posix::wrapper(void *arg) -{ - struct rtapi_task *task; - - /* use the argument to point to the task data */ - task = (struct rtapi_task*)arg; - long int period = App().period; - if(task->period < period) task->period = period; - task->ratio = task->period / period; - task->period = task->ratio * period; - rtapi_print_msg(RTAPI_MSG_INFO, "task %p period = %lu ratio=%u\n", - task, task->period, task->ratio); - - pthread_setspecific(key, arg); - set_namef("rtapi_app:T#%d", task->id); - - Posix &papp = reinterpret_cast(App()); - if(papp.do_thread_lock) - pthread_mutex_lock(&papp.thread_lock); - - struct timespec now; - clock_gettime(RTAPI_CLOCK, &now); - rtapi_timespec_advance(task->nextstart, now, task->period + task->pll_correction); - - /* call the task function with the task argument */ - (task->taskcode) (task->arg); - - rtapi_print("ERROR: reached end of wrapper for task %d\n", task->id); - return NULL; -} - -long long Posix::task_pll_get_reference(void) { - struct rtapi_task *task = reinterpret_cast(pthread_getspecific(key)); - if(!task) return 0; - return task->nextstart.tv_sec * 1000000000LL + task->nextstart.tv_nsec; -} - -int Posix::task_pll_set_correction(long value) { - struct rtapi_task *task = reinterpret_cast(pthread_getspecific(key)); - if(!task) return -EINVAL; - if (value > task->pll_correction_limit) value = task->pll_correction_limit; - if (value < -(task->pll_correction_limit)) value = -(task->pll_correction_limit); - task->pll_correction = value; - return 0; -} - -int Posix::task_pause(int) { - return -ENOSYS; -} - -int Posix::task_resume(int) { - return -ENOSYS; -} - -int Posix::task_self() { - struct rtapi_task *task = reinterpret_cast(pthread_getspecific(key)); - if(!task) return -EINVAL; - return task->id; -} - -void Posix::wait() { - if(do_thread_lock) - pthread_mutex_unlock(&thread_lock); - pthread_testcancel(); - struct rtapi_task *task = reinterpret_cast(pthread_getspecific(key)); - rtapi_timespec_advance(task->nextstart, task->nextstart, task->period + task->pll_correction); - struct timespec now; - clock_gettime(RTAPI_CLOCK, &now); - if(rtapi_timespec_less(task->nextstart, now)) - { - if(policy == SCHED_FIFO) - unexpected_realtime_delay(task); - } - else - { - int res = rtapi_clock_nanosleep(RTAPI_CLOCK, TIMER_ABSTIME, &task->nextstart, nullptr, &now); - if(res < 0) perror("clock_nanosleep"); - } - if(do_thread_lock) - pthread_mutex_lock(&thread_lock); -} - -unsigned char Posix::do_inb(unsigned int port) -{ -#ifdef HAVE_SYS_IO_H - return inb(port); -#else - (void)port; - return 0; -#endif -} - -void Posix::do_outb(unsigned char val, unsigned int port) -{ -#ifdef HAVE_SYS_IO_H - return outb(val, port); -#else - (void)val; - (void)port; -#endif -} - -void Posix::do_delay(long ns) { - struct timespec ts = {0, ns}; - rtapi_clock_nanosleep(CLOCK_MONOTONIC, 0, &ts, NULL, NULL); -} -int rtapi_prio_highest(void) -{ - return App().prio_highest(); -} - -int rtapi_prio_lowest(void) -{ - return App().prio_lowest(); -} - -int rtapi_prio_next_higher(int prio) -{ - return App().prio_next_higher(prio); -} - -int rtapi_prio_next_lower(int prio) -{ - return App().prio_next_lower(prio); -} - -long rtapi_clock_set_period(long nsecs) -{ - return App().clock_set_period(nsecs); -} - -long RtapiApp::clock_set_period(long nsecs) -{ - if(nsecs == 0) return period; - if(period != 0) { - rtapi_print_msg(RTAPI_MSG_ERR, "attempt to set period twice\n"); - return -EINVAL; - } - period = nsecs; - return period; -} - - -int rtapi_task_new(void (*taskcode) (void*), void *arg, - int prio, int owner, unsigned long int stacksize, int uses_fp) { - return App().task_new(taskcode, arg, prio, owner, stacksize, uses_fp); -} - -int rtapi_task_delete(int id) { - return App().task_delete(id); -} - -int rtapi_task_start(int task_id, unsigned long period_nsec) -{ - int ret = App().task_start(task_id, period_nsec); - if(ret != 0) { - errno = -ret; - perror("rtapi_task_start()"); - } - return ret; -} - -int rtapi_task_pause(int task_id) -{ - return App().task_pause(task_id); -} - -int rtapi_task_resume(int task_id) -{ - return App().task_resume(task_id); -} - -int rtapi_task_self() -{ - return App().task_self(); -} - -long long rtapi_task_pll_get_reference(void) -{ - return App().task_pll_get_reference(); -} - -int rtapi_task_pll_set_correction(long value) -{ - return App().task_pll_set_correction(value); -} - -void rtapi_wait(void) -{ - App().wait(); -} - -void rtapi_outb(unsigned char byte, unsigned int port) -{ - App().do_outb(byte, port); -} - -unsigned char rtapi_inb(unsigned int port) -{ - return App().do_inb(port); -} - -long int simple_strtol(const char *nptr, char **endptr, int base) { - return strtol(nptr, endptr, base); -} - -int Posix::run_threads(int fd, int(*callback)(int fd)) { - while(callback(fd)) { /* nothing */ } - return 0; -} - -int sim_rtapi_run_threads(int fd, int (*callback)(int fd)) { - return App().run_threads(fd, callback); -} - -long long rtapi_get_time() { - return App().do_get_time(); -} +void set_namef(const char *fmt, ...) { + char *buf = NULL; + va_list ap; -void default_rtapi_msg_handler(msg_level_t level, const char *fmt, va_list ap) { - if(main_thread && pthread_self() != main_thread) { - message_t m; - m.level = level; - vsnprintf(m.msg, sizeof(m.msg), fmt, ap); - rtapi_msg_queue.push(m); - } else { - vfprintf(level == RTAPI_MSG_ALL ? stdout : stderr, fmt, ap); + va_start(ap, fmt); + if (vasprintf(&buf, fmt, ap) < 0) { + va_end(ap); + return; } -} - -long int rtapi_delay_max() { return 10000; } - -void rtapi_delay(long ns) { - if(ns > rtapi_delay_max()) ns = rtapi_delay_max(); - App().do_delay(ns); -} + va_end(ap); -const unsigned long ONE_SEC_IN_NS = 1000000000; -void rtapi_timespec_advance(struct timespec &result, const struct timespec &src, unsigned long nsec) -{ - time_t sec = src.tv_sec; - while(nsec >= ONE_SEC_IN_NS) - { - ++sec; - nsec -= ONE_SEC_IN_NS; - } - nsec += src.tv_nsec; - if(nsec >= ONE_SEC_IN_NS) - { - ++sec; - nsec -= ONE_SEC_IN_NS; + int res = pthread_setname_np(pthread_self(), buf); + if (res) { + fprintf(stderr, "pthread_setname_np() failed for %s: %d\n", buf, res); } - result.tv_sec = sec; - result.tv_nsec = nsec; -} - -int rtapi_open_as_root(const char *filename, int mode) { - WITH_ROOT; - int r = open(filename, mode); - if(r < 0) return -errno; - return r; -} - -int rtapi_spawn_as_root(pid_t *pid, const char *path, - const posix_spawn_file_actions_t *file_actions, - const posix_spawnattr_t *attrp, - char *const argv[], char *const envp[]) -{ - return posix_spawn(pid, path, file_actions, attrp, argv, envp); -} - -int rtapi_spawnp_as_root(pid_t *pid, const char *path, - const posix_spawn_file_actions_t *file_actions, - const posix_spawnattr_t *attrp, - char *const argv[], char *const envp[]) -{ - return posix_spawnp(pid, path, file_actions, attrp, argv, envp); + free(buf); } diff --git a/src/rtapi/rtapi_uspace.hh b/src/rtapi/uspace_rtapi_app.hh similarity index 70% rename from src/rtapi/rtapi_uspace.hh rename to src/rtapi/uspace_rtapi_app.hh index bc589c12536..e1a59b4fd36 100644 --- a/src/rtapi/rtapi_uspace.hh +++ b/src/rtapi/uspace_rtapi_app.hh @@ -33,43 +33,46 @@ static inline void rtapi_timespec_add(timespec &result, const timespec &ta, cons } } -static inline bool rtapi_timespec_less(const struct timespec &ta, const struct timespec &tb) { - if(ta.tv_sec < tb.tv_sec) return 1; - if(ta.tv_sec > tb.tv_sec) return 0; +static inline bool +rtapi_timespec_less(const struct timespec &ta, const struct timespec &tb) { + if (ta.tv_sec < tb.tv_sec) + return 1; + if (ta.tv_sec > tb.tv_sec) + return 0; return ta.tv_nsec < tb.tv_nsec; } -void rtapi_timespec_advance(struct timespec &result, const struct timespec &src, unsigned long nsec); +void rtapi_timespec_advance( + struct timespec &result, const struct timespec &src, unsigned long nsec +); -struct WithRoot -{ +struct WithRoot { WithRoot(); ~WithRoot(); static std::atomic_int level; }; struct rtapi_task { - rtapi_task(); + rtapi_task(); - int magic; /* to check for valid handle */ - int id; - int owner; - int uses_fp; - size_t stacksize; - int prio; - long period; - struct timespec nextstart; - unsigned ratio; - long pll_correction; - long pll_correction_limit; - void *arg; - void (*taskcode) (void*); /* pointer to task function */ + int magic; /* to check for valid handle */ + int id; + int owner; + int uses_fp; + size_t stacksize; + int prio; + long period; + struct timespec nextstart; + long pll_correction; + long pll_correction_limit; + void *arg; + void (*taskcode)(void *); /* pointer to task function */ }; -struct RtapiApp -{ +struct RtapiApp { - RtapiApp(int policy = SCHED_OTHER) : policy(policy), period(0) {} + RtapiApp(int policy = SCHED_OTHER) : policy(policy), period(0) { + } virtual int prio_highest() const; virtual int prio_lowest() const; @@ -79,12 +82,18 @@ struct RtapiApp int prio_next_higher(int prio) const; int prio_next_lower(int prio) const; long clock_set_period(long int period_nsec); - int task_new(void (*taskcode)(void*), void *arg, - int prio, int owner, unsigned long int stacksize, int uses_fp); + int task_new( + void (*taskcode)(void *), + void *arg, + int prio, + int owner, + unsigned long int stacksize, + int uses_fp + ); virtual rtapi_task *do_task_new() = 0; static int allocate_task_id(); static struct rtapi_task *get_task(int task_id); - void unexpected_realtime_delay(rtapi_task *task, int nperiod=1); + void unexpected_realtime_delay(rtapi_task *task, int nperiod = 1); virtual int task_delete(int id) = 0; virtual int task_start(int task_id, unsigned long period_nsec) = 0; virtual int task_pause(int task_id) = 0; @@ -102,18 +111,20 @@ struct RtapiApp long period; }; -template -T *rtapi_get_task(int task_id) { - return static_cast(RtapiApp::get_task(task_id)); +template T *rtapi_get_task(int task_id) { + return static_cast(RtapiApp::get_task(task_id)); } int find_rt_cpu_number(); +void set_namef(const char *fmt, ...); -#define MAX_TASKS 64 -#define TASK_MAGIC 21979 /* random numbers used as signatures */ -#define TASK_MAGIC_INIT ((rtapi_task*)(-1)) +#define MAX_TASKS 64 +#define TASK_MAGIC 21979 /* random numbers used as signatures */ +#define TASK_MAGIC_INIT ((rtapi_task *)(-1)) extern struct rtapi_task *task_array[MAX_TASKS]; +extern uid_t euid, ruid; //ToDo: Improve + #define WITH_ROOT WithRoot root #endif diff --git a/src/rtapi/uspace_rtapi_main.cc b/src/rtapi/uspace_rtapi_main.cc new file mode 100644 index 00000000000..0617595b5d9 --- /dev/null +++ b/src/rtapi/uspace_rtapi_main.cc @@ -0,0 +1,1062 @@ +/* Copyright (C) 2006-2014 Jeff Epler + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +#include "config.h" +#include + +#ifdef __linux__ +#include +#endif +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef HAVE_SYS_IO_H +#include +#endif +#include +#include +#ifdef __linux__ +#include +#include +#endif +#ifdef __FreeBSD__ +#include +#endif + +#include +#include + +#include "rtapi.h" +#include +#include "hal/hal_priv.h" +#include "uspace_common.h" + +RtapiApp &App(); + +struct message_t { + msg_level_t level; + char msg[1024 - sizeof(level)]; +}; + +boost::lockfree::queue> + rtapi_msg_queue; + +pthread_t queue_thread; +void *queue_function(void * /*arg*/) { + set_namef("rtapi_app:mesg"); + // note: can't use anything in this function that requires App() to exist + // but it's OK to use functions that aren't safe for realtime (that's the + // point of running this in a thread) + while (1) { + pthread_testcancel(); + pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, nullptr); + rtapi_msg_queue.consume_all([](const message_t &m) { + fputs(m.msg, m.level == RTAPI_MSG_ALL ? stdout : stderr); + }); + pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, nullptr); + struct timespec ts = {0, 10000000}; + rtapi_clock_nanosleep(CLOCK_MONOTONIC, 0, &ts, NULL, NULL); + } + return nullptr; +} + +static int sim_rtapi_run_threads(int fd, int (*callback)(int fd)); + +template T DLSYM(void *handle, const std::string &name) { + return (T)(dlsym(handle, name.c_str())); +} + +template T DLSYM(void *handle, const char *name) { + return (T)(dlsym(handle, name)); +} + +static std::map modules; + +static int instance_count = 0; +static int force_exit = 0; + +static int do_newinst_cmd( + const std::string &type, const std::string &name, const std::string &arg +) { + void *module = modules["hal_lib"]; + if (!module) { + rtapi_print_msg( + RTAPI_MSG_ERR, "newinst: hal_lib is required, but not loaded\n" + ); + return -1; + } + + hal_comp_t *(*find_comp_by_name)(char *) = + DLSYM(module, "halpr_find_comp_by_name"); + if (!find_comp_by_name) { + rtapi_print_msg( + RTAPI_MSG_ERR, "newinst: halpr_find_comp_by_name not found\n" + ); + return -1; + } + + hal_comp_t *comp = find_comp_by_name((char *)type.c_str()); + if (!comp) { + rtapi_print_msg( + RTAPI_MSG_ERR, "newinst: component %s not found\n", type.c_str() + ); + return -1; + } + + return comp->make((char *)name.c_str(), (char *)arg.c_str()); +} + +static int do_one_item( + char item_type_char, + const std::string ¶m_name, + const std::string ¶m_value, + void *vitem, + int idx = 0 +) { + char *endp; + switch (item_type_char) { + case 'l': { + long *litem = *(long **)vitem; + litem[idx] = strtol(param_value.c_str(), &endp, 0); + if (*endp) { + rtapi_print_msg( + RTAPI_MSG_ERR, + "`%s' invalid for parameter `%s'\n", + param_value.c_str(), + param_name.c_str() + ); + return -1; + } + return 0; + } + case 'i': { + int *iitem = *(int **)vitem; + iitem[idx] = strtol(param_value.c_str(), &endp, 0); + if (*endp) { + rtapi_print_msg( + RTAPI_MSG_ERR, + "`%s' invalid for parameter `%s'\n", + param_value.c_str(), + param_name.c_str() + ); + return -1; + } + return 0; + } + case 's': { + char **sitem = *(char ***)vitem; + sitem[idx] = strdup(param_value.c_str()); + return 0; + } + default: + rtapi_print_msg( + RTAPI_MSG_ERR, + "%s: Invalid type character `%c'\n", + param_name.c_str(), + item_type_char + ); + return -1; + } + return 0; +} + +void remove_quotes(std::string &s) { + s.erase(remove_copy(s.begin(), s.end(), s.begin(), '"'), s.end()); +} + +static int do_comp_args(void *module, std::vector args) { + for (unsigned i = 1; i < args.size(); i++) { + std::string &s = args[i]; + remove_quotes(s); + size_t idx = s.find('='); + if (idx == std::string::npos) { + rtapi_print_msg( + RTAPI_MSG_ERR, "Invalid parameter `%s'\n", s.c_str() + ); + return -1; + } + std::string param_name(s, 0, idx); + std::string param_value(s, idx + 1); + void *item = DLSYM(module, "rtapi_info_address_" + param_name); + if (!item) { + rtapi_print_msg( + RTAPI_MSG_ERR, "Unknown parameter `%s'\n", s.c_str() + ); + return -1; + } + char **item_type = + DLSYM(module, "rtapi_info_type_" + param_name); + if (!item_type || !*item_type) { + rtapi_print_msg( + RTAPI_MSG_ERR, + "Unknown parameter `%s' (type information missing)\n", + s.c_str() + ); + return -1; + } + + int *max_size_ptr = + DLSYM(module, "rtapi_info_size_" + param_name); + + char item_type_char = **item_type; + if (max_size_ptr) { + int max_size = *max_size_ptr; + size_t idx = 0; + int i = 0; + while (idx != std::string::npos) { + if (i == max_size) { + rtapi_print_msg( + RTAPI_MSG_ERR, + "%s: can only take %d arguments\n", + s.c_str(), + max_size + ); + return -1; + } + size_t idx1 = param_value.find(",", idx); + std::string substr(param_value, idx, idx1 - idx); + int result = do_one_item(item_type_char, s, substr, item, i); + if (result != 0) + return result; + i++; + idx = idx1 == std::string::npos ? idx1 : idx1 + 1; + } + } else { + int result = do_one_item(item_type_char, s, param_value, item); + if (result != 0) + return result; + } + } + return 0; +} + +static int +do_load_cmd(const std::string &name, const std::vector &args) { + void *w = modules[name]; + if (w == NULL) { + std::string what; + what = fmt::format("{}/{}.so", EMC2_RTLIB_DIR, name); + void *module = modules[name] = + dlopen(what.c_str(), RTLD_GLOBAL | RTLD_NOW); + if (!module) { + rtapi_print_msg( + RTAPI_MSG_ERR, "%s: dlopen: %s\n", name.c_str(), dlerror() + ); + modules.erase(name); + return -1; + } + /// XXX handle arguments + int (*start)(void) = DLSYM(module, "rtapi_app_main"); + if (!start) { + rtapi_print_msg( + RTAPI_MSG_ERR, "%s: dlsym: %s\n", name.c_str(), dlerror() + ); + dlclose(module); + modules.erase(name); + return -1; + } + int result; + + result = do_comp_args(module, args); + if (result < 0) { + dlclose(module); + modules.erase(name); + return -1; + } + + if ((result = start()) < 0) { + rtapi_print_msg( + RTAPI_MSG_ERR, + "%s: rtapi_app_main: %s (%d)\n", + name.c_str(), + strerror(-result), + result + ); + dlclose(module); + modules.erase(name); + return result; + } else { + instance_count++; + return 0; + } + } else { + rtapi_print_msg(RTAPI_MSG_ERR, "%s: already exists\n", name.c_str()); + return -1; + } +} + +static int do_unload_cmd(const std::string &name) { + void *w = modules[name]; + if (w == NULL) { + rtapi_print_msg(RTAPI_MSG_ERR, "%s: not loaded\n", name.c_str()); + return -1; + } else { + int (*stop)(void) = DLSYM(w, "rtapi_app_exit"); + if (stop) + stop(); + modules.erase(modules.find(name)); + dlclose(w); + instance_count--; + } + return 0; +} + +static int do_debug_cmd(const std::string &value) { + try { + int new_level = stoi(value); + if (new_level < 0 || new_level > 5) { + rtapi_print_msg( + RTAPI_MSG_ERR, "Debug level must be >=0 and <= 5\n" + ); + return -EINVAL; + } + return rtapi_set_msg_level(new_level); + } catch (std::invalid_argument &e) { + //stoi will throw an exception if parsing is not possible + rtapi_print_msg(RTAPI_MSG_ERR, "Debug level is not a number\n"); + return -EINVAL; + } +} + +struct ReadError : std::exception {}; +struct WriteError : std::exception {}; + +static int read_number(int fd) { + int r = 0, neg = 1; + char ch; + + while (1) { + int res = read(fd, &ch, 1); + if (res != 1) + return -1; + if (ch == '-') + neg = -1; + else if (ch == ' ') + return r * neg; + else + r = 10 * r + ch - '0'; + } +} + +static std::string read_string(int fd) { + int len = read_number(fd); + if (len < 0) + throw ReadError(); + if (!len) + return std::string(); + std::string str(len, 0); + if (read(fd, str.data(), len) != len) + throw ReadError(); + return str; +} + +static std::vector read_strings(int fd) { + std::vector result; + int count = read_number(fd); + if (count < 0) + return result; + for (int i = 0; i < count; i++) { + result.push_back(read_string(fd)); + } + return result; +} + +static void write_number(std::string &buf, int num) { + buf = buf + fmt::format("{} ", num); +} + +static void write_string(std::string &buf, const std::string &s) { + write_number(buf, s.size()); + buf += s; +} + +static void write_strings(int fd, const std::vector &strings) { + std::string buf; + write_number(buf, strings.size()); + for (unsigned int i = 0; i < strings.size(); i++) { + write_string(buf, strings[i]); + } + if (write(fd, buf.data(), buf.size()) != (ssize_t)buf.size()) + throw WriteError(); +} + +static int handle_command(std::vector args) { + if (args.size() == 0) { + return 0; + } + if (args.size() == 1 && args[0] == "exit") { + force_exit = 1; + return 0; + } else if (args.size() >= 2 && args[0] == "load") { + std::string name = args[1]; + args.erase(args.begin()); + return do_load_cmd(name, args); + } else if (args.size() == 2 && args[0] == "unload") { + return do_unload_cmd(args[1]); + } else if (args.size() == 3 && args[0] == "newinst") { + return do_newinst_cmd(args[1], args[2], ""); + } else if (args.size() == 4 && args[0] == "newinst") { + return do_newinst_cmd(args[1], args[2], args[3]); + } else if (args.size() == 2 && args[0] == "debug") { + return do_debug_cmd(args[1]); + } else { + rtapi_print_msg( + RTAPI_MSG_ERR, + "Unrecognized command starting with %s\n", + args[0].c_str() + ); + return -1; + } +} + +static int slave(int fd, const std::vector &args) { + try { + write_strings(fd, args); + } catch (WriteError &e) { + rtapi_print_msg( + RTAPI_MSG_ERR, + "rtapi_app: failed to write to master: %s\n", + strerror(errno) + ); + } + + int result = read_number(fd); + return result; +} + +static int callback(int fd) { + struct sockaddr_un client_addr; + memset(&client_addr, 0, sizeof(client_addr)); + socklen_t len = sizeof(client_addr); + int fd1 = accept(fd, (sockaddr *)&client_addr, &len); + if (fd1 < 0) { + rtapi_print_msg( + RTAPI_MSG_ERR, + "rtapi_app: failed to accept connection from slave: %s\n", + strerror(errno) + ); + return -1; + } else { + int result; + try { + result = handle_command(read_strings(fd1)); + } catch (ReadError &e) { + rtapi_print_msg( + RTAPI_MSG_ERR, + "rtapi_app: failed to read from slave: %s\n", + strerror(errno) + ); + close(fd1); + return -1; + } + std::string buf; + write_number(buf, result); + if (write(fd1, buf.data(), buf.size()) != (ssize_t)buf.size()) { + rtapi_print_msg( + RTAPI_MSG_ERR, + "rtapi_app: failed to write to slave: %s\n", + strerror(errno) + ); + }; + close(fd1); + } + return !force_exit && instance_count > 0; +} + +static pthread_t main_thread{}; + +static int master(int fd, const std::vector &args) { + main_thread = pthread_self(); + int result; + if ((result = + pthread_create(&queue_thread, nullptr, &queue_function, nullptr) + ) != 0) { + errno = result; + perror("pthread_create (queue function)"); + return -1; + } + do_load_cmd("hal_lib", std::vector()); + instance_count = 0; + App(); // force rtapi_app to be created + if (args.size()) { + result = handle_command(args); + if (result != 0) + goto out; + if (force_exit || instance_count == 0) + goto out; + } + sim_rtapi_run_threads(fd, callback); +out: + pthread_cancel(queue_thread); + pthread_join(queue_thread, nullptr); + rtapi_msg_queue.consume_all([](const message_t &m) { + fputs(m.msg, m.level == RTAPI_MSG_ALL ? stdout : stderr); + }); + return result; +} + +static std::string get_fifo_path() { + std::string s; + if (getenv("RTAPI_FIFO_PATH")) { + s = getenv("RTAPI_FIFO_PATH"); + } else if (getenv("HOME")) { + s = std::string(getenv("HOME")) + "/.rtapi_fifo"; + } else { + rtapi_print_msg( + RTAPI_MSG_ERR, + "rtapi_app: RTAPI_FIFO_PATH and HOME are unset. rtapi fifo " + "creation is unsafe.\n" + ); + } + return s; +} + +static int get_fifo_path(char *buf, size_t bufsize) { + int len; + const std::string s = get_fifo_path(); + if (s.empty()) { + return -1; + } + if (s.size() + 1 > sizeof(sockaddr_un::sun_path)) { + rtapi_print_msg( + RTAPI_MSG_ERR, + "rtapi_app: rtapi fifo path is too long (arch limit %zd): %s\n", + sizeof(sockaddr_un::sun_path), + s.c_str() + ); + return -1; + } + len = snprintf(buf + 1, bufsize - 1, "%s", s.c_str()); + return len; +} + +int main(int argc, char **argv) { + if (getuid() == 0) { + char *fallback_uid_str = getenv("RTAPI_UID"); + int fallback_uid = fallback_uid_str ? atoi(fallback_uid_str) : 0; + if (fallback_uid == 0) { + // Cppcheck cannot see EMC2_BIN_DIR when RTAPI is defined, but that + // doesn't happen in uspace. + fprintf( + stderr, + "Refusing to run as root without fallback UID specified\n" + "To run under a debugger with I/O, use e.g.,\n" + // cppcheck-suppress unknownMacro + " sudo env RTAPI_UID=`id -u` " + "RTAPI_FIFO_PATH=$HOME/.rtapi_fifo gdb " EMC2_BIN_DIR + "/rtapi_app\n" + ); + exit(1); + } + if (setreuid(fallback_uid, 0) != 0) { + perror("setreuid"); + abort(); + } + fprintf( + stderr, + "Running with fallback_uid. getuid()=%d geteuid()=%d\n", + getuid(), + geteuid() + ); + } + ruid = getuid(); + euid = geteuid(); + if (setresuid(euid, euid, ruid) != 0) { + perror("setresuid"); + abort(); + } +#ifdef __linux__ + setfsuid(ruid); +#endif + std::vector args; + for (int i = 1; i < argc; i++) { + args.push_back(std::string(argv[i])); + } + +become_master: + int len = 0; + int fd = socket(PF_UNIX, SOCK_STREAM, 0); + if (fd == -1) { + perror("socket"); + exit(1); + } + + int enable = 1; + setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable)); + struct sockaddr_un addr; + memset(&addr, 0x0, sizeof(addr)); + addr.sun_family = AF_UNIX; + if ((len = get_fifo_path(addr.sun_path, sizeof(addr.sun_path))) < 0) + exit(1); + + // plus one because we use the abstract namespace, it will show up in + // /proc/net/unix prefixed with an @ + int result = + ::bind(fd, (sockaddr *)&addr, len + sizeof(addr.sun_family) + 1); + + if (result == 0) { + //If called in master mode with exit command, no need to start master + //and exit again + if (args.size() == 1 && args[0] == "exit") { + return 0; + } + int result = listen(fd, 10); + if (result != 0) { + perror("listen"); + exit(1); + } + setsid(); // create a new session if we can... + result = master(fd, args); + return result; + } else if (errno == EADDRINUSE) { + struct timeval t0, t1; + gettimeofday(&t0, NULL); + gettimeofday(&t1, NULL); + for (int i = 0; i < 3 || (t1.tv_sec < 3 + t0.tv_sec); i++) { + result = connect( + fd, (sockaddr *)&addr, len + sizeof(addr.sun_family) + 1 + ); + if (result == 0) + break; + if (i == 0) + srand48(t0.tv_sec ^ t0.tv_usec); + usleep(lrand48() % 100000); + gettimeofday(&t1, NULL); + } + if (result < 0 && errno == ECONNREFUSED) { + fprintf(stderr, "Waited 3 seconds for master. giving up.\n"); + close(fd); + goto become_master; + } + if (result < 0) { + fprintf(stderr, "connect %s: %s", addr.sun_path, strerror(errno)); + exit(1); + } + return slave(fd, args); + } else { + perror("bind"); + exit(1); + } +} + + +/* These structs hold data associated with objects like tasks, etc. */ +/* Task handles are pointers to these structs. */ + +struct rtapi_module { + int magic; +}; + +#define MODULE_MAGIC 30812 +#define SHMEM_MAGIC 25453 + +#define MAX_MODULES 64 +#define MODULE_OFFSET 32768 + +static void signal_handler(int sig, siginfo_t * /*si*/, void * /*uctx*/) { + switch (sig) { + case SIGXCPU: + // should not happen - must be handled in RTAPI if enabled + rtapi_print_msg( + RTAPI_MSG_ERR, "rtapi_app: BUG: SIGXCPU received - exiting\n" + ); + exit(0); + break; + + case SIGTERM: + rtapi_print_msg(RTAPI_MSG_ERR, "rtapi_app: SIGTERM - shutting down\n"); + exit(0); + break; + + default: // pretty bad + rtapi_print_msg( + RTAPI_MSG_ERR, "rtapi_app: caught signal %d - dumping core\n", sig + ); + sleep(1); // let syslog drain + signal(sig, SIG_DFL); + // for reasons unknown raise(sig); doesn't lead to core dump file + // but this will + kill(getpid(), sig); + break; + } + exit(1); +} + +const size_t PRE_ALLOC_SIZE = 1024 * 1024 * 32; +const static struct rlimit unlimited = {RLIM_INFINITY, RLIM_INFINITY}; +static void configure_memory() { + int res = setrlimit(RLIMIT_MEMLOCK, &unlimited); + if (res < 0) + perror("setrlimit"); + + res = mlockall(MCL_CURRENT | MCL_FUTURE); + if (res < 0) + perror("mlockall"); + +#ifdef __linux__ + /* Turn off malloc trimming.*/ + if (!mallopt(M_TRIM_THRESHOLD, -1)) { + rtapi_print_msg( + RTAPI_MSG_WARN, "mallopt(M_TRIM_THRESHOLD, -1) failed\n" + ); + } + /* Turn off mmap usage. */ + if (!mallopt(M_MMAP_MAX, 0)) { + rtapi_print_msg(RTAPI_MSG_WARN, "mallopt(M_MMAP_MAX, -1) failed\n"); + } +#endif + /* + * The following code seems pointless, but there is a non-observable effect + * in the allocation and loop. + * + * The malloc() is forced to set brk() because mmap() allocation is + * disabled in a call to mallopt() above. All touched pages become resident + * and locked in the loop because of above mlockall() call (see notes in + * mlockall(2)). The mallopt() trim setting prevents the brk() from being + * reduced after free(), effectively creating an open space for future + * allocations that will not generate page faults. + * + * The qualifier 'volatile' on the buffer pointer is required because newer + * clang would remove the malloc(), for()-loop and free() completely. + * Marking 'buf' volatile ensures that the code will remain in place. + */ + volatile char *buf = static_cast(malloc(PRE_ALLOC_SIZE)); + if (buf == NULL) { + rtapi_print_msg(RTAPI_MSG_WARN, "malloc(PRE_ALLOC_SIZE) failed\n"); + return; + } + long pagesize = sysconf(_SC_PAGESIZE); + /* Touch each page in this piece of memory to get it mapped into RAM */ + for (size_t i = 0; i < PRE_ALLOC_SIZE; i += pagesize) { + /* Each write to this buffer will generate a pagefault. + * Once the pagefault is handled a page will be locked in + * memory and never given back to the system. */ + buf[i] = 0; + } + free((void *)buf); +} + +static int harden_rt() { + if (!rtapi_is_realtime()) + return -EINVAL; + + WITH_ROOT; +#if defined(__linux__) && (defined(__x86_64__) || defined(__i386__)) + if (iopl(3) < 0) { + rtapi_print_msg( + RTAPI_MSG_ERR, + "iopl() failed: %s\n" + "cannot gain I/O privileges - " + "forgot 'sudo make setuid' or using secure boot? -" + "parallel port access is not allowed\n", + strerror(errno) + ); + } +#endif + + struct sigaction sig_act = {}; +#ifdef __linux__ + // enable realtime + if (setrlimit(RLIMIT_RTPRIO, &unlimited) < 0) { + rtapi_print_msg( + RTAPI_MSG_WARN, "setrlimit(RTLIMIT_RTPRIO): %s\n", strerror(errno) + ); + return -errno; + } + + // enable core dumps + if (setrlimit(RLIMIT_CORE, &unlimited) < 0) + rtapi_print_msg( + RTAPI_MSG_WARN, + "setrlimit: %s - core dumps may be truncated or non-existent\n", + strerror(errno) + ); + + // even when setuid root + if (prctl(PR_SET_DUMPABLE, 1) < 0) + rtapi_print_msg( + RTAPI_MSG_WARN, + "prctl(PR_SET_DUMPABLE) failed: no core dumps will be created - %d " + "- %s\n", + errno, + strerror(errno) + ); +#endif /* __linux__ */ + + configure_memory(); + + sigemptyset(&sig_act.sa_mask); + sig_act.sa_handler = SIG_IGN; + sig_act.sa_sigaction = NULL; + + // prevent stopping of RT threads by ^Z + sigaction(SIGTSTP, &sig_act, (struct sigaction *)NULL); + + sig_act.sa_sigaction = signal_handler; + sig_act.sa_flags = SA_SIGINFO; + + sigaction(SIGSEGV, &sig_act, (struct sigaction *)NULL); + sigaction(SIGILL, &sig_act, (struct sigaction *)NULL); + sigaction(SIGFPE, &sig_act, (struct sigaction *)NULL); + sigaction(SIGTERM, &sig_act, (struct sigaction *)NULL); + sigaction(SIGINT, &sig_act, (struct sigaction *)NULL); + +#ifdef __linux__ + int fd = open("/dev/cpu_dma_latency", O_WRONLY | O_CLOEXEC); + if (fd < 0) { + rtapi_print_msg( + RTAPI_MSG_WARN, + "failed to open /dev/cpu_dma_latency: %s\n", + strerror(errno) + ); + } else { + int r; + r = write(fd, "\0\0\0\0", 4); + if (r != 4) { + rtapi_print_msg( + RTAPI_MSG_WARN, + "failed to write to /dev/cpu_dma_latency: %s\n", + strerror(errno) + ); + } + // deliberately leak fd until program exit + } +#endif /* __linux__ */ + return 0; +} + +static RtapiApp *makeDllApp(std::string dllName, int policy) { + void *dll = nullptr; + dll = dlopen(dllName.c_str(), RTLD_NOW); + if (!dll) { + fprintf(stderr, "dlopen: %s\n", dlerror()); + return nullptr; + } + auto fn = reinterpret_cast(dlsym(dll, "make")); + if (!fn) { + fprintf(stderr, "dlsym: %s\n", dlerror()); + return nullptr; + } + auto result = fn(policy); + if (!result) { + fprintf(stderr, "dlsym: %s\n", dlerror()); + return nullptr; + } + return result; +} + +static RtapiApp *makeApp() { + RtapiApp *app; + if (euid != 0 || harden_rt() < 0) { + app = makeDllApp(EMC2_HOME "/lib/libuspace-posix.so.0", SCHED_OTHER); + } else { + WithRoot r; + if (detect_xenomai_evl()) { + app = makeDllApp(EMC2_HOME "/lib/libuspace-xenomai-evl.so.0", SCHED_FIFO); + } else if (detect_xenomai()) { + app = makeDllApp(EMC2_HOME "/lib/libuspace-xenomai.so.0", SCHED_FIFO); + } else if (detect_rtai()) { + app = makeDllApp(EMC2_HOME "/lib/libuspace-rtai.so.0", SCHED_FIFO); + } else { + app = makeDllApp(EMC2_HOME "/lib/libuspace-posix.so.0", SCHED_FIFO); + } + } + + if (!app) { + throw std::invalid_argument("Could not load rtapi dll"); + } else { + return app; + } +} +RtapiApp &App() { + static RtapiApp *app = makeApp(); + return *app; +} + +/* data for all tasks */ +struct rtapi_task *task_array[MAX_TASKS]; + +int rtapi_prio_highest(void) { + return App().prio_highest(); +} + +int rtapi_prio_lowest(void) { + return App().prio_lowest(); +} + +int rtapi_prio_next_higher(int prio) { + return App().prio_next_higher(prio); +} + +int rtapi_prio_next_lower(int prio) { + return App().prio_next_lower(prio); +} + +long rtapi_clock_set_period(long nsecs) { + return App().clock_set_period(nsecs); +} + +int rtapi_task_new( + void (*taskcode)(void *), + void *arg, + int prio, + int owner, + unsigned long int stacksize, + int uses_fp +) { + return App().task_new(taskcode, arg, prio, owner, stacksize, uses_fp); +} + +int rtapi_task_delete(int id) { + return App().task_delete(id); +} + +int rtapi_task_start(int task_id, unsigned long period_nsec) { + int ret = App().task_start(task_id, period_nsec); + if (ret != 0) { + errno = -ret; + perror("rtapi_task_start()"); + } + return ret; +} + +int rtapi_task_pause(int task_id) { + return App().task_pause(task_id); +} + +int rtapi_task_resume(int task_id) { + return App().task_resume(task_id); +} + +int rtapi_task_self() { + return App().task_self(); +} + +long long rtapi_task_pll_get_reference(void) { + return App().task_pll_get_reference(); +} + +int rtapi_task_pll_set_correction(long value) { + return App().task_pll_set_correction(value); +} + +void rtapi_wait(void) { + App().wait(); +} + +void rtapi_outb(unsigned char byte, unsigned int port) { + App().do_outb(byte, port); +} + +unsigned char rtapi_inb(unsigned int port) { + return App().do_inb(port); +} + +long int simple_strtol(const char *nptr, char **endptr, int base) { + return strtol(nptr, endptr, base); +} + +int sim_rtapi_run_threads(int fd, int (*callback)(int fd)) { + return App().run_threads(fd, callback); +} + +long long rtapi_get_time() { + return App().do_get_time(); +} + +void default_rtapi_msg_handler(msg_level_t level, const char *fmt, va_list ap) { + if (main_thread && pthread_self() != main_thread) { + message_t m; + m.level = level; + vsnprintf(m.msg, sizeof(m.msg), fmt, ap); + rtapi_msg_queue.push(m); + } else { + vfprintf(level == RTAPI_MSG_ALL ? stdout : stderr, fmt, ap); + } +} + +long int rtapi_delay_max() { + return 10000; +} + +void rtapi_delay(long ns) { + if (ns > rtapi_delay_max()) + ns = rtapi_delay_max(); + App().do_delay(ns); +} + +const unsigned long ONE_SEC_IN_NS = 1000000000; +void rtapi_timespec_advance( + struct timespec &result, const struct timespec &src, unsigned long nsec +) { + time_t sec = src.tv_sec; + while (nsec >= ONE_SEC_IN_NS) { + ++sec; + nsec -= ONE_SEC_IN_NS; + } + nsec += src.tv_nsec; + if (nsec >= ONE_SEC_IN_NS) { + ++sec; + nsec -= ONE_SEC_IN_NS; + } + result.tv_sec = sec; + result.tv_nsec = nsec; +} + +int rtapi_open_as_root(const char *filename, int mode) { + WITH_ROOT; + int r = open(filename, mode); + if (r < 0) + return -errno; + return r; +} + +int rtapi_spawn_as_root( + pid_t *pid, + const char *path, + const posix_spawn_file_actions_t *file_actions, + const posix_spawnattr_t *attrp, + char *const argv[], + char *const envp[] +) { + return posix_spawn(pid, path, file_actions, attrp, argv, envp); +} + +int rtapi_spawnp_as_root( + pid_t *pid, + const char *path, + const posix_spawn_file_actions_t *file_actions, + const posix_spawnattr_t *attrp, + char *const argv[], + char *const envp[] +) { + return posix_spawnp(pid, path, file_actions, attrp, argv, envp); +} diff --git a/src/rtapi/uspace_rtapi_parport.cc b/src/rtapi/uspace_rtapi_parport.cc index f396837c71f..f208181fa97 100644 --- a/src/rtapi/uspace_rtapi_parport.cc +++ b/src/rtapi/uspace_rtapi_parport.cc @@ -19,7 +19,7 @@ #include #include #include -#include "rtapi_uspace.hh" +#include "uspace_rtapi_app.hh" #include #include #include diff --git a/src/rtapi/uspace_xenomai.cc b/src/rtapi/uspace_xenomai.cc index c3c6e432ea5..5f0c5139472 100644 --- a/src/rtapi/uspace_xenomai.cc +++ b/src/rtapi/uspace_xenomai.cc @@ -1,18 +1,35 @@ +/* Copyright (C) 2016 Jeff Epler + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ #include "config.h" #include "rtapi.h" -#include "rtapi_uspace.hh" +#include "uspace_rtapi_app.hh" #include -#include +#include #include #include +#include #ifdef HAVE_SYS_IO_H #include #endif -namespace -{ -struct RtaiTask : rtapi_task { - RtaiTask() : rtapi_task{}, cancel{}, thr{} {} +namespace { +struct XenomaiTask : rtapi_task { + XenomaiTask() : rtapi_task{}, cancel{}, thr{} { + } std::atomic_int cancel; pthread_t thr; }; @@ -23,13 +40,14 @@ struct XenomaiApp : RtapiApp { pthread_once(&key_once, init_key); } - RtaiTask *do_task_new() { - return new RtaiTask; + struct rtapi_task *do_task_new() { + return new XenomaiTask; } int task_delete(int id) { - auto task = ::rtapi_get_task(id); - if(!task) return -EINVAL; + auto task = ::rtapi_get_task(id); + if (!task) + return -EINVAL; task->cancel = 1; pthread_join(task->thr, nullptr); @@ -40,8 +58,9 @@ struct XenomaiApp : RtapiApp { } int task_start(int task_id, unsigned long period_nsec) { - auto task = ::rtapi_get_task(task_id); - if(!task) return -EINVAL; + auto task = ::rtapi_get_task(task_id); + if (!task) + return -EINVAL; task->period = period_nsec; struct sched_param param; @@ -52,39 +71,43 @@ struct XenomaiApp : RtapiApp { task->pll_correction_limit = period_nsec / 100; task->pll_correction = 0; - int nprocs = sysconf( _SC_NPROCESSORS_ONLN ); + int nprocs = sysconf(_SC_NPROCESSORS_ONLN); - int ret; pthread_attr_t attr; - if((ret = pthread_attr_init(&attr)) != 0) + int ret; + if ((ret = pthread_attr_init(&attr)) != 0) return -ret; - if((ret = pthread_attr_setstacksize(&attr, task->stacksize)) != 0) + if ((ret = pthread_attr_setstacksize(&attr, task->stacksize)) != 0) return -ret; - if((ret = pthread_attr_setschedpolicy(&attr, policy)) != 0) + if ((ret = pthread_attr_setschedpolicy(&attr, policy)) != 0) return -ret; - if((ret = pthread_attr_setschedparam(&attr, ¶m)) != 0) + if ((ret = pthread_attr_setschedparam(&attr, ¶m)) != 0) return -ret; - if((ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED)) != 0) + if ((ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED)) != 0) return -ret; - if(nprocs > 1){ + if (nprocs > 1) { const static int rt_cpu_number = find_rt_cpu_number(); - rtapi_print_msg(RTAPI_MSG_INFO, "rt_cpu_number = %i\n", rt_cpu_number); - if(rt_cpu_number != -1) { + rtapi_print_msg( + RTAPI_MSG_INFO, "rt_cpu_number = %i\n", rt_cpu_number + ); + if (rt_cpu_number != -1) { cpu_set_t cpuset; CPU_ZERO(&cpuset); CPU_SET(rt_cpu_number, &cpuset); - if((ret = pthread_attr_setaffinity_np(&attr, sizeof(cpuset), &cpuset)) != 0) + if ((ret = pthread_attr_setaffinity_np( + &attr, sizeof(cpuset), &cpuset + )) != 0) return -ret; } } - if((ret = pthread_create(&task->thr, &attr, &wrapper, reinterpret_cast(task))) != 0) + if ((ret = pthread_create(&task->thr, &attr, &wrapper, reinterpret_cast(task))) != 0) return -ret; return 0; } static void *wrapper(void *arg) { - auto task = reinterpret_cast(arg); + auto task = reinterpret_cast(arg); pthread_setspecific(key, arg); struct timespec now; @@ -95,9 +118,11 @@ struct XenomaiApp : RtapiApp { // "xenomai: watchdog triggered" and rtapi_app was killed. // // encountered on: 3.18.20-xenomai-2.6.5 with a 2-thread SMP system - rtapi_timespec_advance(task->nextstart, now, task->period + task->pll_correction); + rtapi_timespec_advance( + task->nextstart, now, task->period + task->pll_correction + ); - (task->taskcode) (task->arg); + (task->taskcode)(task->arg); rtapi_print("ERROR: reached end of wrapper for task %d\n", task->id); return nullptr; @@ -114,38 +139,48 @@ struct XenomaiApp : RtapiApp { } long long task_pll_get_reference(void) { - struct rtapi_task *task = reinterpret_cast(pthread_getspecific(key)); - if(!task) return 0; + struct rtapi_task *task = + reinterpret_cast(pthread_getspecific(key)); + if (!task) + return 0; return task->nextstart.tv_sec * 1000000000LL + task->nextstart.tv_nsec; } int task_pll_set_correction(long value) { - struct rtapi_task *task = reinterpret_cast(pthread_getspecific(key)); - if(!task) return -EINVAL; - if (value > task->pll_correction_limit) value = task->pll_correction_limit; - if (value < -(task->pll_correction_limit)) value = -(task->pll_correction_limit); + struct rtapi_task *task = + reinterpret_cast(pthread_getspecific(key)); + if (!task) + return -EINVAL; + if (value > task->pll_correction_limit) + value = task->pll_correction_limit; + if (value < -(task->pll_correction_limit)) + value = -(task->pll_correction_limit); task->pll_correction = value; return 0; } void wait() { int task_id = task_self(); - auto task = ::rtapi_get_task(task_id); - if(task->cancel) { + auto task = ::rtapi_get_task(task_id); + if (task->cancel) { pthread_exit(nullptr); } - rtapi_timespec_advance(task->nextstart, task->nextstart, task->period + task->pll_correction); + rtapi_timespec_advance( + task->nextstart, + task->nextstart, + task->period + task->pll_correction + ); struct timespec now; clock_gettime(CLOCK_MONOTONIC, &now); - if(rtapi_timespec_less(task->nextstart, now)) - { - if(policy == SCHED_FIFO) + if (rtapi_timespec_less(task->nextstart, now)) { + if (policy == SCHED_FIFO) unexpected_realtime_delay(task); - } - else - { - int res = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &task->nextstart, nullptr); - if(res < 0) perror("clock_nanosleep"); + } else { + int res = clock_nanosleep( + CLOCK_MONOTONIC, TIMER_ABSTIME, &task->nextstart, nullptr + ); + if (res < 0) + perror("clock_nanosleep"); } } @@ -153,6 +188,7 @@ struct XenomaiApp : RtapiApp { #ifdef HAVE_SYS_IO_H return inb(port); #else + (void)port; return 0; #endif } @@ -161,18 +197,23 @@ struct XenomaiApp : RtapiApp { #ifdef HAVE_SYS_IO_H return outb(val, port); #else - return 0; + (void)val; + (void)port; #endif } int run_threads(int fd, int (*callback)(int fd)) { - while(callback(fd)) { /* nothing */ } + while (callback(fd)) { + /* nothing */ + } return 0; } int task_self() { - struct rtapi_task *task = reinterpret_cast(pthread_getspecific(key)); - if(!task) return -EINVAL; + struct rtapi_task *task = + reinterpret_cast(pthread_getspecific(key)); + if (!task) + return -EINVAL; return task->id; } @@ -196,11 +237,16 @@ struct XenomaiApp : RtapiApp { pthread_once_t XenomaiApp::key_once; pthread_key_t XenomaiApp::key; -} +} // namespace -extern "C" RtapiApp *make(); +extern "C" RtapiApp *make(int policy); -RtapiApp *make() { - rtapi_print_msg(RTAPI_MSG_ERR, "Note: Using XENOMAI (posix-skin) realtime\n"); - return new XenomaiApp; +RtapiApp *make(int policy) { + if (policy != SCHED_FIFO) { + throw std::invalid_argument("Only SCHED_FIFO allowed"); + } + rtapi_print_msg( + RTAPI_MSG_ERR, "Note: Using XENOMAI (posix-skin) realtime\n" + ); + return new XenomaiApp(); } diff --git a/src/rtapi/uspace_xenomai_evl.cc b/src/rtapi/uspace_xenomai_evl.cc index b4f65f2f4c2..a19d57b39ec 100644 --- a/src/rtapi/uspace_xenomai_evl.cc +++ b/src/rtapi/uspace_xenomai_evl.cc @@ -1,6 +1,25 @@ +/* Copyright (C) 2016 Jeff Epler + * Copyright (C) 2026 Hannes Diethelm + * Copy uspace_xenomai.cc and adapted to Xenomai4 EVL + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + #include "config.h" #include "rtapi.h" -#include "rtapi_uspace.hh" +#include "uspace_rtapi_app.hh" #include @@ -13,31 +32,33 @@ #include #include #include +#include #ifdef HAVE_SYS_IO_H #include #endif -namespace -{ -struct RtaiTask : rtapi_task { - RtaiTask() : rtapi_task{}, cancel{}, thr{} {} +namespace { +struct EvlTask : rtapi_task { + EvlTask() : rtapi_task{}, cancel{}, thr{} { + } std::atomic_int cancel; pthread_t thr; }; -struct XenomaiApp : RtapiApp { - XenomaiApp() : RtapiApp(SCHED_FIFO) { +struct EvlApp : RtapiApp { + EvlApp() : RtapiApp(SCHED_FIFO) { pthread_once(&key_once, init_key); } - RtaiTask *do_task_new() { - return new RtaiTask; + struct rtapi_task *do_task_new() { + return new EvlTask; } int task_delete(int id) { - auto task = ::rtapi_get_task(id); - if(!task) return -EINVAL; + auto task = ::rtapi_get_task(id); + if (!task) + return -EINVAL; task->cancel = 1; pthread_join(task->thr, nullptr); @@ -48,8 +69,9 @@ struct XenomaiApp : RtapiApp { } int task_start(int task_id, unsigned long period_nsec) { - auto task = ::rtapi_get_task(task_id); - if(!task) return -EINVAL; + auto task = ::rtapi_get_task(task_id); + if (!task) + return -EINVAL; task->period = period_nsec; struct sched_param param; @@ -60,39 +82,43 @@ struct XenomaiApp : RtapiApp { task->pll_correction_limit = period_nsec / 100; task->pll_correction = 0; - int nprocs = sysconf( _SC_NPROCESSORS_ONLN ); + int nprocs = sysconf(_SC_NPROCESSORS_ONLN); - int ret; pthread_attr_t attr; - if((ret = pthread_attr_init(&attr)) != 0) + int ret; + if ((ret = pthread_attr_init(&attr)) != 0) return -ret; - if((ret = pthread_attr_setstacksize(&attr, task->stacksize)) != 0) + if ((ret = pthread_attr_setstacksize(&attr, task->stacksize)) != 0) return -ret; - if((ret = pthread_attr_setschedpolicy(&attr, policy)) != 0) + if ((ret = pthread_attr_setschedpolicy(&attr, policy)) != 0) return -ret; - if((ret = pthread_attr_setschedparam(&attr, ¶m)) != 0) + if ((ret = pthread_attr_setschedparam(&attr, ¶m)) != 0) return -ret; - if((ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED)) != 0) + if ((ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED)) != 0) return -ret; - if(nprocs > 1){ + if (nprocs > 1) { const static int rt_cpu_number = find_rt_cpu_number(); - rtapi_print_msg(RTAPI_MSG_INFO, "rt_cpu_number = %i\n", rt_cpu_number); - if(rt_cpu_number != -1) { + rtapi_print_msg( + RTAPI_MSG_INFO, "rt_cpu_number = %i\n", rt_cpu_number + ); + if (rt_cpu_number != -1) { cpu_set_t cpuset; CPU_ZERO(&cpuset); CPU_SET(rt_cpu_number, &cpuset); - if((ret = pthread_attr_setaffinity_np(&attr, sizeof(cpuset), &cpuset)) != 0) + if ((ret = pthread_attr_setaffinity_np( + &attr, sizeof(cpuset), &cpuset + )) != 0) return -ret; } } - if((ret = pthread_create(&task->thr, &attr, &wrapper, reinterpret_cast(task))) != 0) + if ((ret = pthread_create(&task->thr, &attr, &wrapper, reinterpret_cast(task))) != 0) return -ret; return 0; } static void *wrapper(void *arg) { - auto task = reinterpret_cast(arg); + auto task = reinterpret_cast(arg); pthread_setspecific(key, arg); { @@ -100,8 +126,10 @@ struct XenomaiApp : RtapiApp { /* Attach to the core. */ rtapi_print("linuxcnc-task:%d\n", gettid()); int tfd = evl_attach_self("linuxcnc-thread:%d", gettid()); - if (tfd < 0){ - rtapi_print("evl_attach_self() failed ret %i errno %i\n", tfd, errno); + if (tfd < 0) { + rtapi_print( + "evl_attach_self() failed ret %i errno %i\n", tfd, errno + ); } } @@ -113,9 +141,11 @@ struct XenomaiApp : RtapiApp { // "xenomai: watchdog triggered" and rtapi_app was killed. // // encountered on: 3.18.20-xenomai-2.6.5 with a 2-thread SMP system - rtapi_timespec_advance(task->nextstart, now, task->period + task->pll_correction); + rtapi_timespec_advance( + task->nextstart, now, task->period + task->pll_correction + ); - (task->taskcode) (task->arg); + (task->taskcode)(task->arg); rtapi_print("ERROR: reached end of wrapper for task %d\n", task->id); return nullptr; @@ -132,38 +162,46 @@ struct XenomaiApp : RtapiApp { } long long task_pll_get_reference(void) { - struct rtapi_task *task = reinterpret_cast(pthread_getspecific(key)); - if(!task) return 0; + struct rtapi_task *task = + reinterpret_cast(pthread_getspecific(key)); + if (!task) + return 0; return task->nextstart.tv_sec * 1000000000LL + task->nextstart.tv_nsec; } int task_pll_set_correction(long value) { - struct rtapi_task *task = reinterpret_cast(pthread_getspecific(key)); - if(!task) return -EINVAL; - if (value > task->pll_correction_limit) value = task->pll_correction_limit; - if (value < -(task->pll_correction_limit)) value = -(task->pll_correction_limit); + struct rtapi_task *task = + reinterpret_cast(pthread_getspecific(key)); + if (!task) + return -EINVAL; + if (value > task->pll_correction_limit) + value = task->pll_correction_limit; + if (value < -(task->pll_correction_limit)) + value = -(task->pll_correction_limit); task->pll_correction = value; return 0; } void wait() { int task_id = task_self(); - auto task = ::rtapi_get_task(task_id); - if(task->cancel) { + auto task = ::rtapi_get_task(task_id); + if (task->cancel) { pthread_exit(nullptr); } - rtapi_timespec_advance(task->nextstart, task->nextstart, task->period + task->pll_correction); + rtapi_timespec_advance( + task->nextstart, + task->nextstart, + task->period + task->pll_correction + ); struct timespec now; evl_read_clock(EVL_CLOCK_MONOTONIC, &now); - if(rtapi_timespec_less(task->nextstart, now)) - { - if(policy == SCHED_FIFO) + if (rtapi_timespec_less(task->nextstart, now)) { + if (policy == SCHED_FIFO) unexpected_realtime_delay(task); - } - else - { + } else { int res = evl_sleep_until(EVL_CLOCK_MONOTONIC, &task->nextstart); - if(res < 0) perror("evl_sleep_until"); + if (res < 0) + perror("evl_sleep_until"); } } @@ -171,6 +209,7 @@ struct XenomaiApp : RtapiApp { #ifdef HAVE_SYS_IO_H return inb(port); #else + (void)port; return 0; #endif } @@ -179,18 +218,23 @@ struct XenomaiApp : RtapiApp { #ifdef HAVE_SYS_IO_H return outb(val, port); #else - return 0; + (void)val; + (void)port; #endif } int run_threads(int fd, int (*callback)(int fd)) { - while(callback(fd)) { /* nothing */ } + while (callback(fd)) { + /* nothing */ + } return 0; } int task_self() { - struct rtapi_task *task = reinterpret_cast(pthread_getspecific(key)); - if(!task) return -EINVAL; + struct rtapi_task *task = + reinterpret_cast(pthread_getspecific(key)); + if (!task) + return -EINVAL; return task->id; } @@ -214,13 +258,16 @@ struct XenomaiApp : RtapiApp { } }; -pthread_once_t XenomaiApp::key_once; -pthread_key_t XenomaiApp::key; -} +pthread_once_t EvlApp::key_once; +pthread_key_t EvlApp::key; +} // namespace -extern "C" RtapiApp *make(); +extern "C" RtapiApp *make(int policy); -RtapiApp *make() { +RtapiApp *make(int policy) { + if (policy != SCHED_FIFO) { + throw std::invalid_argument("Only SCHED_FIFO allowed"); + } rtapi_print_msg(RTAPI_MSG_ERR, "Note: Using XENOMAI4 EVL realtime\n"); - return new XenomaiApp; + return new EvlApp(); }