uspace_rtapi_app.cc
1 /* Copyright (C) 2006-2014 Jeff Epler <jepler@unpythonic.net> 2 * 3 * This program is free software; you can redistribute it and/or modify 4 * it under the terms of the GNU General Public License as published by 5 * the Free Software Foundation; either version 2 of the License, or 6 * (at your option) any later version. 7 * 8 * This program is distributed in the hope that it will be useful, 9 * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 * GNU General Public License for more details. 12 * 13 * You should have received a copy of the GNU General Public License 14 * along with this program; if not, write to the Free Software 15 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 16 */ 17 18 #include "config.h" 19 20 #ifdef __linux__ 21 #include <sys/fsuid.h> 22 #endif 23 #include <sys/types.h> 24 #include <sys/socket.h> 25 #include <sys/un.h> 26 #include <sys/stat.h> 27 #include <fcntl.h> 28 #include <unistd.h> 29 #include <errno.h> 30 #include <dlfcn.h> 31 #include <signal.h> 32 #include <iostream> 33 #include <vector> 34 #include <string> 35 #include <map> 36 #include <algorithm> 37 #include <sys/time.h> 38 #include <time.h> 39 #include <stdlib.h> 40 #include <stdio.h> 41 #include <errno.h> 42 #ifdef HAVE_SYS_IO_H 43 #include <sys/io.h> 44 #endif 45 #include <sys/resource.h> 46 #include <sys/mman.h> 47 #ifdef __linux__ 48 #include <malloc.h> 49 #include <sys/prctl.h> 50 #endif 51 #ifdef __FreeBSD__ 52 #include <pthread_np.h> 53 #endif 54 55 #include "config.h" 56 57 #include "rtapi.h" 58 #include "hal.h" 59 #include "hal/hal_priv.h" 60 #include "rtapi_uspace.hh" 61 62 #include <string.h> 63 #include <boost/lockfree/queue.hpp> 64 65 std::atomic<int> WithRoot::level; 66 static uid_t euid, ruid; 67 68 #include "rtapi/uspace_common.h" 69 70 WithRoot::WithRoot() { 71 if(!level++) { 72 #ifdef __linux__ 73 setfsuid(euid); 74 #endif 75 } 76 } 77 78 WithRoot::~WithRoot() { 79 if(!--level) { 80 #ifdef __linux__ 81 setfsuid(ruid); 82 #endif 83 } 84 } 85 86 extern "C" 87 int rtapi_is_realtime(); 88 89 namespace 90 { 91 RtapiApp &App(); 92 93 struct message_t { 94 msg_level_t level; 95 char msg[1024-sizeof(level)]; 96 }; 97 98 boost::lockfree::queue<message_t, boost::lockfree::capacity<128>> 99 rtapi_msg_queue; 100 101 pthread_t queue_thread; 102 void *queue_function(void *arg) { 103 // note: can't use anything in this function that requires App() to exist 104 // but it's OK to use functions that aren't safe for realtime (that's the 105 // point of running this in a thread) 106 while(1) { 107 pthread_testcancel(); 108 pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, nullptr); 109 rtapi_msg_queue.consume_all([](const message_t &m) { 110 fputs(m.msg, m.level == RTAPI_MSG_ALL ? stdout : stderr); 111 }); 112 pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, nullptr); 113 struct timespec ts = {0, 10000000}; 114 rtapi_clock_nanosleep(CLOCK_MONOTONIC, 0, &ts, NULL, NULL); 115 } 116 return nullptr; 117 } 118 } 119 120 static int sim_rtapi_run_threads(int fd, int (*callback)(int fd)); 121 122 using namespace std; 123 124 template<class T> T DLSYM(void *handle, const string &name) { 125 return (T)(dlsym(handle, name.c_str())); 126 } 127 128 template<class T> T DLSYM(void *handle, const char *name) { 129 return (T)(dlsym(handle, name)); 130 } 131 132 static std::map<string, void*> modules; 133 134 static int instance_count = 0; 135 static int force_exit = 0; 136 137 static int do_newinst_cmd(string type, string name, string arg) { 138 void *module = modules["hal_lib"]; 139 if(!module) { 140 rtapi_print_msg(RTAPI_MSG_ERR, 141 "newinst: hal_lib is required, but not loaded\n"); 142 return -1; 143 } 144 145 hal_comp_t *(*find_comp_by_name)(char*) = 146 DLSYM<hal_comp_t*(*)(char *)>(module, "halpr_find_comp_by_name"); 147 if(!find_comp_by_name) { 148 rtapi_print_msg(RTAPI_MSG_ERR, 149 "newinst: halpr_find_comp_by_name not found\n"); 150 return -1; 151 } 152 153 hal_comp_t *comp = find_comp_by_name((char*)type.c_str()); 154 if(!comp) { 155 rtapi_print_msg(RTAPI_MSG_ERR, 156 "newinst: component %s not found\n", type.c_str()); 157 return -1; 158 } 159 160 return comp->make((char*)name.c_str(), (char*)arg.c_str()); 161 } 162 163 static int do_one_item(char item_type_char, const string ¶m_name, const string ¶m_value, void *vitem, int idx=0) { 164 char *endp; 165 switch(item_type_char) { 166 case 'l': { 167 long *litem = *(long**) vitem; 168 litem[idx] = strtol(param_value.c_str(), &endp, 0); 169 if(*endp) { 170 rtapi_print_msg(RTAPI_MSG_ERR, 171 "`%s' invalid for parameter `%s'", 172 param_value.c_str(), param_name.c_str()); 173 return -1; 174 } 175 return 0; 176 } 177 case 'i': { 178 int *iitem = *(int**) vitem; 179 iitem[idx] = strtol(param_value.c_str(), &endp, 0); 180 if(*endp) { 181 rtapi_print_msg(RTAPI_MSG_ERR, 182 "`%s' invalid for parameter `%s'", 183 param_value.c_str(), param_name.c_str()); 184 return -1; 185 } 186 return 0; 187 } 188 case 's': { 189 char **sitem = *(char***) vitem; 190 sitem[idx] = strdup(param_value.c_str()); 191 return 0; 192 } 193 default: 194 rtapi_print_msg(RTAPI_MSG_ERR, 195 "%s: Invalid type character `%c'\n", 196 param_name.c_str(), item_type_char); 197 return -1; 198 } 199 return 0; 200 } 201 202 void remove_quotes(string &s) { 203 s.erase(remove_copy(s.begin(), s.end(), s.begin(), '"'), s.end()); 204 } 205 206 static int do_comp_args(void *module, vector<string> args) { 207 for(unsigned i=1; i < args.size(); i++) { 208 string &s = args[i]; 209 remove_quotes(s); 210 size_t idx = s.find('='); 211 if(idx == string::npos) { 212 rtapi_print_msg(RTAPI_MSG_ERR, "Invalid parameter `%s'\n", 213 s.c_str()); 214 return -1; 215 } 216 string param_name(s, 0, idx); 217 string param_value(s, idx+1); 218 void *item=DLSYM<void*>(module, "rtapi_info_address_" + param_name); 219 if(!item) { 220 rtapi_print_msg(RTAPI_MSG_ERR, 221 "Unknown parameter `%s'\n", s.c_str()); 222 return -1; 223 } 224 char **item_type=DLSYM<char**>(module, "rtapi_info_type_" + param_name); 225 if(!item_type || !*item_type) { 226 rtapi_print_msg(RTAPI_MSG_ERR, 227 "Unknown parameter `%s' (type information missing)\n", 228 s.c_str()); 229 return -1; 230 } 231 232 int*max_size_ptr=DLSYM<int*>(module, "rtapi_info_size_" + param_name); 233 234 char item_type_char = **item_type; 235 if(max_size_ptr) { 236 int max_size = *max_size_ptr; 237 size_t idx = 0; 238 int i = 0; 239 while(idx != string::npos) { 240 if(i == max_size) { 241 rtapi_print_msg(RTAPI_MSG_ERR, 242 "%s: can only take %d arguments\n", 243 s.c_str(), max_size); 244 return -1; 245 } 246 size_t idx1 = param_value.find(",", idx); 247 string substr(param_value, idx, idx1 - idx); 248 int result = do_one_item(item_type_char, s, substr, item, i); 249 if(result != 0) return result; 250 i++; 251 idx = idx1 == string::npos ? idx1 : idx1 + 1; 252 } 253 } else { 254 int result = do_one_item(item_type_char, s, param_value, item); 255 if(result != 0) return result; 256 } 257 } 258 return 0; 259 } 260 261 static int do_load_cmd(string name, vector<string> args) { 262 void *w = modules[name]; 263 if(w == NULL) { 264 char what[LINELEN+1]; 265 snprintf(what, LINELEN, "%s/%s.so", EMC2_RTLIB_DIR, name.c_str()); 266 void *module = modules[name] = dlopen(what, RTLD_GLOBAL | RTLD_NOW); 267 if(!module) { 268 rtapi_print_msg(RTAPI_MSG_ERR, "%s: dlopen: %s\n", name.c_str(), dlerror()); 269 modules.erase(name); 270 return -1; 271 } 272 /// XXX handle arguments 273 int (*start)(void) = DLSYM<int(*)(void)>(module, "rtapi_app_main"); 274 if(!start) { 275 rtapi_print_msg(RTAPI_MSG_ERR, "%s: dlsym: %s\n", name.c_str(), dlerror()); 276 dlclose(module); 277 modules.erase(name); 278 return -1; 279 } 280 int result; 281 282 result = do_comp_args(module, args); 283 if(result < 0) { 284 dlclose(module); 285 modules.erase(name); 286 return -1; 287 } 288 289 if ((result=start()) < 0) { 290 rtapi_print_msg(RTAPI_MSG_ERR, "%s: rtapi_app_main: %s (%d)\n", 291 name.c_str(), strerror(-result), result); 292 dlclose(module); 293 modules.erase(name); 294 return result; 295 } else { 296 instance_count ++; 297 return 0; 298 } 299 } else { 300 rtapi_print_msg(RTAPI_MSG_ERR, "%s: already exists\n", name.c_str()); 301 return -1; 302 } 303 } 304 305 static int do_unload_cmd(string name) { 306 void *w = modules[name]; 307 if(w == NULL) { 308 rtapi_print_msg(RTAPI_MSG_ERR, "%s: not loaded\n", name.c_str()); 309 return -1; 310 } else { 311 int (*stop)(void) = DLSYM<int(*)(void)>(w, "rtapi_app_exit"); 312 if(stop) stop(); 313 modules.erase(modules.find(name)); 314 dlclose(w); 315 instance_count --; 316 } 317 return 0; 318 } 319 320 struct ReadError : std::exception {}; 321 struct WriteError : std::exception {}; 322 323 static int read_number(int fd) { 324 int r = 0, neg=1; 325 char ch; 326 327 while(1) { 328 int res = read(fd, &ch, 1); 329 if(res != 1) return -1; 330 if(ch == '-') neg = -1; 331 else if(ch == ' ') return r * neg; 332 else r = 10 * r + ch - '0'; 333 } 334 } 335 336 static string read_string(int fd) { 337 int len = read_number(fd); 338 char buf[len]; 339 if(read(fd, buf, len) != len) throw ReadError(); 340 return string(buf, len); 341 } 342 343 static vector<string> read_strings(int fd) { 344 vector<string> result; 345 int count = read_number(fd); 346 for(int i=0; i<count; i++) { 347 result.push_back(read_string(fd)); 348 } 349 return result; 350 } 351 352 static void write_number(string &buf, int num) { 353 char numbuf[10]; 354 sprintf(numbuf, "%d ", num); 355 buf = buf + numbuf; 356 } 357 358 static void write_string(string &buf, string s) { 359 write_number(buf, s.size()); 360 buf += s; 361 } 362 363 static void write_strings(int fd, vector<string> strings) { 364 string buf; 365 write_number(buf, strings.size()); 366 for(unsigned int i=0; i<strings.size(); i++) { 367 write_string(buf, strings[i]); 368 } 369 if(write(fd, buf.data(), buf.size()) != (ssize_t)buf.size()) throw WriteError(); 370 } 371 372 static int handle_command(vector<string> args) { 373 if(args.size() == 0) { return 0; } 374 if(args.size() == 1 && args[0] == "exit") { 375 force_exit = 1; 376 return 0; 377 } else if(args.size() >= 2 && args[0] == "load") { 378 string name = args[1]; 379 args.erase(args.begin()); 380 return do_load_cmd(name, args); 381 } else if(args.size() == 2 && args[0] == "unload") { 382 return do_unload_cmd(args[1]); 383 } else if(args.size() == 3 && args[0] == "newinst") { 384 return do_newinst_cmd(args[1], args[2], ""); 385 } else if(args.size() == 4 && args[0] == "newinst") { 386 return do_newinst_cmd(args[1], args[2], args[3]); 387 } else { 388 rtapi_print_msg(RTAPI_MSG_ERR, 389 "Unrecognized command starting with %s\n", 390 args[0].c_str()); 391 return -1; 392 } 393 } 394 395 static int slave(int fd, vector<string> args) { 396 try { 397 write_strings(fd, args); 398 } 399 catch (WriteError &e) { 400 rtapi_print_msg(RTAPI_MSG_ERR, 401 "rtapi_app: failed to write to master: %s\n", strerror(errno)); 402 } 403 404 int result = read_number(fd); 405 return result; 406 } 407 408 static int callback(int fd) 409 { 410 struct sockaddr_un client_addr; 411 memset(&client_addr, 0, sizeof(client_addr)); 412 socklen_t len = sizeof(client_addr); 413 int fd1 = accept(fd, (sockaddr*)&client_addr, &len); 414 if(fd1 < 0) { 415 rtapi_print_msg(RTAPI_MSG_ERR, 416 "rtapi_app: failed to accept connection from slave: %s\n", strerror(errno)); 417 return -1; 418 } else { 419 int result; 420 try { 421 result = handle_command(read_strings(fd1)); 422 } catch (ReadError &e) { 423 rtapi_print_msg(RTAPI_MSG_ERR, 424 "rtapi_app: failed to read from slave: %s\n", strerror(errno)); 425 close(fd1); 426 return -1; 427 } 428 string buf; 429 write_number(buf, result); 430 if(write(fd1, buf.data(), buf.size()) != (ssize_t)buf.size()) { 431 rtapi_print_msg(RTAPI_MSG_ERR, 432 "rtapi_app: failed to write to slave: %s\n", strerror(errno)); 433 }; 434 close(fd1); 435 } 436 return !force_exit && instance_count > 0; 437 } 438 439 static pthread_t main_thread{}; 440 441 static int master(int fd, vector<string> args) { 442 main_thread = pthread_self(); 443 if(pthread_create(&queue_thread, nullptr, &queue_function, nullptr) < 0) { 444 perror("pthread_create (queue function)"); 445 return -1; 446 } 447 do_load_cmd("hal_lib", vector<string>()); instance_count = 0; 448 App(); // force rtapi_app to be created 449 int result=0; 450 if(args.size()) { 451 result = handle_command(args); 452 if(result != 0) goto out; 453 if(force_exit || instance_count == 0) goto out; 454 } 455 sim_rtapi_run_threads(fd, callback); 456 out: 457 pthread_cancel(queue_thread); 458 pthread_join(queue_thread, nullptr); 459 rtapi_msg_queue.consume_all([](const message_t &m) { 460 fputs(m.msg, m.level == RTAPI_MSG_ALL ? stdout : stderr); 461 }); 462 return result; 463 } 464 465 static std::string 466 _get_fifo_path() { 467 std::string s; 468 if(getenv("RTAPI_FIFO_PATH")) 469 s = getenv("RTAPI_FIFO_PATH"); 470 else if(getenv("HOME")) 471 s = std::string(getenv("HOME")) + "/.rtapi_fifo"; 472 else { 473 rtapi_print_msg(RTAPI_MSG_ERR, 474 "rtapi_app: RTAPI_FIFO_PATH and HOME are unset. rtapi fifo creation is unsafe."); 475 return NULL; 476 } 477 if(s.size() + 1 > sizeof(sockaddr_un::sun_path)) { 478 rtapi_print_msg(RTAPI_MSG_ERR, 479 "rtapi_app: rtapi fifo path is too long (arch limit %zd): %s", 480 sizeof(sockaddr_un::sun_path), s.c_str()); 481 return NULL; 482 } 483 return s; 484 } 485 486 static const char * 487 get_fifo_path() { 488 static std::string path = _get_fifo_path(); 489 return path.c_str(); 490 } 491 492 static int 493 get_fifo_path(char *buf, size_t bufsize) { 494 const char *s = get_fifo_path(); 495 if(!s) return -1; 496 strncpy(buf, s, bufsize); 497 return 0; 498 } 499 500 int main(int argc, char **argv) { 501 if(getuid() == 0) { 502 char *fallback_uid_str = getenv("RTAPI_UID"); 503 int fallback_uid = fallback_uid_str ? atoi(fallback_uid_str) : 0; 504 if(fallback_uid == 0) 505 { 506 fprintf(stderr, 507 "Refusing to run as root without fallback UID specified\n" 508 "To run under a debugger with I/O, use e.g.,\n" 509 " sudo env RTAPI_UID=`id -u` RTAPI_FIFO_PATH=$HOME/.rtapi_fifo gdb " EMC2_BIN_DIR "/rtapi_app\n"); 510 exit(1); 511 } 512 setreuid(fallback_uid, 0); 513 fprintf(stderr, 514 "Running with fallback_uid. getuid()=%d geteuid()=%d\n", 515 getuid(), geteuid()); 516 } 517 ruid = getuid(); 518 euid = geteuid(); 519 setresuid(euid, euid, ruid); 520 #ifdef __linux__ 521 setfsuid(ruid); 522 #endif 523 vector<string> args; 524 for(int i=1; i<argc; i++) { args.push_back(string(argv[i])); } 525 526 become_master: 527 int fd = socket(PF_UNIX, SOCK_STREAM, 0); 528 if(fd == -1) { perror("socket"); exit(1); } 529 530 int enable = 1; 531 setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable)); 532 struct sockaddr_un addr; 533 addr.sun_family = AF_UNIX; 534 if(get_fifo_path(addr.sun_path, sizeof(addr.sun_path)) < 0) 535 exit(1); 536 int result = ::bind(fd, (sockaddr*)&addr, sizeof(addr)); 537 538 if(result == 0) { 539 int result = listen(fd, 10); 540 if(result != 0) { perror("listen"); exit(1); } 541 setsid(); // create a new session if we can... 542 result = master(fd, args); 543 unlink(get_fifo_path()); 544 return result; 545 } else if(errno == EADDRINUSE) { 546 struct timeval t0, t1; 547 gettimeofday(&t0, NULL); 548 gettimeofday(&t1, NULL); 549 for(int i=0; i < 3 || (t1.tv_sec < 3 + t0.tv_sec) ; i++) { 550 result = connect(fd, (sockaddr*)&addr, sizeof(addr)); 551 if(result == 0) break; 552 if(i==0) srand48(t0.tv_sec ^ t0.tv_usec); 553 usleep(lrand48() % 100000); 554 gettimeofday(&t1, NULL); 555 } 556 if(result < 0 && errno == ECONNREFUSED) { 557 unlink(get_fifo_path()); 558 fprintf(stderr, "Waited 3 seconds for master. giving up.\n"); 559 close(fd); 560 goto become_master; 561 } 562 if(result < 0) { fprintf(stderr, "connect %s: %s", addr.sun_path, strerror(errno)); exit(1); } 563 return slave(fd, args); 564 } else { 565 perror("bind"); exit(1); 566 } 567 } 568 569 570 /* These structs hold data associated with objects like tasks, etc. */ 571 /* Task handles are pointers to these structs. */ 572 573 struct rtapi_module { 574 int magic; 575 }; 576 577 #define MODULE_MAGIC 30812 578 #define SHMEM_MAGIC 25453 579 580 #define MAX_MODULES 64 581 #define MODULE_OFFSET 32768 582 583 rtapi_task::rtapi_task() 584 : magic{}, id{}, owner{}, stacksize{}, prio{}, 585 period{}, nextstart{}, 586 ratio{}, arg{}, taskcode{} 587 {} 588 589 namespace 590 { 591 struct PosixTask : rtapi_task 592 { 593 PosixTask() : rtapi_task{}, thr{} 594 {} 595 596 pthread_t thr; /* thread's context */ 597 }; 598 599 struct Posix : RtapiApp 600 { 601 Posix(int policy = SCHED_FIFO) : RtapiApp(policy), do_thread_lock(policy != SCHED_FIFO) { 602 pthread_once(&key_once, init_key); 603 if(do_thread_lock) 604 pthread_mutex_init(&thread_lock, 0); 605 } 606 int task_delete(int id); 607 int task_start(int task_id, unsigned long period_nsec); 608 int task_pause(int task_id); 609 int task_resume(int task_id); 610 int task_self(); 611 long long task_pll_get_reference(void); 612 int task_pll_set_correction(long value); 613 void wait(); 614 struct rtapi_task *do_task_new() { 615 return new PosixTask; 616 } 617 unsigned char do_inb(unsigned int port); 618 void do_outb(unsigned char value, unsigned int port); 619 int run_threads(int fd, int (*callback)(int fd)); 620 static void *wrapper(void *arg); 621 bool do_thread_lock; 622 pthread_mutex_t thread_lock; 623 624 static pthread_once_t key_once; 625 static pthread_key_t key; 626 static void init_key(void) { 627 pthread_key_create(&key, NULL); 628 } 629 630 long long do_get_time(void) { 631 struct timespec ts; 632 clock_gettime(CLOCK_MONOTONIC, &ts); 633 return ts.tv_sec * 1000000000LL + ts.tv_nsec; 634 } 635 636 void do_delay(long ns); 637 }; 638 639 static void signal_handler(int sig, siginfo_t *si, void *uctx) 640 { 641 switch (sig) { 642 case SIGXCPU: 643 // should not happen - must be handled in RTAPI if enabled 644 rtapi_print_msg(RTAPI_MSG_ERR, 645 "rtapi_app: BUG: SIGXCPU received - exiting\n"); 646 exit(0); 647 break; 648 649 case SIGTERM: 650 rtapi_print_msg(RTAPI_MSG_ERR, 651 "rtapi_app: SIGTERM - shutting down\n"); 652 exit(0); 653 break; 654 655 default: // pretty bad 656 rtapi_print_msg(RTAPI_MSG_ERR, 657 "rtapi_app: caught signal %d - dumping core\n", sig); 658 sleep(1); // let syslog drain 659 signal(sig, SIG_DFL); 660 raise(sig); 661 break; 662 } 663 exit(1); 664 } 665 666 const size_t PRE_ALLOC_SIZE = 1024*1024*32; 667 const static struct rlimit unlimited = {RLIM_INFINITY, RLIM_INFINITY}; 668 static void configure_memory() 669 { 670 int res = setrlimit(RLIMIT_MEMLOCK, &unlimited); 671 if(res < 0) perror("setrlimit"); 672 673 res = mlockall(MCL_CURRENT | MCL_FUTURE); 674 if(res < 0) perror("mlockall"); 675 676 #ifdef __linux__ 677 /* Turn off malloc trimming.*/ 678 if (!mallopt(M_TRIM_THRESHOLD, -1)) { 679 rtapi_print_msg(RTAPI_MSG_WARN, 680 "mallopt(M_TRIM_THRESHOLD, -1) failed\n"); 681 } 682 /* Turn off mmap usage. */ 683 if (!mallopt(M_MMAP_MAX, 0)) { 684 rtapi_print_msg(RTAPI_MSG_WARN, 685 "mallopt(M_MMAP_MAX, -1) failed\n"); 686 } 687 #endif 688 char *buf = static_cast<char *>(malloc(PRE_ALLOC_SIZE)); 689 if (buf == NULL) { 690 rtapi_print_msg(RTAPI_MSG_WARN, "malloc(PRE_ALLOC_SIZE) failed\n"); 691 return; 692 } 693 long pagesize = sysconf(_SC_PAGESIZE); 694 /* Touch each page in this piece of memory to get it mapped into RAM */ 695 for (size_t i = 0; i < PRE_ALLOC_SIZE; i += pagesize) { 696 /* Each write to this buffer will generate a pagefault. 697 * Once the pagefault is handled a page will be locked in 698 * memory and never given back to the system. */ 699 buf[i] = 0; 700 } 701 free(buf); 702 } 703 704 static int harden_rt() 705 { 706 if(!rtapi_is_realtime()) return -EINVAL; 707 708 WITH_ROOT; 709 #if defined(__linux__) && (defined(__x86_64__) || defined(__i386__)) 710 if (iopl(3) < 0) { 711 rtapi_print_msg(RTAPI_MSG_ERR, 712 "cannot gain I/O privileges - " 713 "forgot 'sudo make setuid'?\n"); 714 return -EPERM; 715 } 716 #endif 717 718 struct sigaction sig_act = {}; 719 #ifdef __linux__ 720 // enable realtime 721 if (setrlimit(RLIMIT_RTPRIO, &unlimited) < 0) 722 { 723 rtapi_print_msg(RTAPI_MSG_WARN, 724 "setrlimit(RTLIMIT_RTPRIO): %s\n", 725 strerror(errno)); 726 return -errno; 727 } 728 729 // enable core dumps 730 if (setrlimit(RLIMIT_CORE, &unlimited) < 0) 731 rtapi_print_msg(RTAPI_MSG_WARN, 732 "setrlimit: %s - core dumps may be truncated or non-existant\n", 733 strerror(errno)); 734 735 // even when setuid root 736 if (prctl(PR_SET_DUMPABLE, 1) < 0) 737 rtapi_print_msg(RTAPI_MSG_WARN, 738 "prctl(PR_SET_DUMPABLE) failed: no core dumps will be created - %d - %s\n", 739 errno, strerror(errno)); 740 #endif /* __linux__ */ 741 742 configure_memory(); 743 744 sigemptyset( &sig_act.sa_mask ); 745 sig_act.sa_handler = SIG_IGN; 746 sig_act.sa_sigaction = NULL; 747 748 // prevent stopping of RT threads by ^Z 749 sigaction(SIGTSTP, &sig_act, (struct sigaction *) NULL); 750 751 sig_act.sa_sigaction = signal_handler; 752 sig_act.sa_flags = SA_SIGINFO; 753 754 sigaction(SIGSEGV, &sig_act, (struct sigaction *) NULL); 755 sigaction(SIGILL, &sig_act, (struct sigaction *) NULL); 756 sigaction(SIGFPE, &sig_act, (struct sigaction *) NULL); 757 sigaction(SIGTERM, &sig_act, (struct sigaction *) NULL); 758 sigaction(SIGINT, &sig_act, (struct sigaction *) NULL); 759 760 #ifdef __linux__ 761 int fd = open("/dev/cpu_dma_latency", O_WRONLY | O_CLOEXEC); 762 if (fd < 0) { 763 rtapi_print_msg(RTAPI_MSG_WARN, "failed to open /dev/cpu_dma_latency: %s\n", strerror(errno)); 764 } else { 765 int r; 766 r = write(fd, "\0\0\0\0", 4); 767 if (r != 4) { 768 rtapi_print_msg(RTAPI_MSG_WARN, "failed to write to /dev/cpu_dma_latency: %s\n", strerror(errno)); 769 } 770 // deliberately leak fd until program exit 771 } 772 #endif /* __linux__ */ 773 return 0; 774 } 775 776 777 static RtapiApp *makeApp() 778 { 779 if(euid != 0 || harden_rt() < 0) 780 { 781 rtapi_print_msg(RTAPI_MSG_ERR, "Note: Using POSIX non-realtime\n"); 782 return new Posix(SCHED_OTHER); 783 } 784 WithRoot r; 785 void *dll = nullptr; 786 if(detect_xenomai()) { 787 dll = dlopen(EMC2_HOME "/lib/libuspace-xenomai.so.0", RTLD_NOW); 788 if(!dll) fprintf(stderr, "dlopen: %s\n", dlerror()); 789 } else if(detect_rtai()) { 790 dll = dlopen(EMC2_HOME "/lib/libuspace-rtai.so.0", RTLD_NOW); 791 if(!dll) fprintf(stderr, "dlopen: %s\n", dlerror()); 792 } 793 if(dll) 794 { 795 auto fn = reinterpret_cast<RtapiApp*(*)()>(dlsym(dll, "make")); 796 if(!fn) fprintf(stderr, "dlopen: %s\n", dlerror()); 797 auto result = fn ? fn() : nullptr; 798 if(result) { 799 return result; 800 } 801 } 802 rtapi_print_msg(RTAPI_MSG_ERR, "Note: Using POSIX realtime\n"); 803 return new Posix(SCHED_FIFO); 804 } 805 RtapiApp &App() 806 { 807 static RtapiApp *app = makeApp(); 808 return *app; 809 } 810 811 } 812 /* data for all tasks */ 813 struct rtapi_task *task_array[MAX_TASKS]; 814 815 /* Priority functions. Uspace uses POSIX task priorities. */ 816 817 int RtapiApp::prio_highest() 818 { 819 return sched_get_priority_max(policy); 820 } 821 822 int RtapiApp::prio_lowest() 823 { 824 return sched_get_priority_min(policy); 825 } 826 827 int RtapiApp::prio_next_higher(int prio) 828 { 829 /* return a valid priority for out of range arg */ 830 if (prio >= rtapi_prio_highest()) 831 return rtapi_prio_highest(); 832 if (prio < rtapi_prio_lowest()) 833 return rtapi_prio_lowest(); 834 835 /* return next higher priority for in-range arg */ 836 return prio + 1; 837 } 838 839 int RtapiApp::prio_next_lower(int prio) 840 { 841 /* return a valid priority for out of range arg */ 842 if (prio <= rtapi_prio_lowest()) 843 return rtapi_prio_lowest(); 844 if (prio > rtapi_prio_highest()) 845 return rtapi_prio_highest(); 846 /* return next lower priority for in-range arg */ 847 return prio - 1; 848 } 849 850 int RtapiApp::allocate_task_id() 851 { 852 for(int n=0; n<MAX_TASKS; n++) 853 { 854 rtapi_task **taskptr = &(task_array[n]); 855 if(__sync_bool_compare_and_swap(taskptr, (rtapi_task*)0, TASK_MAGIC_INIT)) 856 return n; 857 } 858 return -ENOSPC; 859 } 860 861 int RtapiApp::task_new(void (*taskcode) (void*), void *arg, 862 int prio, int owner, unsigned long int stacksize, int uses_fp) { 863 /* check requested priority */ 864 if ((prio > rtapi_prio_highest()) || (prio < rtapi_prio_lowest())) 865 { 866 return -EINVAL; 867 } 868 869 /* label as a valid task structure */ 870 int n = allocate_task_id(); 871 if(n < 0) return n; 872 873 struct rtapi_task *task = do_task_new(); 874 if(stacksize < (1024*1024)) stacksize = (1024*1024); 875 memset(task, 0, sizeof(*task)); 876 task->id = n; 877 task->owner = owner; 878 task->uses_fp = uses_fp; 879 task->arg = arg; 880 task->stacksize = stacksize; 881 task->taskcode = taskcode; 882 task->prio = prio; 883 task->magic = TASK_MAGIC; 884 task_array[n] = task; 885 886 /* and return handle to the caller */ 887 888 return n; 889 } 890 891 rtapi_task *RtapiApp::get_task(int task_id) { 892 if(task_id < 0 || task_id >= MAX_TASKS) return NULL; 893 /* validate task handle */ 894 rtapi_task *task = task_array[task_id]; 895 if(!task || task == TASK_MAGIC_INIT || task->magic != TASK_MAGIC) 896 return NULL; 897 898 return task; 899 } 900 901 void RtapiApp::unexpected_realtime_delay(rtapi_task *task, int nperiod) { 902 static int printed = 0; 903 if(!printed) 904 { 905 rtapi_print_msg(RTAPI_MSG_ERR, 906 "Unexpected realtime delay on task %d with period %ld\n" 907 "This Message will only display once per session.\n" 908 "Run the Latency Test and resolve before continuing.\n", 909 task->id, task->period); 910 printed = 1; 911 } 912 } 913 914 int Posix::task_delete(int id) 915 { 916 auto task = ::rtapi_get_task<PosixTask>(id); 917 if(!task) return -EINVAL; 918 919 pthread_cancel(task->thr); 920 pthread_join(task->thr, 0); 921 task->magic = 0; 922 task_array[id] = 0; 923 delete task; 924 return 0; 925 } 926 927 static int find_rt_cpu_number() { 928 if(getenv("RTAPI_CPU_NUMBER")) return atoi(getenv("RTAPI_CPU_NUMBER")); 929 930 #ifdef __linux__ 931 cpu_set_t cpuset_orig; 932 int r = sched_getaffinity(getpid(), sizeof(cpuset_orig), &cpuset_orig); 933 if(r < 0) 934 // if getaffinity fails, (it shouldn't be able to), just use CPU#0 935 return 0; 936 937 cpu_set_t cpuset; 938 CPU_ZERO(&cpuset); 939 long top_probe = sysconf(_SC_NPROCESSORS_CONF); 940 // in old glibc versions, it was an error to pass to sched_setaffinity bits 941 // that are higher than an imagined/probed kernel-side CPU mask size. 942 // this caused the message 943 // sched_setaffinity: Invalid argument 944 // to be printed at startup, and the probed CPU would not take into 945 // account CPUs masked from this process by default (whether by 946 // isolcpus or taskset). By only setting bits up to the "number of 947 // processes configured", the call is successful on glibc versions such as 948 // 2.19 and older. 949 for(long i=0; i<top_probe && i<CPU_SETSIZE; i++) CPU_SET(i, &cpuset); 950 951 r = sched_setaffinity(getpid(), sizeof(cpuset), &cpuset); 952 if(r < 0) 953 // if setaffinity fails, (it shouldn't be able to), go on with 954 // whatever the default CPUs were. 955 perror("sched_setaffinity"); 956 957 r = sched_getaffinity(getpid(), sizeof(cpuset), &cpuset); 958 if(r < 0) { 959 // if getaffinity fails, (it shouldn't be able to), copy the 960 // original affinity list in and use it 961 perror("sched_getaffinity"); 962 CPU_AND(&cpuset, &cpuset_orig, &cpuset); 963 } 964 965 int top = -1; 966 for(int i=0; i<CPU_SETSIZE; i++) { 967 if(CPU_ISSET(i, &cpuset)) top = i; 968 } 969 return top; 970 #else 971 return (-1); 972 #endif 973 } 974 975 int Posix::task_start(int task_id, unsigned long int period_nsec) 976 { 977 auto task = ::rtapi_get_task<PosixTask>(task_id); 978 if(!task) return -EINVAL; 979 980 if(period_nsec < (unsigned long)period) period_nsec = (unsigned long)period; 981 task->period = period_nsec; 982 task->ratio = period_nsec / period; 983 984 struct sched_param param; 985 memset(¶m, 0, sizeof(param)); 986 param.sched_priority = task->prio; 987 988 // limit PLL correction values to +/-1% of cycle time 989 task->pll_correction_limit = period_nsec / 100; 990 task->pll_correction = 0; 991 992 int nprocs = sysconf( _SC_NPROCESSORS_ONLN ); 993 994 pthread_attr_t attr; 995 if(pthread_attr_init(&attr) < 0) 996 return -errno; 997 if(pthread_attr_setstacksize(&attr, task->stacksize) < 0) 998 return -errno; 999 if(pthread_attr_setschedpolicy(&attr, policy) < 0) 1000 return -errno; 1001 if(pthread_attr_setschedparam(&attr, ¶m) < 0) 1002 return -errno; 1003 if(pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) < 0) 1004 return -errno; 1005 if(nprocs > 1) { 1006 const static int rt_cpu_number = find_rt_cpu_number(); 1007 if(rt_cpu_number != -1) { 1008 #ifdef __FreeBSD__ 1009 cpuset_t cpuset; 1010 #else 1011 cpu_set_t cpuset; 1012 #endif 1013 CPU_ZERO(&cpuset); 1014 CPU_SET(rt_cpu_number, &cpuset); 1015 if(pthread_attr_setaffinity_np(&attr, sizeof(cpuset), &cpuset) < 0) 1016 return -errno; 1017 } 1018 } 1019 if(pthread_create(&task->thr, &attr, &wrapper, reinterpret_cast<void*>(task)) < 0) 1020 return -errno; 1021 1022 return 0; 1023 } 1024 1025 #define RTAPI_CLOCK (CLOCK_MONOTONIC) 1026 1027 pthread_once_t Posix::key_once = PTHREAD_ONCE_INIT; 1028 pthread_key_t Posix::key; 1029 1030 void *Posix::wrapper(void *arg) 1031 { 1032 struct rtapi_task *task; 1033 1034 /* use the argument to point to the task data */ 1035 task = (struct rtapi_task*)arg; 1036 long int period = App().period; 1037 if(task->period < period) task->period = period; 1038 task->ratio = task->period / period; 1039 task->period = task->ratio * period; 1040 rtapi_print_msg(RTAPI_MSG_INFO, "task %p period = %lu ratio=%u\n", 1041 task, task->period, task->ratio); 1042 1043 pthread_setspecific(key, arg); 1044 1045 Posix &papp = reinterpret_cast<Posix&>(App()); 1046 if(papp.do_thread_lock) 1047 pthread_mutex_lock(&papp.thread_lock); 1048 1049 struct timespec now; 1050 clock_gettime(RTAPI_CLOCK, &now); 1051 rtapi_timespec_advance(task->nextstart, now, task->period + task->pll_correction); 1052 1053 /* call the task function with the task argument */ 1054 (task->taskcode) (task->arg); 1055 1056 rtapi_print("ERROR: reached end of wrapper for task %d\n", task->id); 1057 return NULL; 1058 } 1059 1060 long long Posix::task_pll_get_reference(void) { 1061 struct rtapi_task *task = reinterpret_cast<rtapi_task*>(pthread_getspecific(key)); 1062 if(!task) return 0; 1063 return task->nextstart.tv_sec * 1000000000LL + task->nextstart.tv_nsec; 1064 } 1065 1066 int Posix::task_pll_set_correction(long value) { 1067 struct rtapi_task *task = reinterpret_cast<rtapi_task*>(pthread_getspecific(key)); 1068 if(!task) return -EINVAL; 1069 if (value > task->pll_correction_limit) value = task->pll_correction_limit; 1070 if (value < -(task->pll_correction_limit)) value = -(task->pll_correction_limit); 1071 task->pll_correction = value; 1072 return 0; 1073 } 1074 1075 int Posix::task_pause(int) { 1076 return -ENOSYS; 1077 } 1078 1079 int Posix::task_resume(int) { 1080 return -ENOSYS; 1081 } 1082 1083 int Posix::task_self() { 1084 struct rtapi_task *task = reinterpret_cast<rtapi_task*>(pthread_getspecific(key)); 1085 if(!task) return -EINVAL; 1086 return task->id; 1087 } 1088 1089 void Posix::wait() { 1090 if(do_thread_lock) 1091 pthread_mutex_unlock(&thread_lock); 1092 pthread_testcancel(); 1093 struct rtapi_task *task = reinterpret_cast<rtapi_task*>(pthread_getspecific(key)); 1094 rtapi_timespec_advance(task->nextstart, task->nextstart, task->period + task->pll_correction); 1095 struct timespec now; 1096 clock_gettime(RTAPI_CLOCK, &now); 1097 if(rtapi_timespec_less(task->nextstart, now)) 1098 { 1099 if(policy == SCHED_FIFO) 1100 unexpected_realtime_delay(task); 1101 } 1102 else 1103 { 1104 int res = rtapi_clock_nanosleep(RTAPI_CLOCK, TIMER_ABSTIME, &task->nextstart, nullptr, &now); 1105 if(res < 0) perror("clock_nanosleep"); 1106 } 1107 if(do_thread_lock) 1108 pthread_mutex_lock(&thread_lock); 1109 } 1110 1111 unsigned char Posix::do_inb(unsigned int port) 1112 { 1113 #ifdef HAVE_SYS_IO_H 1114 return inb(port); 1115 #else 1116 return 0; 1117 #endif 1118 } 1119 1120 void Posix::do_outb(unsigned char val, unsigned int port) 1121 { 1122 #ifdef HAVE_SYS_IO_H 1123 return outb(val, port); 1124 #endif 1125 } 1126 1127 void Posix::do_delay(long ns) { 1128 struct timespec ts = {0, ns}; 1129 rtapi_clock_nanosleep(CLOCK_MONOTONIC, 0, &ts, NULL, NULL); 1130 } 1131 int rtapi_prio_highest(void) 1132 { 1133 return App().prio_highest(); 1134 } 1135 1136 int rtapi_prio_lowest(void) 1137 { 1138 return App().prio_lowest(); 1139 } 1140 1141 int rtapi_prio_next_higher(int prio) 1142 { 1143 return App().prio_next_higher(prio); 1144 } 1145 1146 int rtapi_prio_next_lower(int prio) 1147 { 1148 return App().prio_next_lower(prio); 1149 } 1150 1151 long rtapi_clock_set_period(long nsecs) 1152 { 1153 return App().clock_set_period(nsecs); 1154 } 1155 1156 long RtapiApp::clock_set_period(long nsecs) 1157 { 1158 if(nsecs == 0) return period; 1159 if(period != 0) { 1160 rtapi_print_msg(RTAPI_MSG_ERR, "attempt to set period twice\n"); 1161 return -EINVAL; 1162 } 1163 period = nsecs; 1164 return period; 1165 } 1166 1167 1168 int rtapi_task_new(void (*taskcode) (void*), void *arg, 1169 int prio, int owner, unsigned long int stacksize, int uses_fp) { 1170 return App().task_new(taskcode, arg, prio, owner, stacksize, uses_fp); 1171 } 1172 1173 int rtapi_task_delete(int id) { 1174 return App().task_delete(id); 1175 } 1176 1177 int rtapi_task_start(int task_id, unsigned long period_nsec) 1178 { 1179 return App().task_start(task_id, period_nsec); 1180 } 1181 1182 int rtapi_task_pause(int task_id) 1183 { 1184 return App().task_pause(task_id); 1185 } 1186 1187 int rtapi_task_resume(int task_id) 1188 { 1189 return App().task_resume(task_id); 1190 } 1191 1192 int rtapi_task_self() 1193 { 1194 return App().task_self(); 1195 } 1196 1197 long long rtapi_task_pll_get_reference(void) 1198 { 1199 return App().task_pll_get_reference(); 1200 } 1201 1202 int rtapi_task_pll_set_correction(long value) 1203 { 1204 return App().task_pll_set_correction(value); 1205 } 1206 1207 void rtapi_wait(void) 1208 { 1209 App().wait(); 1210 } 1211 1212 void rtapi_outb(unsigned char byte, unsigned int port) 1213 { 1214 App().do_outb(byte, port); 1215 } 1216 1217 unsigned char rtapi_inb(unsigned int port) 1218 { 1219 return App().do_inb(port); 1220 } 1221 1222 long int simple_strtol(const char *nptr, char **endptr, int base) { 1223 return strtol(nptr, endptr, base); 1224 } 1225 1226 int Posix::run_threads(int fd, int(*callback)(int fd)) { 1227 while(callback(fd)) { /* nothing */ } 1228 return 0; 1229 } 1230 1231 int sim_rtapi_run_threads(int fd, int (*callback)(int fd)) { 1232 return App().run_threads(fd, callback); 1233 } 1234 1235 long long rtapi_get_time() { 1236 return App().do_get_time(); 1237 } 1238 1239 void default_rtapi_msg_handler(msg_level_t level, const char *fmt, va_list ap) { 1240 if(main_thread && pthread_self() != main_thread) { 1241 message_t m; 1242 m.level = level; 1243 vsnprintf(m.msg, sizeof(m.msg), fmt, ap); 1244 rtapi_msg_queue.push(m); 1245 } else { 1246 vfprintf(level == RTAPI_MSG_ALL ? stdout : stderr, fmt, ap); 1247 } 1248 } 1249 1250 long int rtapi_delay_max() { return 10000; } 1251 1252 void rtapi_delay(long ns) { 1253 if(ns > rtapi_delay_max()) ns = rtapi_delay_max(); 1254 App().do_delay(ns); 1255 } 1256 1257 const unsigned long ONE_SEC_IN_NS = 1000000000; 1258 void rtapi_timespec_advance(struct timespec &result, const struct timespec &src, unsigned long nsec) 1259 { 1260 time_t sec = src.tv_sec; 1261 while(nsec >= ONE_SEC_IN_NS) 1262 { 1263 ++sec; 1264 nsec -= ONE_SEC_IN_NS; 1265 } 1266 nsec += src.tv_nsec; 1267 if(nsec >= ONE_SEC_IN_NS) 1268 { 1269 ++sec; 1270 nsec -= ONE_SEC_IN_NS; 1271 } 1272 result.tv_sec = sec; 1273 result.tv_nsec = nsec; 1274 } 1275 1276 int rtapi_open_as_root(const char *filename, int mode) { 1277 WITH_ROOT; 1278 int r = open(filename, mode); 1279 if(r < 0) return -errno; 1280 return r; 1281 } 1282 1283 int rtapi_spawn_as_root(pid_t *pid, const char *path, 1284 const posix_spawn_file_actions_t *file_actions, 1285 const posix_spawnattr_t *attrp, 1286 char *const argv[], char *const envp[]) 1287 { 1288 return posix_spawn(pid, path, file_actions, attrp, argv, envp); 1289 } 1290 1291 int rtapi_spawnp_as_root(pid_t *pid, const char *path, 1292 const posix_spawn_file_actions_t *file_actions, 1293 const posix_spawnattr_t *attrp, 1294 char *const argv[], char *const envp[]) 1295 { 1296 return posix_spawnp(pid, path, file_actions, attrp, argv, envp); 1297 }