hal_lib.c
1 /** HAL stands for Hardware Abstraction Layer, and is used by EMC to 2 transfer realtime data to and from I/O devices and other low-level 3 modules. 4 */ 5 /******************************************************************** 6 * Description: hal_libc.c 7 * This file, 'hal_lib.c', implements the HAL API, for 8 * both user space and realtime modules. It uses the 9 * RTAPI and ULAPI #define symbols to determine which 10 * version to compile. 11 * 12 * Author: John Kasunich 13 * License: LGPL Version 2 14 * 15 * Copyright (c) 2003 All rights reserved. 16 * 17 * Last change: 18 ********************************************************************/ 19 20 /** Copyright (C) 2003 John Kasunich 21 <jmkasunich AT users DOT sourceforge DOT net> 22 23 Other contributors: 24 Paul Fox 25 <pgf AT foxharp DOT boston DOT ma DOT us> 26 Alex Joni 27 <alex_joni AT users DOT sourceforge DOT net> 28 */ 29 30 /** This library is free software; you can redistribute it and/or 31 modify it under the terms of version 2.1 of the GNU Lesser General 32 Public License as published by the Free Software Foundation. 33 This library is distributed in the hope that it will be useful, 34 but WITHOUT ANY WARRANTY; without even the implied warranty of 35 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 36 GNU Lesser General Public License for more details. 37 38 You should have received a copy of the GNU Lesser General Public 39 License along with this library; if not, write to the Free Software 40 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 41 42 THE AUTHORS OF THIS LIBRARY ACCEPT ABSOLUTELY NO LIABILITY FOR 43 ANY HARM OR LOSS RESULTING FROM ITS USE. IT IS _EXTREMELY_ UNWISE 44 TO RELY ON SOFTWARE ALONE FOR SAFETY. Any machinery capable of 45 harming persons must have provisions for completely removing power 46 from all motors, etc, before persons enter any danger area. All 47 machinery must be designed to comply with local and national safety 48 codes, and the authors of this software can not, and do not, take 49 any responsibility for such compliance. 50 51 This code was written as part of the EMC HAL project. For more 52 information, go to www.linuxcnc.org. 53 54 */ 55 56 #include "rtapi.h" /* RTAPI realtime OS API */ 57 #include "hal.h" /* HAL public API decls */ 58 #include "hal_priv.h" /* HAL private decls */ 59 60 #include "rtapi_string.h" 61 #include "rtapi_atomic.h" 62 63 #ifdef RTAPI 64 #include "rtapi_app.h" 65 /* module information */ 66 MODULE_AUTHOR("John Kasunich"); 67 MODULE_DESCRIPTION("Hardware Abstraction Layer for EMC"); 68 MODULE_LICENSE("GPL"); 69 #endif /* RTAPI */ 70 71 #if defined(ULAPI) 72 #include <sys/types.h> /* pid_t */ 73 #include <unistd.h> /* getpid() */ 74 #include <time.h> 75 #endif 76 77 char *hal_shmem_base = 0; 78 hal_data_t *hal_data = 0; 79 static int lib_module_id = -1; /* RTAPI module ID for library module */ 80 static int lib_mem_id = 0; /* RTAPI shmem ID for library module */ 81 82 /*********************************************************************** 83 * LOCAL FUNCTION DECLARATIONS * 84 ************************************************************************/ 85 86 /* These functions are used internally by this file. The code is at 87 the end of the file. */ 88 89 /** init_hal_data() initializes the entire HAL data structure, only 90 if the structure has not already been initialized. (The init 91 is done by the first HAL component to be loaded. 92 */ 93 static int init_hal_data(void); 94 95 /** The 'shmalloc_xx()' functions allocate blocks of shared memory. 96 Each function allocates a block that is 'size' bytes long. 97 If 'size' is 3 or more, the block is aligned on a 4 byte 98 boundary. If 'size' is 2, it is aligned on a 2 byte boundary, 99 and if 'size' is 1, it is unaligned. 100 These functions do not test a mutex - they are called from 101 within the hal library by code that already has the mutex. 102 (The public function 'hal_malloc()' is a wrapper that gets the 103 mutex and then calls 'shmalloc_up()'.) 104 The only difference between the two functions is the location 105 of the allocated memory. 'shmalloc_up()' allocates from the 106 base of shared memory and works upward, while 'shmalloc_dn()' 107 starts at the top and works down. 108 This is done to improve realtime performance. 'shmalloc_up()' 109 is used to allocate data that will be accessed by realtime 110 code, while 'shmalloc_dn()' is used to allocate the much 111 larger structures that are accessed only occaisionally during 112 init. This groups all the realtime data together, inproving 113 cache performance. 114 */ 115 static void *shmalloc_up(long int size); 116 static void *shmalloc_dn(long int size); 117 118 /** The alloc_xxx_struct() functions allocate a structure of the 119 appropriate type and return a pointer to it, or 0 if they fail. 120 They attempt to re-use freed structs first, if none are 121 available, then they call hal_malloc() to create a new one. 122 The free_xxx_struct() functions add the structure at 'p' to 123 the appropriate free list, for potential re-use later. 124 All of these functions assume that the caller has already 125 grabbed the hal_data mutex. 126 */ 127 hal_comp_t *halpr_alloc_comp_struct(void); 128 static hal_pin_t *alloc_pin_struct(void); 129 static hal_sig_t *alloc_sig_struct(void); 130 static hal_param_t *alloc_param_struct(void); 131 static hal_oldname_t *halpr_alloc_oldname_struct(void); 132 #ifdef RTAPI 133 static hal_funct_t *alloc_funct_struct(void); 134 #endif /* RTAPI */ 135 static hal_funct_entry_t *alloc_funct_entry_struct(void); 136 #ifdef RTAPI 137 static hal_thread_t *alloc_thread_struct(void); 138 #endif /* RTAPI */ 139 140 static void free_comp_struct(hal_comp_t * comp); 141 static void unlink_pin(hal_pin_t * pin); 142 static void free_pin_struct(hal_pin_t * pin); 143 static void free_sig_struct(hal_sig_t * sig); 144 static void free_param_struct(hal_param_t * param); 145 static void free_oldname_struct(hal_oldname_t * oldname); 146 #ifdef RTAPI 147 static void free_funct_struct(hal_funct_t * funct); 148 #endif /* RTAPI */ 149 static void free_funct_entry_struct(hal_funct_entry_t * funct_entry); 150 #ifdef RTAPI 151 static void free_thread_struct(hal_thread_t * thread); 152 #endif /* RTAPI */ 153 154 #ifdef RTAPI 155 /** 'thread_task()' is a function that is invoked as a realtime task. 156 It implements a thread, by running down the thread's function list 157 and calling each function in turn. 158 */ 159 static void thread_task(void *arg); 160 #endif /* RTAPI */ 161 162 /*********************************************************************** 163 * PUBLIC (API) FUNCTION CODE * 164 ************************************************************************/ 165 166 static int ref_cnt = 0; 167 168 int hal_init(const char *name) 169 { 170 int comp_id; 171 #ifdef ULAPI 172 int retval; 173 void *mem; 174 #endif 175 char rtapi_name[RTAPI_NAME_LEN + 1]; 176 char hal_name[HAL_NAME_LEN + 1]; 177 hal_comp_t *comp; 178 179 if (name == 0) { 180 rtapi_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: no component name\n"); 181 return -EINVAL; 182 } 183 if (strlen(name) > HAL_NAME_LEN) { 184 rtapi_print_msg(RTAPI_MSG_ERR, 185 "HAL: ERROR: component name '%s' is too long\n", name); 186 return -EINVAL; 187 } 188 189 #ifdef ULAPI 190 if(!lib_mem_id) { 191 rtapi_print_msg(RTAPI_MSG_DBG, "HAL: initializing hal_lib\n"); 192 rtapi_snprintf(rtapi_name, RTAPI_NAME_LEN, "HAL_LIB_%d", (int)getpid()); 193 lib_module_id = rtapi_init(rtapi_name); 194 if (lib_module_id < 0) { 195 rtapi_print_msg(RTAPI_MSG_ERR, 196 "HAL: ERROR: could not initialize RTAPI\n"); 197 return -EINVAL; 198 } 199 200 /* get HAL shared memory block from RTAPI */ 201 lib_mem_id = rtapi_shmem_new(HAL_KEY, lib_module_id, HAL_SIZE); 202 if (lib_mem_id < 0) { 203 rtapi_print_msg(RTAPI_MSG_ERR, 204 "HAL: ERROR: could not open shared memory\n"); 205 rtapi_exit(lib_module_id); 206 return -EINVAL; 207 } 208 /* get address of shared memory area */ 209 retval = rtapi_shmem_getptr(lib_mem_id, &mem); 210 if (retval < 0) { 211 rtapi_print_msg(RTAPI_MSG_ERR, 212 "HAL: ERROR: could not access shared memory\n"); 213 rtapi_exit(lib_module_id); 214 return -EINVAL; 215 } 216 /* set up internal pointers to shared mem and data structure */ 217 hal_shmem_base = (char *) mem; 218 hal_data = (hal_data_t *) mem; 219 /* perform a global init if needed */ 220 retval = init_hal_data(); 221 if ( retval ) { 222 rtapi_print_msg(RTAPI_MSG_ERR, 223 "HAL: ERROR: could not init shared memory\n"); 224 rtapi_exit(lib_module_id); 225 return -EINVAL; 226 } 227 } 228 #endif 229 rtapi_print_msg(RTAPI_MSG_DBG, "HAL: initializing component '%s'\n", 230 name); 231 /* copy name to local vars, truncating if needed */ 232 rtapi_snprintf(rtapi_name, RTAPI_NAME_LEN, "HAL_%s", name); 233 rtapi_snprintf(hal_name, sizeof(hal_name), "%s", name); 234 /* do RTAPI init */ 235 comp_id = rtapi_init(rtapi_name); 236 if (comp_id < 0) { 237 rtapi_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: rtapi init failed\n"); 238 return -EINVAL; 239 } 240 /* get mutex before manipulating the shared data */ 241 rtapi_mutex_get(&(hal_data->mutex)); 242 /* make sure name is unique in the system */ 243 if (halpr_find_comp_by_name(hal_name) != 0) { 244 /* a component with this name already exists */ 245 rtapi_mutex_give(&(hal_data->mutex)); 246 rtapi_print_msg(RTAPI_MSG_ERR, 247 "HAL: ERROR: duplicate component name '%s'\n", hal_name); 248 rtapi_exit(comp_id); 249 return -EINVAL; 250 } 251 /* allocate a new component structure */ 252 comp = halpr_alloc_comp_struct(); 253 if (comp == 0) { 254 /* couldn't allocate structure */ 255 rtapi_mutex_give(&(hal_data->mutex)); 256 rtapi_print_msg(RTAPI_MSG_ERR, 257 "HAL: ERROR: insufficient memory for component '%s'\n", hal_name); 258 rtapi_exit(comp_id); 259 return -ENOMEM; 260 } 261 /* initialize the structure */ 262 comp->comp_id = comp_id; 263 #ifdef RTAPI 264 comp->type = 1; 265 comp->pid = 0; 266 #else /* ULAPI */ 267 comp->type = 0; 268 comp->pid = getpid(); 269 #endif 270 comp->ready = 0; 271 comp->shmem_base = hal_shmem_base; 272 comp->insmod_args = 0; 273 rtapi_snprintf(comp->name, sizeof(comp->name), "%s", hal_name); 274 /* insert new structure at head of list */ 275 comp->next_ptr = hal_data->comp_list_ptr; 276 hal_data->comp_list_ptr = SHMOFF(comp); 277 /* done with list, release mutex */ 278 rtapi_mutex_give(&(hal_data->mutex)); 279 /* done */ 280 rtapi_print_msg(RTAPI_MSG_DBG, 281 "HAL: component '%s' initialized, ID = %02d\n", hal_name, comp_id); 282 ref_cnt ++; 283 return comp_id; 284 } 285 286 int hal_exit(int comp_id) 287 { 288 rtapi_intptr_t *prev, next; 289 hal_comp_t *comp; 290 char name[HAL_NAME_LEN + 1]; 291 292 if (hal_data == 0) { 293 rtapi_print_msg(RTAPI_MSG_ERR, 294 "HAL: ERROR: exit called before init\n"); 295 return -EINVAL; 296 } 297 rtapi_print_msg(RTAPI_MSG_DBG, "HAL: removing component %02d\n", comp_id); 298 /* grab mutex before manipulating list */ 299 rtapi_mutex_get(&(hal_data->mutex)); 300 /* search component list for 'comp_id' */ 301 prev = &(hal_data->comp_list_ptr); 302 next = *prev; 303 if (next == 0) { 304 /* list is empty - should never happen, but... */ 305 rtapi_mutex_give(&(hal_data->mutex)); 306 rtapi_print_msg(RTAPI_MSG_ERR, 307 "HAL: ERROR: component %d not found\n", comp_id); 308 return -EINVAL; 309 } 310 comp = SHMPTR(next); 311 while (comp->comp_id != comp_id) { 312 /* not a match, try the next one */ 313 prev = &(comp->next_ptr); 314 next = *prev; 315 if (next == 0) { 316 /* reached end of list without finding component */ 317 rtapi_mutex_give(&(hal_data->mutex)); 318 rtapi_print_msg(RTAPI_MSG_ERR, 319 "HAL: ERROR: component %d not found\n", comp_id); 320 return -EINVAL; 321 } 322 comp = SHMPTR(next); 323 } 324 /* found our component, unlink it from the list */ 325 *prev = comp->next_ptr; 326 /* save component name for later */ 327 rtapi_snprintf(name, sizeof(name), "%s", comp->name); 328 /* get rid of the component */ 329 free_comp_struct(comp); 330 /*! \todo Another #if 0 */ 331 #if 0 332 /*! \todo FIXME - this is the beginning of a two pronged approach to managing 333 shared memory. Prong 1 - re-init the shared memory allocator whenever 334 it is known to be safe. Prong 2 - make a better allocator that can 335 reclaim memory allocated by components when those components are 336 removed. To be finished later. */ 337 /* was that the last component? */ 338 if (hal_data->comp_list_ptr == 0) { 339 /* yes, are there any signals or threads defined? */ 340 if ((hal_data->sig_list_ptr == 0) && (hal_data->thread_list_ptr == 0)) { 341 /* no, invalidate "magic" number so shmem will be re-inited when 342 a new component is loaded */ 343 hal_data->magic = 0; 344 } 345 } 346 #endif 347 /* release mutex */ 348 rtapi_mutex_give(&(hal_data->mutex)); 349 --ref_cnt; 350 #ifdef ULAPI 351 if(ref_cnt == 0) { 352 /* release RTAPI resources */ 353 rtapi_shmem_delete(lib_mem_id, lib_module_id); 354 rtapi_exit(lib_module_id); 355 lib_mem_id = 0; 356 lib_module_id = -1; 357 hal_shmem_base = NULL; 358 hal_data = NULL; 359 } 360 #endif 361 rtapi_exit(comp_id); 362 /* done */ 363 rtapi_print_msg(RTAPI_MSG_DBG, 364 "HAL: component %02d removed, name = '%s'\n", comp_id, name); 365 366 367 return 0; 368 } 369 370 void *hal_malloc(long int size) 371 { 372 void *retval; 373 374 if (hal_data == 0) { 375 rtapi_print_msg(RTAPI_MSG_ERR, 376 "HAL: ERROR: hal_malloc called before init\n"); 377 return 0; 378 } 379 /* get the mutex */ 380 rtapi_mutex_get(&(hal_data->mutex)); 381 /* allocate memory */ 382 retval = shmalloc_up(size); 383 /* release the mutex */ 384 rtapi_mutex_give(&(hal_data->mutex)); 385 /* check return value */ 386 if (retval == 0) { 387 rtapi_print_msg(RTAPI_MSG_DBG, 388 "HAL: hal_malloc() can't allocate %ld bytes\n", size); 389 } 390 return retval; 391 } 392 393 #ifdef RTAPI 394 int hal_set_constructor(int comp_id, constructor make) { 395 int next; 396 hal_comp_t *comp; 397 398 rtapi_mutex_get(&(hal_data->mutex)); 399 400 /* search component list for 'comp_id' */ 401 next = hal_data->comp_list_ptr; 402 if (next == 0) { 403 /* list is empty - should never happen, but... */ 404 rtapi_mutex_give(&(hal_data->mutex)); 405 rtapi_print_msg(RTAPI_MSG_ERR, 406 "HAL: ERROR: component %d not found\n", comp_id); 407 return -EINVAL; 408 } 409 410 comp = SHMPTR(next); 411 while (comp->comp_id != comp_id) { 412 /* not a match, try the next one */ 413 next = comp->next_ptr; 414 if (next == 0) { 415 /* reached end of list without finding component */ 416 rtapi_mutex_give(&(hal_data->mutex)); 417 rtapi_print_msg(RTAPI_MSG_ERR, 418 "HAL: ERROR: component %d not found\n", comp_id); 419 return -EINVAL; 420 } 421 comp = SHMPTR(next); 422 } 423 424 comp->make = make; 425 426 rtapi_mutex_give(&(hal_data->mutex)); 427 return 0; 428 } 429 #endif 430 431 int hal_ready(int comp_id) { 432 int next; 433 hal_comp_t *comp; 434 435 rtapi_mutex_get(&(hal_data->mutex)); 436 437 /* search component list for 'comp_id' */ 438 next = hal_data->comp_list_ptr; 439 if (next == 0) { 440 /* list is empty - should never happen, but... */ 441 rtapi_mutex_give(&(hal_data->mutex)); 442 rtapi_print_msg(RTAPI_MSG_ERR, 443 "HAL: ERROR: component %d not found\n", comp_id); 444 return -EINVAL; 445 } 446 447 comp = SHMPTR(next); 448 while (comp->comp_id != comp_id) { 449 /* not a match, try the next one */ 450 next = comp->next_ptr; 451 if (next == 0) { 452 /* reached end of list without finding component */ 453 rtapi_mutex_give(&(hal_data->mutex)); 454 rtapi_print_msg(RTAPI_MSG_ERR, 455 "HAL: ERROR: component %d not found\n", comp_id); 456 return -EINVAL; 457 } 458 comp = SHMPTR(next); 459 } 460 if(comp->ready > 0) { 461 rtapi_print_msg(RTAPI_MSG_ERR, 462 "HAL: ERROR: Component '%s' already ready\n", comp->name); 463 rtapi_mutex_give(&(hal_data->mutex)); 464 return -EINVAL; 465 } 466 comp->ready = 1; 467 rtapi_mutex_give(&(hal_data->mutex)); 468 return 0; 469 } 470 471 char *hal_comp_name(int comp_id) 472 { 473 hal_comp_t *comp; 474 char *result = NULL; 475 rtapi_mutex_get(&(hal_data->mutex)); 476 comp = halpr_find_comp_by_id(comp_id); 477 if(comp) result = comp->name; 478 rtapi_mutex_give(&(hal_data->mutex)); 479 return result; 480 } 481 482 /*********************************************************************** 483 * "LOCKING" FUNCTIONS * 484 ************************************************************************/ 485 /** The 'hal_set_lock()' function sets locking based on one of the 486 locking types defined in hal.h 487 */ 488 int hal_set_lock(unsigned char lock_type) { 489 if (hal_data == 0) { 490 rtapi_print_msg(RTAPI_MSG_ERR, 491 "HAL: ERROR: set_lock called before init\n"); 492 return -EINVAL; 493 } 494 hal_data->lock = lock_type; 495 return 0; 496 } 497 498 /** The 'hal_get_lock()' function returns the current locking level 499 locking types defined in hal.h 500 */ 501 502 unsigned char hal_get_lock() { 503 if (hal_data == 0) { 504 rtapi_print_msg(RTAPI_MSG_ERR, 505 "HAL: ERROR: get_lock called before init\n"); 506 return -EINVAL; 507 } 508 return hal_data->lock; 509 } 510 511 512 /*********************************************************************** 513 * "PIN" FUNCTIONS * 514 ************************************************************************/ 515 516 /* wrapper functs for typed pins - these call the generic funct below */ 517 518 int hal_pin_bit_new(const char *name, hal_pin_dir_t dir, 519 hal_bit_t ** data_ptr_addr, int comp_id) 520 { 521 return hal_pin_new(name, HAL_BIT, dir, (void **) data_ptr_addr, comp_id); 522 } 523 524 int hal_pin_float_new(const char *name, hal_pin_dir_t dir, 525 hal_float_t ** data_ptr_addr, int comp_id) 526 { 527 return hal_pin_new(name, HAL_FLOAT, dir, (void **) data_ptr_addr, 528 comp_id); 529 } 530 531 int hal_pin_u32_new(const char *name, hal_pin_dir_t dir, 532 hal_u32_t ** data_ptr_addr, int comp_id) 533 { 534 return hal_pin_new(name, HAL_U32, dir, (void **) data_ptr_addr, comp_id); 535 } 536 537 int hal_pin_s32_new(const char *name, hal_pin_dir_t dir, 538 hal_s32_t ** data_ptr_addr, int comp_id) 539 { 540 return hal_pin_new(name, HAL_S32, dir, (void **) data_ptr_addr, comp_id); 541 } 542 543 int hal_pin_port_new(const char *name, hal_pin_dir_t dir, 544 hal_port_t ** data_ptr_addr, int comp_id) 545 { 546 return hal_pin_new(name, HAL_PORT, dir, (void **)data_ptr_addr, comp_id); 547 } 548 549 550 static int hal_pin_newfv(hal_type_t type, hal_pin_dir_t dir, 551 void ** data_ptr_addr, int comp_id, const char *fmt, va_list ap) 552 { 553 char name[HAL_NAME_LEN + 1]; 554 int sz; 555 sz = rtapi_vsnprintf(name, sizeof(name), fmt, ap); 556 if(sz == -1 || sz > HAL_NAME_LEN) { 557 rtapi_print_msg(RTAPI_MSG_ERR, 558 "hal_pin_newfv: length %d too long for name starting '%s'\n", 559 sz, name); 560 return -ENOMEM; 561 } 562 return hal_pin_new(name, type, dir, data_ptr_addr, comp_id); 563 } 564 565 int hal_pin_bit_newf(hal_pin_dir_t dir, 566 hal_bit_t ** data_ptr_addr, int comp_id, const char *fmt, ...) 567 { 568 va_list ap; 569 int ret; 570 va_start(ap, fmt); 571 ret = hal_pin_newfv(HAL_BIT, dir, (void**)data_ptr_addr, comp_id, fmt, ap); 572 va_end(ap); 573 return ret; 574 } 575 576 int hal_pin_float_newf(hal_pin_dir_t dir, 577 hal_float_t ** data_ptr_addr, int comp_id, const char *fmt, ...) 578 { 579 va_list ap; 580 int ret; 581 va_start(ap, fmt); 582 ret = hal_pin_newfv(HAL_FLOAT, dir, (void**)data_ptr_addr, comp_id, fmt, ap); 583 va_end(ap); 584 return ret; 585 } 586 587 int hal_pin_u32_newf(hal_pin_dir_t dir, 588 hal_u32_t ** data_ptr_addr, int comp_id, const char *fmt, ...) 589 { 590 va_list ap; 591 int ret; 592 va_start(ap, fmt); 593 ret = hal_pin_newfv(HAL_U32, dir, (void**)data_ptr_addr, comp_id, fmt, ap); 594 va_end(ap); 595 return ret; 596 } 597 598 int hal_pin_s32_newf(hal_pin_dir_t dir, 599 hal_s32_t ** data_ptr_addr, int comp_id, const char *fmt, ...) 600 { 601 va_list ap; 602 int ret; 603 va_start(ap, fmt); 604 ret = hal_pin_newfv(HAL_S32, dir, (void**)data_ptr_addr, comp_id, fmt, ap); 605 va_end(ap); 606 return ret; 607 } 608 609 int hal_pin_port_newf(hal_pin_dir_t dir, 610 hal_port_t **data_ptr_addr, int comp_id, const char *fmt, ...) 611 { 612 va_list ap; 613 int ret; 614 va_start(ap, fmt); 615 ret = hal_pin_newfv(HAL_PORT, dir, (void**)data_ptr_addr, comp_id, fmt, ap); 616 va_end(ap); 617 return ret; 618 } 619 620 621 /* this is a generic function that does the majority of the work. */ 622 623 int hal_pin_new(const char *name, hal_type_t type, hal_pin_dir_t dir, 624 void **data_ptr_addr, int comp_id) 625 { 626 rtapi_intptr_t *prev, next; 627 int cmp; 628 hal_pin_t *new, *ptr; 629 hal_comp_t *comp; 630 631 if (hal_data == 0) { 632 rtapi_print_msg(RTAPI_MSG_ERR, 633 "HAL: ERROR: pin_new called before init\n"); 634 return -EINVAL; 635 } 636 637 if(*data_ptr_addr) 638 { 639 rtapi_print_msg(RTAPI_MSG_ERR, 640 "HAL: ERROR: pin_new(%s) called with already-initialized memory\n", 641 name); 642 } 643 if (type != HAL_BIT && type != HAL_FLOAT && type != HAL_S32 && type != HAL_U32 && type != HAL_PORT) { 644 rtapi_print_msg(RTAPI_MSG_ERR, 645 "HAL: ERROR: pin type not one of HAL_BIT, HAL_FLOAT, HAL_S32, HAL_U32 or HAL_PORT\n"); 646 return -EINVAL; 647 } 648 649 if(dir != HAL_IN && dir != HAL_OUT && dir != HAL_IO) { 650 rtapi_print_msg(RTAPI_MSG_ERR, 651 "HAL: ERROR: pin direction not one of HAL_IN, HAL_OUT, or HAL_IO\n"); 652 return -EINVAL; 653 } 654 if(type == HAL_PORT && dir == HAL_IO) { 655 rtapi_print_msg(RTAPI_MSG_ERR, 656 "HAL: ERROR: pin direction must be one of HAL_IN or HAL_OUT for hal port\n"); 657 return -EINVAL; 658 } 659 if (strlen(name) > HAL_NAME_LEN) { 660 rtapi_print_msg(RTAPI_MSG_ERR, 661 "HAL: ERROR: pin name '%s' is too long\n", name); 662 return -EINVAL; 663 } 664 if (hal_data->lock & HAL_LOCK_LOAD) { 665 rtapi_print_msg(RTAPI_MSG_ERR, 666 "HAL: ERROR: pin_new called while HAL locked\n"); 667 return -EPERM; 668 } 669 670 rtapi_print_msg(RTAPI_MSG_DBG, "HAL: creating pin '%s'\n", name); 671 /* get mutex before accessing shared data */ 672 rtapi_mutex_get(&(hal_data->mutex)); 673 /* validate comp_id */ 674 comp = halpr_find_comp_by_id(comp_id); 675 if (comp == 0) { 676 /* bad comp_id */ 677 rtapi_mutex_give(&(hal_data->mutex)); 678 rtapi_print_msg(RTAPI_MSG_ERR, 679 "HAL: ERROR: component %d not found\n", comp_id); 680 return -EINVAL; 681 } 682 /* validate passed in pointer - must point to HAL shmem */ 683 if (! SHMCHK(data_ptr_addr)) { 684 /* bad pointer */ 685 rtapi_mutex_give(&(hal_data->mutex)); 686 rtapi_print_msg(RTAPI_MSG_ERR, 687 "HAL: ERROR: data_ptr_addr not in shared memory\n"); 688 return -EINVAL; 689 } 690 if(comp->ready) { 691 rtapi_mutex_give(&(hal_data->mutex)); 692 rtapi_print_msg(RTAPI_MSG_ERR, 693 "HAL: ERROR: pin_new called after hal_ready\n"); 694 return -EINVAL; 695 } 696 /* allocate a new variable structure */ 697 new = alloc_pin_struct(); 698 if (new == 0) { 699 /* alloc failed */ 700 rtapi_mutex_give(&(hal_data->mutex)); 701 rtapi_print_msg(RTAPI_MSG_ERR, 702 "HAL: ERROR: insufficient memory for pin '%s'\n", name); 703 return -ENOMEM; 704 } 705 /* initialize the structure */ 706 new->data_ptr_addr = SHMOFF(data_ptr_addr); 707 new->owner_ptr = SHMOFF(comp); 708 new->type = type; 709 new->dir = dir; 710 new->signal = 0; 711 memset(&new->dummysig, 0, sizeof(hal_data_u)); 712 rtapi_snprintf(new->name, sizeof(new->name), "%s", name); 713 /* make 'data_ptr' point to dummy signal */ 714 *data_ptr_addr = comp->shmem_base + SHMOFF(&(new->dummysig)); 715 /* search list for 'name' and insert new structure */ 716 prev = &(hal_data->pin_list_ptr); 717 next = *prev; 718 while (1) { 719 if (next == 0) { 720 /* reached end of list, insert here */ 721 new->next_ptr = next; 722 *prev = SHMOFF(new); 723 rtapi_mutex_give(&(hal_data->mutex)); 724 return 0; 725 } 726 ptr = SHMPTR(next); 727 cmp = strcmp(ptr->name, new->name); 728 if (cmp > 0) { 729 /* found the right place for it, insert here */ 730 new->next_ptr = next; 731 *prev = SHMOFF(new); 732 rtapi_mutex_give(&(hal_data->mutex)); 733 return 0; 734 } 735 if (cmp == 0) { 736 /* name already in list, can't insert */ 737 free_pin_struct(new); 738 rtapi_mutex_give(&(hal_data->mutex)); 739 rtapi_print_msg(RTAPI_MSG_ERR, 740 "HAL: ERROR: duplicate variable '%s'\n", name); 741 return -EINVAL; 742 } 743 /* didn't find it yet, look at next one */ 744 prev = &(ptr->next_ptr); 745 next = *prev; 746 } 747 } 748 749 int hal_pin_alias(const char *pin_name, const char *alias) 750 { 751 rtapi_intptr_t *prev, next; 752 int cmp; 753 hal_pin_t *pin, *ptr; 754 hal_oldname_t *oldname; 755 756 if (hal_data == 0) { 757 rtapi_print_msg(RTAPI_MSG_ERR, 758 "HAL: ERROR: pin_alias called before init\n"); 759 return -EINVAL; 760 } 761 if (hal_data->lock & HAL_LOCK_CONFIG) { 762 rtapi_print_msg(RTAPI_MSG_ERR, 763 "HAL: ERROR: pin_alias called while HAL locked\n"); 764 return -EPERM; 765 } 766 if (alias != NULL ) { 767 if (strlen(alias) > HAL_NAME_LEN) { 768 rtapi_print_msg(RTAPI_MSG_ERR, 769 "HAL: ERROR: alias name '%s' is too long\n", alias); 770 return -EINVAL; 771 } 772 } 773 /* get mutex before accessing shared data */ 774 rtapi_mutex_get(&(hal_data->mutex)); 775 if (alias != NULL ) { 776 pin = halpr_find_pin_by_name(alias); 777 if ( pin != NULL ) { 778 rtapi_mutex_give(&(hal_data->mutex)); 779 rtapi_print_msg(RTAPI_MSG_ERR, 780 "HAL: ERROR: duplicate pin/alias name '%s'\n", alias); 781 return -EINVAL; 782 } 783 } 784 /* once we unlink the pin from the list, we don't want to have to 785 abort the change and repair things. So we allocate an oldname 786 struct here, then free it (which puts it on the free list). This 787 allocation might fail, in which case we abort the command. But 788 if we actually need the struct later, the next alloc is guaranteed 789 to succeed since at least one struct is on the free list. */ 790 oldname = halpr_alloc_oldname_struct(); 791 if ( oldname == NULL ) { 792 rtapi_mutex_give(&(hal_data->mutex)); 793 rtapi_print_msg(RTAPI_MSG_ERR, 794 "HAL: ERROR: insufficient memory for pin_alias\n"); 795 return -EINVAL; 796 } 797 free_oldname_struct(oldname); 798 /* find the pin and unlink it from pin list */ 799 prev = &(hal_data->pin_list_ptr); 800 next = *prev; 801 while (1) { 802 if (next == 0) { 803 /* reached end of list, not found */ 804 rtapi_mutex_give(&(hal_data->mutex)); 805 rtapi_print_msg(RTAPI_MSG_ERR, 806 "HAL: ERROR: pin '%s' not found\n", pin_name); 807 return -EINVAL; 808 } 809 pin = SHMPTR(next); 810 if ( strcmp(pin->name, pin_name) == 0 ) { 811 /* found it, unlink from list */ 812 *prev = pin->next_ptr; 813 break; 814 } 815 if (pin->oldname != 0 ) { 816 oldname = SHMPTR(pin->oldname); 817 if (strcmp(oldname->name, pin_name) == 0) { 818 /* found it, unlink from list */ 819 *prev = pin->next_ptr; 820 break; 821 } 822 } 823 /* didn't find it yet, look at next one */ 824 prev = &(pin->next_ptr); 825 next = *prev; 826 } 827 if ( alias != NULL ) { 828 /* adding a new alias */ 829 if ( pin->oldname == 0 ) { 830 /* save old name (only if not already saved) */ 831 oldname = halpr_alloc_oldname_struct(); 832 pin->oldname = SHMOFF(oldname); 833 rtapi_snprintf(oldname->name, sizeof(oldname->name), "%s", pin->name); 834 } 835 /* change pin's name to 'alias' */ 836 rtapi_snprintf(pin->name, sizeof(pin->name), "%s", alias); 837 } else { 838 /* removing an alias */ 839 if ( pin->oldname != 0 ) { 840 /* restore old name (only if pin is aliased) */ 841 oldname = SHMPTR(pin->oldname); 842 rtapi_snprintf(pin->name, sizeof(pin->name), "%s", oldname->name); 843 pin->oldname = 0; 844 free_oldname_struct(oldname); 845 } 846 } 847 /* insert pin back into list in proper place */ 848 prev = &(hal_data->pin_list_ptr); 849 next = *prev; 850 while (1) { 851 if (next == 0) { 852 /* reached end of list, insert here */ 853 pin->next_ptr = next; 854 *prev = SHMOFF(pin); 855 rtapi_mutex_give(&(hal_data->mutex)); 856 return 0; 857 } 858 ptr = SHMPTR(next); 859 cmp = strcmp(ptr->name, pin->name); 860 if (cmp > 0) { 861 /* found the right place for it, insert here */ 862 pin->next_ptr = next; 863 *prev = SHMOFF(pin); 864 rtapi_mutex_give(&(hal_data->mutex)); 865 return 0; 866 } 867 /* didn't find it yet, look at next one */ 868 prev = &(ptr->next_ptr); 869 next = *prev; 870 } 871 } 872 873 /*********************************************************************** 874 * "SIGNAL" FUNCTIONS * 875 ************************************************************************/ 876 877 int hal_signal_new(const char *name, hal_type_t type) 878 { 879 880 rtapi_intptr_t *prev, next; 881 int cmp; 882 hal_sig_t *new, *ptr; 883 void *data_addr; 884 885 if (hal_data == 0) { 886 rtapi_print_msg(RTAPI_MSG_ERR, 887 "HAL: ERROR: signal_new called before init\n"); 888 return -EINVAL; 889 } 890 891 if (strlen(name) > HAL_NAME_LEN) { 892 rtapi_print_msg(RTAPI_MSG_ERR, 893 "HAL: ERROR: signal name '%s' is too long\n", name); 894 return -EINVAL; 895 } 896 if (hal_data->lock & HAL_LOCK_CONFIG) { 897 rtapi_print_msg(RTAPI_MSG_ERR, 898 "HAL: ERROR: signal_new called while HAL is locked\n"); 899 return -EPERM; 900 } 901 902 rtapi_print_msg(RTAPI_MSG_DBG, "HAL: creating signal '%s'\n", name); 903 /* get mutex before accessing shared data */ 904 rtapi_mutex_get(&(hal_data->mutex)); 905 /* check for an existing signal with the same name */ 906 if (halpr_find_sig_by_name(name) != 0) { 907 rtapi_mutex_give(&(hal_data->mutex)); 908 rtapi_print_msg(RTAPI_MSG_ERR, 909 "HAL: ERROR: duplicate signal '%s'\n", name); 910 return -EINVAL; 911 } 912 /* allocate memory for the signal value */ 913 /* 914 because accesses will later be through pointer of type hal_data_u, 915 allocate something that big. Otherwise, gcc -fsanitize=undefined will 916 issue diagnostics like 917 hal/hal_lib.c:3203:35: runtime error: member access within misaligned address 0x7fcf3d11f10b for type 'union hal_data_u', which requires 8 byte alignment 918 on accesses through hal_data_u. 919 920 This does increase memory usage somewhat, but is required for compliance 921 with the C standard. 922 */ 923 switch (type) { 924 case HAL_BIT: 925 case HAL_S32: 926 case HAL_U32: 927 case HAL_FLOAT: 928 case HAL_PORT: 929 data_addr = shmalloc_up(sizeof(hal_data_u)); 930 break; 931 default: 932 rtapi_mutex_give(&(hal_data->mutex)); 933 rtapi_print_msg(RTAPI_MSG_ERR, 934 "HAL: ERROR: illegal signal type %d'\n", type); 935 return -EINVAL; 936 break; 937 } 938 /* allocate a new signal structure */ 939 new = alloc_sig_struct(); 940 if ((new == 0) || (data_addr == 0)) { 941 /* alloc failed */ 942 rtapi_mutex_give(&(hal_data->mutex)); 943 rtapi_print_msg(RTAPI_MSG_ERR, 944 "HAL: ERROR: insufficient memory for signal '%s'\n", name); 945 return -ENOMEM; 946 } 947 /* initialize the signal value */ 948 switch (type) { 949 case HAL_BIT: 950 *((hal_bit_t *) data_addr) = 0; 951 break; 952 case HAL_S32: 953 *((hal_s32_t *) data_addr) = 0; 954 break; 955 case HAL_U32: 956 *((hal_u32_t *) data_addr) = 0; 957 break; 958 case HAL_FLOAT: 959 *((hal_float_t *) data_addr) = 0.0; 960 break; 961 case HAL_PORT: 962 *((int *) data_addr) = 0; 963 break; 964 default: 965 break; 966 } 967 /* initialize the structure */ 968 new->data_ptr = SHMOFF(data_addr); 969 new->type = type; 970 new->readers = 0; 971 new->writers = 0; 972 new->bidirs = 0; 973 rtapi_snprintf(new->name, sizeof(new->name), "%s", name); 974 /* search list for 'name' and insert new structure */ 975 prev = &(hal_data->sig_list_ptr); 976 next = *prev; 977 while (1) { 978 if (next == 0) { 979 /* reached end of list, insert here */ 980 new->next_ptr = next; 981 *prev = SHMOFF(new); 982 rtapi_mutex_give(&(hal_data->mutex)); 983 return 0; 984 } 985 ptr = SHMPTR(next); 986 cmp = strcmp(ptr->name, new->name); 987 if (cmp > 0) { 988 /* found the right place for it, insert here */ 989 new->next_ptr = next; 990 *prev = SHMOFF(new); 991 rtapi_mutex_give(&(hal_data->mutex)); 992 return 0; 993 } 994 /* didn't find it yet, look at next one */ 995 prev = &(ptr->next_ptr); 996 next = *prev; 997 } 998 } 999 1000 int hal_signal_delete(const char *name) 1001 { 1002 hal_sig_t *sig; 1003 rtapi_intptr_t *prev, next; 1004 1005 if (hal_data == 0) { 1006 rtapi_print_msg(RTAPI_MSG_ERR, 1007 "HAL: ERROR: signal_delete called before init\n"); 1008 return -EINVAL; 1009 } 1010 1011 if (hal_data->lock & HAL_LOCK_CONFIG) { 1012 rtapi_print_msg(RTAPI_MSG_ERR, 1013 "HAL: ERROR: signal_delete called while HAL locked\n"); 1014 return -EPERM; 1015 } 1016 1017 rtapi_print_msg(RTAPI_MSG_DBG, "HAL: deleting signal '%s'\n", name); 1018 /* get mutex before accessing shared data */ 1019 rtapi_mutex_get(&(hal_data->mutex)); 1020 /* search for the signal */ 1021 prev = &(hal_data->sig_list_ptr); 1022 next = *prev; 1023 while (next != 0) { 1024 sig = SHMPTR(next); 1025 if (strcmp(sig->name, name) == 0) { 1026 /* this is the right signal, unlink from list */ 1027 *prev = sig->next_ptr; 1028 /* and delete it */ 1029 free_sig_struct(sig); 1030 /* done */ 1031 rtapi_mutex_give(&(hal_data->mutex)); 1032 return 0; 1033 } 1034 /* no match, try the next one */ 1035 prev = &(sig->next_ptr); 1036 next = *prev; 1037 } 1038 /* if we get here, we didn't find a match */ 1039 rtapi_mutex_give(&(hal_data->mutex)); 1040 rtapi_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: signal '%s' not found\n", 1041 name); 1042 return -EINVAL; 1043 } 1044 1045 int hal_link(const char *pin_name, const char *sig_name) 1046 { 1047 hal_pin_t *pin; 1048 hal_sig_t *sig; 1049 hal_comp_t *comp; 1050 void **data_ptr_addr, *data_addr; 1051 1052 if (hal_data == 0) { 1053 rtapi_print_msg(RTAPI_MSG_ERR, 1054 "HAL: ERROR: link called before init\n"); 1055 return -EINVAL; 1056 } 1057 1058 if (hal_data->lock & HAL_LOCK_CONFIG) { 1059 rtapi_print_msg(RTAPI_MSG_ERR, 1060 "HAL: ERROR: link called while HAL locked\n"); 1061 return -EPERM; 1062 } 1063 /* make sure we were given a pin name */ 1064 if (pin_name == 0) { 1065 rtapi_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: pin name not given\n"); 1066 return -EINVAL; 1067 } 1068 /* make sure we were given a signal name */ 1069 if (sig_name == 0) { 1070 rtapi_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: signal name not given\n"); 1071 return -EINVAL; 1072 } 1073 rtapi_print_msg(RTAPI_MSG_DBG, 1074 "HAL: linking pin '%s' to '%s'\n", pin_name, sig_name); 1075 /* get mutex before accessing data structures */ 1076 rtapi_mutex_get(&(hal_data->mutex)); 1077 /* locate the pin */ 1078 pin = halpr_find_pin_by_name(pin_name); 1079 if (pin == 0) { 1080 /* not found */ 1081 rtapi_mutex_give(&(hal_data->mutex)); 1082 rtapi_print_msg(RTAPI_MSG_ERR, 1083 "HAL: ERROR: pin '%s' not found\n", pin_name); 1084 return -EINVAL; 1085 } 1086 /* locate the signal */ 1087 sig = halpr_find_sig_by_name(sig_name); 1088 if (sig == 0) { 1089 /* not found */ 1090 rtapi_mutex_give(&(hal_data->mutex)); 1091 rtapi_print_msg(RTAPI_MSG_ERR, 1092 "HAL: ERROR: signal '%s' not found\n", sig_name); 1093 return -EINVAL; 1094 } 1095 /* found both pin and signal, are they already connected? */ 1096 if (SHMPTR(pin->signal) == sig) { 1097 rtapi_mutex_give(&(hal_data->mutex)); 1098 rtapi_print_msg(RTAPI_MSG_WARN, 1099 "HAL: Warning: pin '%s' already linked to '%s'\n", pin_name, sig_name); 1100 return 0; 1101 } 1102 /* is the pin connected to something else? */ 1103 if(pin->signal) { 1104 rtapi_mutex_give(&(hal_data->mutex)); 1105 sig = SHMPTR(pin->signal); 1106 rtapi_print_msg(RTAPI_MSG_ERR, 1107 "HAL: ERROR: pin '%s' is linked to '%s', cannot link to '%s'\n", 1108 pin_name, sig->name, sig_name); 1109 return -EINVAL; 1110 } 1111 /* check types */ 1112 if (pin->type != sig->type) { 1113 rtapi_mutex_give(&(hal_data->mutex)); 1114 rtapi_print_msg(RTAPI_MSG_ERR, 1115 "HAL: ERROR: type mismatch '%s' <- '%s'\n", pin_name, sig_name); 1116 return -EINVAL; 1117 } 1118 /* linking output pin to sig that already has output or I/O pins? */ 1119 if ((pin->dir == HAL_OUT) && ((sig->writers > 0) || (sig->bidirs > 0 ))) { 1120 /* yes, can't do that */ 1121 rtapi_mutex_give(&(hal_data->mutex)); 1122 rtapi_print_msg(RTAPI_MSG_ERR, 1123 "HAL: ERROR: signal '%s' already has output or I/O pin(s)\n", sig_name); 1124 return -EINVAL; 1125 } 1126 /* linking bidir pin to sig that is a port?*/ 1127 if ((pin->dir == HAL_IO) && (pin->type == HAL_PORT)) { 1128 rtapi_mutex_give(&(hal_data->mutex)); 1129 rtapi_print_msg(RTAPI_MSG_ERR, 1130 "HAL: ERROR: signal '%s' is a port and cannot have I/O pin(s)\n", sig_name); 1131 return -EINVAL; 1132 } 1133 /* linking bidir pin to sig that already has output pin? */ 1134 if ((pin->dir == HAL_IO) && (sig->writers > 0)) { 1135 /* yes, can't do that */ 1136 rtapi_mutex_give(&(hal_data->mutex)); 1137 rtapi_print_msg(RTAPI_MSG_ERR, 1138 "HAL: ERROR: signal '%s' already has output pin\n", sig_name); 1139 return -EINVAL; 1140 } 1141 1142 /* linking input pin to port sig that already has an input port? */ 1143 if ((pin->type == HAL_PORT) && (pin->dir == HAL_IN) && (sig->readers > 0)) { 1144 /* ports can only have one reader */ 1145 rtapi_mutex_give(&(hal_data->mutex)); 1146 rtapi_print_msg(RTAPI_MSG_ERR, 1147 "HAL: ERROR: siganl '%s' can only have one input pin\n", sig_name); 1148 return -EINVAL; 1149 } 1150 1151 /* everything is OK, make the new link */ 1152 data_ptr_addr = SHMPTR(pin->data_ptr_addr); 1153 comp = SHMPTR(pin->owner_ptr); 1154 data_addr = comp->shmem_base + sig->data_ptr; 1155 *data_ptr_addr = data_addr; 1156 1157 /* if the pin is a HAL_PORT the buffer belongs to the signal, port pins not linked 1158 to a signal will always be empty (unreadable and unwritable)*/ 1159 bool drive_pin_default_value_onto_signal = 1160 (pin->type != HAL_PORT) && (pin->dir != HAL_IN || sig->readers == 0 ) 1161 && ( sig->writers == 0 ) && ( sig->bidirs == 0 ); 1162 if (drive_pin_default_value_onto_signal) { 1163 /* this is the first pin for this signal, copy value from pin's "dummy" field */ 1164 data_addr = hal_shmem_base + sig->data_ptr; 1165 1166 // assure proper typing on assignment, assigning a hal_data_u is 1167 // a surefire cause for memory corrupion as hal_data_u is larger 1168 // than hal_bit_t, hal_s32_t, and hal_u32_t - this works only for 1169 // hal_float_t (!) 1170 // my old, buggy code: 1171 //*((hal_data_u *)data_addr) = pin->dummysig; 1172 1173 switch (pin->type) { 1174 case HAL_BIT: 1175 *((hal_bit_t *) data_addr) = pin->dummysig.b; 1176 break; 1177 case HAL_S32: 1178 *((hal_s32_t *) data_addr) = pin->dummysig.s; 1179 break; 1180 case HAL_U32: 1181 *((hal_u32_t *) data_addr) = pin->dummysig.u; 1182 break; 1183 case HAL_FLOAT: 1184 *((hal_float_t *) data_addr) = pin->dummysig.f; 1185 break; 1186 default: 1187 rtapi_print_msg(RTAPI_MSG_ERR, 1188 "HAL: BUG: pin '%s' has invalid type %d !!\n", 1189 pin->name, pin->type); 1190 return -EINVAL; 1191 } 1192 } 1193 /* update the signal's reader/writer/bidir counts */ 1194 if ((pin->dir & HAL_IN) != 0) { 1195 sig->readers++; 1196 } 1197 if (pin->dir == HAL_OUT) { 1198 sig->writers++; 1199 } 1200 if (pin->dir == HAL_IO) { 1201 sig->bidirs++; 1202 } 1203 /* and update the pin */ 1204 pin->signal = SHMOFF(sig); 1205 /* done, release the mutex and return */ 1206 rtapi_mutex_give(&(hal_data->mutex)); 1207 return 0; 1208 } 1209 1210 int hal_unlink(const char *pin_name) 1211 { 1212 hal_pin_t *pin; 1213 1214 if (hal_data == 0) { 1215 rtapi_print_msg(RTAPI_MSG_ERR, 1216 "HAL: ERROR: unlink called before init\n"); 1217 return -EINVAL; 1218 } 1219 1220 if (hal_data->lock & HAL_LOCK_CONFIG) { 1221 rtapi_print_msg(RTAPI_MSG_ERR, 1222 "HAL: ERROR: unlink called while HAL locked\n"); 1223 return -EPERM; 1224 } 1225 /* make sure we were given a pin name */ 1226 if (pin_name == 0) { 1227 rtapi_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: pin name not given\n"); 1228 return -EINVAL; 1229 } 1230 rtapi_print_msg(RTAPI_MSG_DBG, 1231 "HAL: unlinking pin '%s'\n", pin_name); 1232 /* get mutex before accessing data structures */ 1233 rtapi_mutex_get(&(hal_data->mutex)); 1234 /* locate the pin */ 1235 pin = halpr_find_pin_by_name(pin_name); 1236 if (pin == 0) { 1237 /* not found */ 1238 rtapi_mutex_give(&(hal_data->mutex)); 1239 rtapi_print_msg(RTAPI_MSG_ERR, 1240 "HAL: ERROR: pin '%s' not found\n", pin_name); 1241 return -EINVAL; 1242 } 1243 /* found pin, unlink it */ 1244 unlink_pin(pin); 1245 /* done, release the mutex and return */ 1246 rtapi_mutex_give(&(hal_data->mutex)); 1247 return 0; 1248 } 1249 1250 /*********************************************************************** 1251 * "PARAM" FUNCTIONS * 1252 ************************************************************************/ 1253 1254 /* wrapper functs for typed params - these call the generic funct below */ 1255 1256 int hal_param_bit_new(const char *name, hal_param_dir_t dir, hal_bit_t * data_addr, 1257 int comp_id) 1258 { 1259 return hal_param_new(name, HAL_BIT, dir, (void *) data_addr, comp_id); 1260 } 1261 1262 int hal_param_float_new(const char *name, hal_param_dir_t dir, hal_float_t * data_addr, 1263 int comp_id) 1264 { 1265 return hal_param_new(name, HAL_FLOAT, dir, (void *) data_addr, comp_id); 1266 } 1267 1268 int hal_param_u32_new(const char *name, hal_param_dir_t dir, hal_u32_t * data_addr, 1269 int comp_id) 1270 { 1271 return hal_param_new(name, HAL_U32, dir, (void *) data_addr, comp_id); 1272 } 1273 1274 int hal_param_s32_new(const char *name, hal_param_dir_t dir, hal_s32_t * data_addr, 1275 int comp_id) 1276 { 1277 return hal_param_new(name, HAL_S32, dir, (void *) data_addr, comp_id); 1278 } 1279 1280 static int hal_param_newfv(hal_type_t type, hal_param_dir_t dir, 1281 void *data_addr, int comp_id, const char *fmt, va_list ap) { 1282 char name[HAL_NAME_LEN + 1]; 1283 int sz; 1284 sz = rtapi_vsnprintf(name, sizeof(name), fmt, ap); 1285 if(sz == -1 || sz > HAL_NAME_LEN) { 1286 rtapi_print_msg(RTAPI_MSG_ERR, 1287 "hal_param_newfv: length %d too long for name starting '%s'\n", 1288 sz, name); 1289 return -ENOMEM; 1290 } 1291 return hal_param_new(name, type, dir, (void *) data_addr, comp_id); 1292 } 1293 1294 int hal_param_bit_newf(hal_param_dir_t dir, hal_bit_t * data_addr, 1295 int comp_id, const char *fmt, ...) 1296 { 1297 va_list ap; 1298 int ret; 1299 va_start(ap, fmt); 1300 ret = hal_param_newfv(HAL_BIT, dir, (void*)data_addr, comp_id, fmt, ap); 1301 va_end(ap); 1302 return ret; 1303 } 1304 1305 int hal_param_float_newf(hal_param_dir_t dir, hal_float_t * data_addr, 1306 int comp_id, const char *fmt, ...) 1307 { 1308 va_list ap; 1309 int ret; 1310 va_start(ap, fmt); 1311 ret = hal_param_newfv(HAL_FLOAT, dir, (void*)data_addr, comp_id, fmt, ap); 1312 va_end(ap); 1313 return ret; 1314 } 1315 1316 int hal_param_u32_newf(hal_param_dir_t dir, hal_u32_t * data_addr, 1317 int comp_id, const char *fmt, ...) 1318 { 1319 va_list ap; 1320 int ret; 1321 va_start(ap, fmt); 1322 ret = hal_param_newfv(HAL_U32, dir, (void*)data_addr, comp_id, fmt, ap); 1323 va_end(ap); 1324 return ret; 1325 } 1326 1327 int hal_param_s32_newf(hal_param_dir_t dir, hal_s32_t * data_addr, 1328 int comp_id, const char *fmt, ...) 1329 { 1330 va_list ap; 1331 int ret; 1332 va_start(ap, fmt); 1333 ret = hal_param_newfv(HAL_S32, dir, (void*)data_addr, comp_id, fmt, ap); 1334 va_end(ap); 1335 return ret; 1336 } 1337 1338 1339 /* this is a generic function that does the majority of the work. */ 1340 1341 int hal_param_new(const char *name, hal_type_t type, hal_param_dir_t dir, void *data_addr, 1342 int comp_id) 1343 { 1344 rtapi_intptr_t *prev, next; 1345 int cmp; 1346 hal_param_t *new, *ptr; 1347 hal_comp_t *comp; 1348 1349 if (hal_data == 0) { 1350 rtapi_print_msg(RTAPI_MSG_ERR, 1351 "HAL: ERROR: param_new called before init\n"); 1352 return -EINVAL; 1353 } 1354 1355 if (type != HAL_BIT && type != HAL_FLOAT && type != HAL_S32 && type != HAL_U32) { 1356 rtapi_print_msg(RTAPI_MSG_ERR, 1357 "HAL: ERROR: pin type not one of HAL_BIT, HAL_FLOAT, HAL_S32 or HAL_U32\n"); 1358 return -EINVAL; 1359 } 1360 1361 if(dir != HAL_RO && dir != HAL_RW) { 1362 rtapi_print_msg(RTAPI_MSG_ERR, 1363 "HAL: ERROR: param direction not one of HAL_RO, or HAL_RW\n"); 1364 return -EINVAL; 1365 } 1366 1367 if (strlen(name) > HAL_NAME_LEN) { 1368 rtapi_print_msg(RTAPI_MSG_ERR, 1369 "HAL: ERROR: parameter name '%s' is too long\n", name); 1370 return -EINVAL; 1371 } 1372 if (hal_data->lock & HAL_LOCK_LOAD) { 1373 rtapi_print_msg(RTAPI_MSG_ERR, 1374 "HAL: ERROR: param_new called while HAL locked\n"); 1375 return -EPERM; 1376 } 1377 1378 rtapi_print_msg(RTAPI_MSG_DBG, "HAL: creating parameter '%s'\n", name); 1379 /* get mutex before accessing shared data */ 1380 rtapi_mutex_get(&(hal_data->mutex)); 1381 /* validate comp_id */ 1382 comp = halpr_find_comp_by_id(comp_id); 1383 if (comp == 0) { 1384 /* bad comp_id */ 1385 rtapi_mutex_give(&(hal_data->mutex)); 1386 rtapi_print_msg(RTAPI_MSG_ERR, 1387 "HAL: ERROR: component %d not found\n", comp_id); 1388 return -EINVAL; 1389 } 1390 /* validate passed in pointer - must point to HAL shmem */ 1391 if (! SHMCHK(data_addr)) { 1392 /* bad pointer */ 1393 rtapi_mutex_give(&(hal_data->mutex)); 1394 rtapi_print_msg(RTAPI_MSG_ERR, 1395 "HAL: ERROR: data_addr not in shared memory\n"); 1396 return -EINVAL; 1397 } 1398 if(comp->ready) { 1399 rtapi_mutex_give(&(hal_data->mutex)); 1400 rtapi_print_msg(RTAPI_MSG_ERR, 1401 "HAL: ERROR: param_new called after hal_ready\n"); 1402 return -EINVAL; 1403 } 1404 /* allocate a new parameter structure */ 1405 new = alloc_param_struct(); 1406 if (new == 0) { 1407 /* alloc failed */ 1408 rtapi_mutex_give(&(hal_data->mutex)); 1409 rtapi_print_msg(RTAPI_MSG_ERR, 1410 "HAL: ERROR: insufficient memory for parameter '%s'\n", name); 1411 return -ENOMEM; 1412 } 1413 /* initialize the structure */ 1414 new->owner_ptr = SHMOFF(comp); 1415 new->data_ptr = SHMOFF(data_addr); 1416 new->type = type; 1417 new->dir = dir; 1418 rtapi_snprintf(new->name, sizeof(new->name), "%s", name); 1419 /* search list for 'name' and insert new structure */ 1420 prev = &(hal_data->param_list_ptr); 1421 next = *prev; 1422 while (1) { 1423 if (next == 0) { 1424 /* reached end of list, insert here */ 1425 new->next_ptr = next; 1426 *prev = SHMOFF(new); 1427 rtapi_mutex_give(&(hal_data->mutex)); 1428 return 0; 1429 } 1430 ptr = SHMPTR(next); 1431 cmp = strcmp(ptr->name, new->name); 1432 if (cmp > 0) { 1433 /* found the right place for it, insert here */ 1434 new->next_ptr = next; 1435 *prev = SHMOFF(new); 1436 rtapi_mutex_give(&(hal_data->mutex)); 1437 return 0; 1438 } 1439 if (cmp == 0) { 1440 /* name already in list, can't insert */ 1441 free_param_struct(new); 1442 rtapi_mutex_give(&(hal_data->mutex)); 1443 rtapi_print_msg(RTAPI_MSG_ERR, 1444 "HAL: ERROR: duplicate parameter '%s'\n", name); 1445 return -EINVAL; 1446 } 1447 /* didn't find it yet, look at next one */ 1448 prev = &(ptr->next_ptr); 1449 next = *prev; 1450 } 1451 } 1452 1453 /* wrapper functs for typed params - these call the generic funct below */ 1454 1455 int hal_param_bit_set(const char *name, int value) 1456 { 1457 return hal_param_set(name, HAL_BIT, &value); 1458 } 1459 1460 int hal_param_float_set(const char *name, double value) 1461 { 1462 return hal_param_set(name, HAL_FLOAT, &value); 1463 } 1464 1465 int hal_param_u32_set(const char *name, unsigned long value) 1466 { 1467 return hal_param_set(name, HAL_U32, &value); 1468 } 1469 1470 int hal_param_s32_set(const char *name, signed long value) 1471 { 1472 return hal_param_set(name, HAL_S32, &value); 1473 } 1474 1475 /* this is a generic function that does the majority of the work */ 1476 1477 int hal_param_set(const char *name, hal_type_t type, void *value_addr) 1478 { 1479 hal_param_t *param; 1480 void *d_ptr; 1481 1482 if (hal_data == 0) { 1483 rtapi_print_msg(RTAPI_MSG_ERR, 1484 "HAL: ERROR: param_set called before init\n"); 1485 return -EINVAL; 1486 } 1487 1488 if (hal_data->lock & HAL_LOCK_PARAMS) { 1489 rtapi_print_msg(RTAPI_MSG_ERR, 1490 "HAL: ERROR: param_set called while HAL locked\n"); 1491 return -EPERM; 1492 } 1493 1494 rtapi_print_msg(RTAPI_MSG_DBG, "HAL: setting parameter '%s'\n", name); 1495 /* get mutex before accessing shared data */ 1496 rtapi_mutex_get(&(hal_data->mutex)); 1497 1498 /* search param list for name */ 1499 param = halpr_find_param_by_name(name); 1500 if (param == 0) { 1501 /* parameter not found */ 1502 rtapi_mutex_give(&(hal_data->mutex)); 1503 rtapi_print_msg(RTAPI_MSG_ERR, 1504 "HAL: ERROR: parameter '%s' not found\n", name); 1505 return -EINVAL; 1506 } 1507 /* found it, is type compatible? */ 1508 if (param->type != type) { 1509 rtapi_mutex_give(&(hal_data->mutex)); 1510 rtapi_print_msg(RTAPI_MSG_ERR, 1511 "HAL: ERROR: type mismatch setting param '%s'\n", name); 1512 return -EINVAL; 1513 } 1514 /* is it read only? */ 1515 if (param->dir == HAL_RO) { 1516 rtapi_mutex_give(&(hal_data->mutex)); 1517 rtapi_print_msg(RTAPI_MSG_ERR, 1518 "HAL: ERROR: param '%s' is not writable\n", name); 1519 return -EINVAL; 1520 } 1521 /* everything is OK, set the value */ 1522 d_ptr = SHMPTR(param->data_ptr); 1523 switch (param->type) { 1524 case HAL_BIT: 1525 if (*((int *) value_addr) == 0) { 1526 *(hal_bit_t *) (d_ptr) = 0; 1527 } else { 1528 *(hal_bit_t *) (d_ptr) = 1; 1529 } 1530 break; 1531 case HAL_FLOAT: 1532 *((hal_float_t *) (d_ptr)) = *((double *) (value_addr)); 1533 break; 1534 case HAL_S32: 1535 *((hal_s32_t *) (d_ptr)) = *((signed long *) (value_addr)); 1536 break; 1537 case HAL_U32: 1538 *((hal_u32_t *) (d_ptr)) = *((unsigned long *) (value_addr)); 1539 break; 1540 default: 1541 /* Shouldn't get here, but just in case... */ 1542 rtapi_mutex_give(&(hal_data->mutex)); 1543 rtapi_print_msg(RTAPI_MSG_ERR, 1544 "HAL: ERROR: bad type %d setting param\n", param->type); 1545 return -EINVAL; 1546 } 1547 rtapi_mutex_give(&(hal_data->mutex)); 1548 return 0; 1549 } 1550 1551 int hal_param_alias(const char *param_name, const char *alias) 1552 { 1553 rtapi_intptr_t *prev, next; 1554 int cmp; 1555 hal_param_t *param, *ptr; 1556 hal_oldname_t *oldname; 1557 1558 if (hal_data == 0) { 1559 rtapi_print_msg(RTAPI_MSG_ERR, 1560 "HAL: ERROR: param_alias called before init\n"); 1561 return -EINVAL; 1562 } 1563 if (hal_data->lock & HAL_LOCK_CONFIG) { 1564 rtapi_print_msg(RTAPI_MSG_ERR, 1565 "HAL: ERROR: param_alias called while HAL locked\n"); 1566 return -EPERM; 1567 } 1568 if (alias != NULL ) { 1569 if (strlen(alias) > HAL_NAME_LEN) { 1570 rtapi_print_msg(RTAPI_MSG_ERR, 1571 "HAL: ERROR: alias name '%s' is too long\n", alias); 1572 return -EINVAL; 1573 } 1574 } 1575 /* get mutex before accessing shared data */ 1576 rtapi_mutex_get(&(hal_data->mutex)); 1577 if (alias != NULL ) { 1578 param = halpr_find_param_by_name(alias); 1579 if ( param != NULL ) { 1580 rtapi_mutex_give(&(hal_data->mutex)); 1581 rtapi_print_msg(RTAPI_MSG_ERR, 1582 "HAL: ERROR: duplicate pin/alias name '%s'\n", alias); 1583 return -EINVAL; 1584 } 1585 } 1586 /* once we unlink the param from the list, we don't want to have to 1587 abort the change and repair things. So we allocate an oldname 1588 struct here, then free it (which puts it on the free list). This 1589 allocation might fail, in which case we abort the command. But 1590 if we actually need the struct later, the next alloc is guaranteed 1591 to succeed since at least one struct is on the free list. */ 1592 oldname = halpr_alloc_oldname_struct(); 1593 if ( oldname == NULL ) { 1594 rtapi_mutex_give(&(hal_data->mutex)); 1595 rtapi_print_msg(RTAPI_MSG_ERR, 1596 "HAL: ERROR: insufficient memory for param_alias\n"); 1597 return -EINVAL; 1598 } 1599 free_oldname_struct(oldname); 1600 /* find the param and unlink it from pin list */ 1601 prev = &(hal_data->param_list_ptr); 1602 next = *prev; 1603 while (1) { 1604 if (next == 0) { 1605 /* reached end of list, not found */ 1606 rtapi_mutex_give(&(hal_data->mutex)); 1607 rtapi_print_msg(RTAPI_MSG_ERR, 1608 "HAL: ERROR: param '%s' not found\n", param_name); 1609 return -EINVAL; 1610 } 1611 param = SHMPTR(next); 1612 if ( strcmp(param->name, param_name) == 0 ) { 1613 /* found it, unlink from list */ 1614 *prev = param->next_ptr; 1615 break; 1616 } 1617 if (param->oldname != 0 ) { 1618 oldname = SHMPTR(param->oldname); 1619 if (strcmp(oldname->name, param_name) == 0) { 1620 /* found it, unlink from list */ 1621 *prev = param->next_ptr; 1622 break; 1623 } 1624 } 1625 /* didn't find it yet, look at next one */ 1626 prev = &(param->next_ptr); 1627 next = *prev; 1628 } 1629 if ( alias != NULL ) { 1630 /* adding a new alias */ 1631 if ( param->oldname == 0 ) { 1632 /* save old name (only if not already saved) */ 1633 oldname = halpr_alloc_oldname_struct(); 1634 param->oldname = SHMOFF(oldname); 1635 rtapi_snprintf(oldname->name, sizeof(oldname->name), "%s", param->name); 1636 } 1637 /* change param's name to 'alias' */ 1638 rtapi_snprintf(param->name, sizeof(param->name), "%s", alias); 1639 } else { 1640 /* removing an alias */ 1641 if ( param->oldname != 0 ) { 1642 /* restore old name (only if param is aliased) */ 1643 oldname = SHMPTR(param->oldname); 1644 rtapi_snprintf(param->name, sizeof(param->name), "%s", oldname->name); 1645 param->oldname = 0; 1646 free_oldname_struct(oldname); 1647 } 1648 } 1649 /* insert param back into list in proper place */ 1650 prev = &(hal_data->param_list_ptr); 1651 next = *prev; 1652 while (1) { 1653 if (next == 0) { 1654 /* reached end of list, insert here */ 1655 param->next_ptr = next; 1656 *prev = SHMOFF(param); 1657 rtapi_mutex_give(&(hal_data->mutex)); 1658 return 0; 1659 } 1660 ptr = SHMPTR(next); 1661 cmp = strcmp(ptr->name, param->name); 1662 if (cmp > 0) { 1663 /* found the right place for it, insert here */ 1664 param->next_ptr = next; 1665 *prev = SHMOFF(param); 1666 rtapi_mutex_give(&(hal_data->mutex)); 1667 return 0; 1668 } 1669 /* didn't find it yet, look at next one */ 1670 prev = &(ptr->next_ptr); 1671 next = *prev; 1672 } 1673 } 1674 1675 /*********************************************************************** 1676 * EXECUTION RELATED FUNCTIONS * 1677 ************************************************************************/ 1678 1679 #ifdef RTAPI 1680 1681 int hal_export_funct(const char *name, void (*funct) (void *, long), 1682 void *arg, int uses_fp, int reentrant, int comp_id) 1683 { 1684 rtapi_intptr_t *prev, next; 1685 int cmp; 1686 hal_funct_t *new, *fptr; 1687 hal_comp_t *comp; 1688 char buf[HAL_NAME_LEN + 1]; 1689 1690 if (hal_data == 0) { 1691 rtapi_print_msg(RTAPI_MSG_ERR, 1692 "HAL: ERROR: export_funct called before init\n"); 1693 return -EINVAL; 1694 } 1695 1696 if (strlen(name) > HAL_NAME_LEN) { 1697 rtapi_print_msg(RTAPI_MSG_ERR, 1698 "HAL: ERROR: function name '%s' is too long\n", name); 1699 return -EINVAL; 1700 } 1701 if (hal_data->lock & HAL_LOCK_LOAD) { 1702 rtapi_print_msg(RTAPI_MSG_ERR, 1703 "HAL: ERROR: export_funct called while HAL locked\n"); 1704 return -EPERM; 1705 } 1706 1707 rtapi_print_msg(RTAPI_MSG_DBG, "HAL: exporting function '%s'\n", name); 1708 /* get mutex before accessing shared data */ 1709 rtapi_mutex_get(&(hal_data->mutex)); 1710 /* validate comp_id */ 1711 comp = halpr_find_comp_by_id(comp_id); 1712 if (comp == 0) { 1713 /* bad comp_id */ 1714 rtapi_mutex_give(&(hal_data->mutex)); 1715 rtapi_print_msg(RTAPI_MSG_ERR, 1716 "HAL: ERROR: component %d not found\n", comp_id); 1717 return -EINVAL; 1718 } 1719 if (comp->type == 0) { 1720 /* not a realtime component */ 1721 rtapi_mutex_give(&(hal_data->mutex)); 1722 rtapi_print_msg(RTAPI_MSG_ERR, 1723 "HAL: ERROR: component %d is not realtime\n", comp_id); 1724 return -EINVAL; 1725 } 1726 if(comp->ready) { 1727 rtapi_mutex_give(&(hal_data->mutex)); 1728 rtapi_print_msg(RTAPI_MSG_ERR, 1729 "HAL: ERROR: export_funct called after hal_ready\n"); 1730 return -EINVAL; 1731 } 1732 /* allocate a new function structure */ 1733 new = alloc_funct_struct(); 1734 if (new == 0) { 1735 /* alloc failed */ 1736 rtapi_mutex_give(&(hal_data->mutex)); 1737 rtapi_print_msg(RTAPI_MSG_ERR, 1738 "HAL: ERROR: insufficient memory for function '%s'\n", name); 1739 return -ENOMEM; 1740 } 1741 /* initialize the structure */ 1742 new->uses_fp = uses_fp; 1743 new->owner_ptr = SHMOFF(comp); 1744 new->reentrant = reentrant; 1745 new->users = 0; 1746 new->arg = arg; 1747 new->funct = funct; 1748 rtapi_snprintf(new->name, sizeof(new->name), "%s", name); 1749 /* search list for 'name' and insert new structure */ 1750 prev = &(hal_data->funct_list_ptr); 1751 next = *prev; 1752 while (1) { 1753 if (next == 0) { 1754 /* reached end of list, insert here */ 1755 new->next_ptr = next; 1756 *prev = SHMOFF(new); 1757 /* break out of loop and init the new function */ 1758 break; 1759 } 1760 fptr = SHMPTR(next); 1761 cmp = strcmp(fptr->name, new->name); 1762 if (cmp > 0) { 1763 /* found the right place for it, insert here */ 1764 new->next_ptr = next; 1765 *prev = SHMOFF(new); 1766 /* break out of loop and init the new function */ 1767 break; 1768 } 1769 if (cmp == 0) { 1770 /* name already in list, can't insert */ 1771 free_funct_struct(new); 1772 rtapi_mutex_give(&(hal_data->mutex)); 1773 rtapi_print_msg(RTAPI_MSG_ERR, 1774 "HAL: ERROR: duplicate function '%s'\n", name); 1775 return -EINVAL; 1776 } 1777 /* didn't find it yet, look at next one */ 1778 prev = &(fptr->next_ptr); 1779 next = *prev; 1780 } 1781 /* at this point we have a new function and can yield the mutex */ 1782 rtapi_mutex_give(&(hal_data->mutex)); 1783 1784 /* create a pin with the function's runtime in it */ 1785 if (hal_pin_s32_newf(HAL_OUT, &(new->runtime), comp_id,"%s.time",name)) { 1786 rtapi_print_msg(RTAPI_MSG_ERR, 1787 "HAL: ERROR: fail to create pin '%s.time'\n", name); 1788 return -EINVAL; 1789 } 1790 *(new->runtime) = 0; 1791 1792 /* note that failure to successfully create the following params 1793 does not cause the "export_funct()" call to fail - they are 1794 for debugging and testing use only */ 1795 /* create a parameter with the function's maximum runtime in it */ 1796 rtapi_snprintf(buf, sizeof(buf), "%s.tmax", name); 1797 new->maxtime = 0; 1798 hal_param_s32_new(buf, HAL_RW, &(new->maxtime), comp_id); 1799 1800 /* create a parameter with the function's maximum runtime in it */ 1801 rtapi_snprintf(buf, sizeof(buf), "%s.tmax-increased", name); 1802 new->maxtime_increased = 0; 1803 hal_param_bit_new(buf, HAL_RO, &(new->maxtime_increased), comp_id); 1804 1805 return 0; 1806 } 1807 1808 int hal_create_thread(const char *name, unsigned long period_nsec, int uses_fp) 1809 { 1810 int next, cmp, prev_priority; 1811 int retval, n; 1812 hal_thread_t *new, *tptr; 1813 long prev_period, curr_period; 1814 char buf[HAL_NAME_LEN + 1]; 1815 1816 rtapi_print_msg(RTAPI_MSG_DBG, 1817 "HAL: creating thread %s, %ld nsec\n", name, period_nsec); 1818 if (hal_data == 0) { 1819 rtapi_print_msg(RTAPI_MSG_ERR, 1820 "HAL: ERROR: create_thread called before init\n"); 1821 return -EINVAL; 1822 } 1823 if (period_nsec == 0) { 1824 rtapi_print_msg(RTAPI_MSG_ERR, 1825 "HAL: ERROR: create_thread called with period of zero\n"); 1826 return -EINVAL; 1827 } 1828 1829 if (strlen(name) > HAL_NAME_LEN) { 1830 rtapi_print_msg(RTAPI_MSG_ERR, 1831 "HAL: ERROR: thread name '%s' is too long\n", name); 1832 return -EINVAL; 1833 } 1834 if (hal_data->lock & HAL_LOCK_CONFIG) { 1835 rtapi_print_msg(RTAPI_MSG_ERR, 1836 "HAL: ERROR: create_thread called while HAL is locked\n"); 1837 return -EPERM; 1838 } 1839 1840 /* get mutex before accessing shared data */ 1841 rtapi_mutex_get(&(hal_data->mutex)); 1842 /* make sure name is unique on thread list */ 1843 next = hal_data->thread_list_ptr; 1844 while (next != 0) { 1845 tptr = SHMPTR(next); 1846 cmp = strcmp(tptr->name, name); 1847 if (cmp == 0) { 1848 /* name already in list, can't insert */ 1849 rtapi_mutex_give(&(hal_data->mutex)); 1850 rtapi_print_msg(RTAPI_MSG_ERR, 1851 "HAL: ERROR: duplicate thread name %s\n", name); 1852 return -EINVAL; 1853 } 1854 /* didn't find it yet, look at next one */ 1855 next = tptr->next_ptr; 1856 } 1857 /* allocate a new thread structure */ 1858 new = alloc_thread_struct(); 1859 if (new == 0) { 1860 /* alloc failed */ 1861 rtapi_mutex_give(&(hal_data->mutex)); 1862 rtapi_print_msg(RTAPI_MSG_ERR, 1863 "HAL: ERROR: insufficient memory to create thread\n"); 1864 return -ENOMEM; 1865 } 1866 /* initialize the structure */ 1867 new->uses_fp = uses_fp; 1868 rtapi_snprintf(new->name, sizeof(new->name), "%s", name); 1869 /* have to create and start a task to run the thread */ 1870 if (hal_data->thread_list_ptr == 0) { 1871 /* this is the first thread created */ 1872 /* is timer started? if so, what period? */ 1873 curr_period = rtapi_clock_set_period(0); 1874 if (curr_period == 0) { 1875 /* not running, start it */ 1876 curr_period = rtapi_clock_set_period(period_nsec); 1877 if (curr_period < 0) { 1878 rtapi_mutex_give(&(hal_data->mutex)); 1879 rtapi_print_msg(RTAPI_MSG_ERR, 1880 "HAL_LIB: ERROR: clock_set_period returned %ld\n", 1881 curr_period); 1882 return -EINVAL; 1883 } 1884 } 1885 /* make sure period <= desired period (allow 1% roundoff error) */ 1886 if (curr_period > (period_nsec + (period_nsec / 100))) { 1887 rtapi_mutex_give(&(hal_data->mutex)); 1888 rtapi_print_msg(RTAPI_MSG_ERR, 1889 "HAL_LIB: ERROR: clock period too long: %ld\n", curr_period); 1890 return -EINVAL; 1891 } 1892 if(hal_data->exact_base_period) { 1893 hal_data->base_period = period_nsec; 1894 } else { 1895 hal_data->base_period = curr_period; 1896 } 1897 /* reserve the highest priority (maybe for a watchdog?) */ 1898 prev_priority = rtapi_prio_highest(); 1899 /* no previous period to worry about */ 1900 prev_period = 0; 1901 } else { 1902 /* there are other threads, slowest (and lowest 1903 priority) is at head of list */ 1904 tptr = SHMPTR(hal_data->thread_list_ptr); 1905 prev_period = tptr->period; 1906 prev_priority = tptr->priority; 1907 } 1908 if ( period_nsec < hal_data->base_period) { 1909 rtapi_mutex_give(&(hal_data->mutex)); 1910 rtapi_print_msg(RTAPI_MSG_ERR, 1911 "HAL_LIB: ERROR: new thread period %ld is less than clock period %ld\n", 1912 period_nsec, hal_data->base_period); 1913 return -EINVAL; 1914 } 1915 /* make period an integer multiple of the timer period */ 1916 n = (period_nsec + hal_data->base_period / 2) / hal_data->base_period; 1917 new->period = hal_data->base_period * n; 1918 if ( new->period < prev_period ) { 1919 rtapi_mutex_give(&(hal_data->mutex)); 1920 rtapi_print_msg(RTAPI_MSG_ERR, 1921 "HAL_LIB: ERROR: new thread period %ld is less than existing thread period %ld\n", 1922 period_nsec, prev_period); 1923 return -EINVAL; 1924 } 1925 /* make priority one lower than previous */ 1926 new->priority = rtapi_prio_next_lower(prev_priority); 1927 /* create task - owned by library module, not caller */ 1928 retval = rtapi_task_new(thread_task, new, new->priority, 1929 lib_module_id, HAL_STACKSIZE, uses_fp); 1930 if (retval < 0) { 1931 rtapi_mutex_give(&(hal_data->mutex)); 1932 rtapi_print_msg(RTAPI_MSG_ERR, 1933 "HAL_LIB: could not create task for thread %s\n", name); 1934 return -EINVAL; 1935 } 1936 new->task_id = retval; 1937 /* start task */ 1938 retval = rtapi_task_start(new->task_id, new->period); 1939 if (retval < 0) { 1940 rtapi_mutex_give(&(hal_data->mutex)); 1941 rtapi_print_msg(RTAPI_MSG_ERR, 1942 "HAL_LIB: could not start task for thread %s: %d\n", name, retval); 1943 return -EINVAL; 1944 } 1945 /* insert new structure at head of list */ 1946 new->next_ptr = hal_data->thread_list_ptr; 1947 hal_data->thread_list_ptr = SHMOFF(new); 1948 /* done, release mutex */ 1949 rtapi_mutex_give(&(hal_data->mutex)); 1950 1951 rtapi_snprintf(buf,sizeof(buf), HAL_PSEUDO_COMP_PREFIX"%s",new->name); // pseudo prefix 1952 new->comp_id = hal_init(buf); 1953 if (new->comp_id < 0) { 1954 rtapi_print_msg(RTAPI_MSG_ERR, 1955 "HAL: ERROR: fail to create pseudo comp for thread: '%s'\n", new->name); 1956 return -EINVAL; 1957 } 1958 1959 rtapi_snprintf(buf, sizeof(buf), "%s.tmax", new->name); 1960 new->maxtime = 0; 1961 if (hal_param_s32_new(buf, HAL_RW, &(new->maxtime), new->comp_id)) { 1962 rtapi_print_msg(RTAPI_MSG_ERR, 1963 "HAL: ERROR: fail to create param '%s.tmax'\n", new->name); 1964 return -EINVAL; 1965 } 1966 1967 if (hal_pin_s32_newf(HAL_OUT, &(new->runtime), new->comp_id,"%s.time",new->name)) { 1968 rtapi_print_msg(RTAPI_MSG_ERR, 1969 "HAL: ERROR: fail to create pin '%s.time'\n", new->name); 1970 return -EINVAL; 1971 } 1972 *(new->runtime) = 0; 1973 hal_ready(new->comp_id); 1974 1975 rtapi_print_msg(RTAPI_MSG_DBG, "HAL: thread created\n"); 1976 return 0; 1977 } 1978 1979 extern int hal_thread_delete(const char *name) 1980 { 1981 hal_thread_t *thread; 1982 rtapi_intptr_t *prev, next; 1983 1984 if (hal_data == 0) { 1985 rtapi_print_msg(RTAPI_MSG_ERR, 1986 "HAL: ERROR: thread_delete called before init\n"); 1987 return -EINVAL; 1988 } 1989 1990 if (hal_data->lock & HAL_LOCK_CONFIG) { 1991 rtapi_print_msg(RTAPI_MSG_ERR, 1992 "HAL: ERROR: thread_delete called while HAL is locked\n"); 1993 return -EPERM; 1994 } 1995 1996 rtapi_print_msg(RTAPI_MSG_DBG, "HAL: deleting thread '%s'\n", name); 1997 /* get mutex before accessing shared data */ 1998 rtapi_mutex_get(&(hal_data->mutex)); 1999 /* search for the signal */ 2000 prev = &(hal_data->thread_list_ptr); 2001 next = *prev; 2002 while (next != 0) { 2003 thread = SHMPTR(next); 2004 if (strcmp(thread->name, name) == 0) { 2005 /* this is the right thread, unlink from list */ 2006 if (thread->comp_id != 0) { 2007 hal_exit(thread->comp_id); 2008 thread->comp_id = 0; 2009 } 2010 *prev = thread->next_ptr; 2011 /* and delete it */ 2012 free_thread_struct(thread); 2013 /* done */ 2014 rtapi_mutex_give(&(hal_data->mutex)); 2015 return 0; 2016 } 2017 /* no match, try the next one */ 2018 prev = &(thread->next_ptr); 2019 next = *prev; 2020 } 2021 /* if we get here, we didn't find a match */ 2022 rtapi_mutex_give(&(hal_data->mutex)); 2023 rtapi_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: thread '%s' not found\n", 2024 name); 2025 return -EINVAL; 2026 } 2027 2028 #endif /* RTAPI */ 2029 2030 int hal_add_funct_to_thread(const char *funct_name, const char *thread_name, int position) 2031 { 2032 hal_thread_t *thread; 2033 hal_funct_t *funct; 2034 hal_list_t *list_root, *list_entry; 2035 int n; 2036 hal_funct_entry_t *funct_entry; 2037 2038 if (hal_data == 0) { 2039 rtapi_print_msg(RTAPI_MSG_ERR, 2040 "HAL: ERROR: add_funct called before init\n"); 2041 return -EINVAL; 2042 } 2043 2044 if (hal_data->lock & HAL_LOCK_CONFIG) { 2045 rtapi_print_msg(RTAPI_MSG_ERR, 2046 "HAL: ERROR: add_funct_to_thread called while HAL is locked\n"); 2047 return -EPERM; 2048 } 2049 2050 rtapi_print_msg(RTAPI_MSG_DBG, 2051 "HAL: adding function '%s' to thread '%s'\n", 2052 funct_name, thread_name); 2053 /* get mutex before accessing data structures */ 2054 rtapi_mutex_get(&(hal_data->mutex)); 2055 /* make sure position is valid */ 2056 if (position == 0) { 2057 /* zero is not allowed */ 2058 rtapi_mutex_give(&(hal_data->mutex)); 2059 rtapi_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: bad position: 0\n"); 2060 return -EINVAL; 2061 } 2062 /* make sure we were given a function name */ 2063 if (funct_name == 0) { 2064 /* no name supplied */ 2065 rtapi_mutex_give(&(hal_data->mutex)); 2066 rtapi_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: missing function name\n"); 2067 return -EINVAL; 2068 } 2069 /* make sure we were given a thread name */ 2070 if (thread_name == 0) { 2071 /* no name supplied */ 2072 rtapi_mutex_give(&(hal_data->mutex)); 2073 rtapi_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: missing thread name\n"); 2074 return -EINVAL; 2075 } 2076 /* search function list for the function */ 2077 funct = halpr_find_funct_by_name(funct_name); 2078 if (funct == 0) { 2079 /* function not found */ 2080 rtapi_mutex_give(&(hal_data->mutex)); 2081 rtapi_print_msg(RTAPI_MSG_ERR, 2082 "HAL: ERROR: function '%s' not found\n", funct_name); 2083 return -EINVAL; 2084 } 2085 /* found the function, is it available? */ 2086 if ((funct->users > 0) && (funct->reentrant == 0)) { 2087 rtapi_mutex_give(&(hal_data->mutex)); 2088 rtapi_print_msg(RTAPI_MSG_ERR, 2089 "HAL: ERROR: function '%s' may only be added to one thread\n", funct_name); 2090 return -EINVAL; 2091 } 2092 /* search thread list for thread_name */ 2093 thread = halpr_find_thread_by_name(thread_name); 2094 if (thread == 0) { 2095 /* thread not found */ 2096 rtapi_mutex_give(&(hal_data->mutex)); 2097 rtapi_print_msg(RTAPI_MSG_ERR, 2098 "HAL: ERROR: thread '%s' not found\n", thread_name); 2099 return -EINVAL; 2100 } 2101 /* ok, we have thread and function, are they compatible? */ 2102 if ((funct->uses_fp) && (!thread->uses_fp)) { 2103 rtapi_mutex_give(&(hal_data->mutex)); 2104 rtapi_print_msg(RTAPI_MSG_ERR, 2105 "HAL: ERROR: function '%s' needs FP\n", funct_name); 2106 return -EINVAL; 2107 } 2108 /* find insertion point */ 2109 list_root = &(thread->funct_list); 2110 list_entry = list_root; 2111 n = 0; 2112 if (position > 0) { 2113 /* insertion is relative to start of list */ 2114 while (++n < position) { 2115 /* move further into list */ 2116 list_entry = list_next(list_entry); 2117 if (list_entry == list_root) { 2118 /* reached end of list */ 2119 rtapi_mutex_give(&(hal_data->mutex)); 2120 rtapi_print_msg(RTAPI_MSG_ERR, 2121 "HAL: ERROR: position '%d' is too high\n", position); 2122 return -EINVAL; 2123 } 2124 } 2125 } else { 2126 /* insertion is relative to end of list */ 2127 while (--n > position) { 2128 /* move further into list */ 2129 list_entry = list_prev(list_entry); 2130 if (list_entry == list_root) { 2131 /* reached end of list */ 2132 rtapi_mutex_give(&(hal_data->mutex)); 2133 rtapi_print_msg(RTAPI_MSG_ERR, 2134 "HAL: ERROR: position '%d' is too low\n", position); 2135 return -EINVAL; 2136 } 2137 } 2138 /* want to insert before list_entry, so back up one more step */ 2139 list_entry = list_prev(list_entry); 2140 } 2141 /* allocate a funct entry structure */ 2142 funct_entry = alloc_funct_entry_struct(); 2143 if (funct_entry == 0) { 2144 /* alloc failed */ 2145 rtapi_mutex_give(&(hal_data->mutex)); 2146 rtapi_print_msg(RTAPI_MSG_ERR, 2147 "HAL: ERROR: insufficient memory for thread->function link\n"); 2148 return -ENOMEM; 2149 } 2150 /* init struct contents */ 2151 funct_entry->funct_ptr = SHMOFF(funct); 2152 funct_entry->arg = funct->arg; 2153 funct_entry->funct = funct->funct; 2154 /* add the entry to the list */ 2155 list_add_after((hal_list_t *) funct_entry, list_entry); 2156 /* update the function usage count */ 2157 funct->users++; 2158 rtapi_mutex_give(&(hal_data->mutex)); 2159 return 0; 2160 } 2161 2162 int hal_del_funct_from_thread(const char *funct_name, const char *thread_name) 2163 { 2164 hal_thread_t *thread; 2165 hal_funct_t *funct; 2166 hal_list_t *list_root, *list_entry; 2167 hal_funct_entry_t *funct_entry; 2168 2169 if (hal_data == 0) { 2170 rtapi_print_msg(RTAPI_MSG_ERR, 2171 "HAL: ERROR: del_funct called before init\n"); 2172 return -EINVAL; 2173 } 2174 2175 if (hal_data->lock & HAL_LOCK_CONFIG) { 2176 rtapi_print_msg(RTAPI_MSG_ERR, 2177 "HAL: ERROR: del_funct_from_thread called while HAL is locked\n"); 2178 return -EPERM; 2179 } 2180 2181 rtapi_print_msg(RTAPI_MSG_DBG, 2182 "HAL: removing function '%s' from thread '%s'\n", 2183 funct_name, thread_name); 2184 /* get mutex before accessing data structures */ 2185 rtapi_mutex_get(&(hal_data->mutex)); 2186 /* make sure we were given a function name */ 2187 if (funct_name == 0) { 2188 /* no name supplied */ 2189 rtapi_mutex_give(&(hal_data->mutex)); 2190 rtapi_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: missing function name\n"); 2191 return -EINVAL; 2192 } 2193 /* make sure we were given a thread name */ 2194 if (thread_name == 0) { 2195 /* no name supplied */ 2196 rtapi_mutex_give(&(hal_data->mutex)); 2197 rtapi_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: missing thread name\n"); 2198 return -EINVAL; 2199 } 2200 /* search function list for the function */ 2201 funct = halpr_find_funct_by_name(funct_name); 2202 if (funct == 0) { 2203 /* function not found */ 2204 rtapi_mutex_give(&(hal_data->mutex)); 2205 rtapi_print_msg(RTAPI_MSG_ERR, 2206 "HAL: ERROR: function '%s' not found\n", funct_name); 2207 return -EINVAL; 2208 } 2209 /* found the function, is it in use? */ 2210 if (funct->users == 0) { 2211 rtapi_mutex_give(&(hal_data->mutex)); 2212 rtapi_print_msg(RTAPI_MSG_ERR, 2213 "HAL: ERROR: function '%s' is not in use\n", funct_name); 2214 return -EINVAL; 2215 } 2216 /* search thread list for thread_name */ 2217 thread = halpr_find_thread_by_name(thread_name); 2218 if (thread == 0) { 2219 /* thread not found */ 2220 rtapi_mutex_give(&(hal_data->mutex)); 2221 rtapi_print_msg(RTAPI_MSG_ERR, 2222 "HAL: ERROR: thread '%s' not found\n", thread_name); 2223 return -EINVAL; 2224 } 2225 /* ok, we have thread and function, does thread use funct? */ 2226 list_root = &(thread->funct_list); 2227 list_entry = list_next(list_root); 2228 while (1) { 2229 if (list_entry == list_root) { 2230 /* reached end of list, funct not found */ 2231 rtapi_mutex_give(&(hal_data->mutex)); 2232 rtapi_print_msg(RTAPI_MSG_ERR, 2233 "HAL: ERROR: thread '%s' doesn't use %s\n", thread_name, 2234 funct_name); 2235 return -EINVAL; 2236 } 2237 funct_entry = (hal_funct_entry_t *) list_entry; 2238 if (SHMPTR(funct_entry->funct_ptr) == funct) { 2239 /* this funct entry points to our funct, unlink */ 2240 list_remove_entry(list_entry); 2241 /* and delete it */ 2242 free_funct_entry_struct(funct_entry); 2243 /* done */ 2244 rtapi_mutex_give(&(hal_data->mutex)); 2245 return 0; 2246 } 2247 /* try next one */ 2248 list_entry = list_next(list_entry); 2249 } 2250 } 2251 2252 int hal_start_threads(void) 2253 { 2254 /* a trivial function for a change! */ 2255 if (hal_data == 0) { 2256 rtapi_print_msg(RTAPI_MSG_ERR, 2257 "HAL: ERROR: start_threads called before init\n"); 2258 return -EINVAL; 2259 } 2260 2261 if (hal_data->lock & HAL_LOCK_RUN) { 2262 rtapi_print_msg(RTAPI_MSG_ERR, 2263 "HAL: ERROR: start_threads called while HAL is locked\n"); 2264 return -EPERM; 2265 } 2266 2267 2268 rtapi_print_msg(RTAPI_MSG_DBG, "HAL: starting threads\n"); 2269 hal_data->threads_running = 1; 2270 return 0; 2271 } 2272 2273 int hal_stop_threads(void) 2274 { 2275 /* wow, two in a row! */ 2276 if (hal_data == 0) { 2277 rtapi_print_msg(RTAPI_MSG_ERR, 2278 "HAL: ERROR: stop_threads called before init\n"); 2279 return -EINVAL; 2280 } 2281 2282 if (hal_data->lock & HAL_LOCK_RUN) { 2283 rtapi_print_msg(RTAPI_MSG_ERR, 2284 "HAL: ERROR: stop_threads called while HAL is locked\n"); 2285 return -EPERM; 2286 } 2287 2288 hal_data->threads_running = 0; 2289 rtapi_print_msg(RTAPI_MSG_DBG, "HAL: threads stopped\n"); 2290 return 0; 2291 } 2292 2293 /*********************************************************************** 2294 * PRIVATE FUNCTION CODE * 2295 ************************************************************************/ 2296 2297 hal_list_t *list_prev(hal_list_t * entry) 2298 { 2299 /* this function is only needed because of memory mapping */ 2300 return SHMPTR(entry->prev); 2301 } 2302 2303 hal_list_t *list_next(hal_list_t * entry) 2304 { 2305 /* this function is only needed because of memory mapping */ 2306 return SHMPTR(entry->next); 2307 } 2308 2309 void list_init_entry(hal_list_t * entry) 2310 { 2311 int entry_n; 2312 2313 entry_n = SHMOFF(entry); 2314 entry->next = entry_n; 2315 entry->prev = entry_n; 2316 } 2317 2318 void list_add_after(hal_list_t * entry, hal_list_t * prev) 2319 { 2320 int entry_n, prev_n, next_n; 2321 hal_list_t *next; 2322 2323 /* messiness needed because of memory mapping */ 2324 entry_n = SHMOFF(entry); 2325 prev_n = SHMOFF(prev); 2326 next_n = prev->next; 2327 next = SHMPTR(next_n); 2328 /* insert the entry */ 2329 entry->next = next_n; 2330 entry->prev = prev_n; 2331 prev->next = entry_n; 2332 next->prev = entry_n; 2333 } 2334 2335 void list_add_before(hal_list_t * entry, hal_list_t * next) 2336 { 2337 int entry_n, prev_n, next_n; 2338 hal_list_t *prev; 2339 2340 /* messiness needed because of memory mapping */ 2341 entry_n = SHMOFF(entry); 2342 next_n = SHMOFF(next); 2343 prev_n = next->prev; 2344 prev = SHMPTR(prev_n); 2345 /* insert the entry */ 2346 entry->next = next_n; 2347 entry->prev = prev_n; 2348 prev->next = entry_n; 2349 next->prev = entry_n; 2350 } 2351 2352 hal_list_t *list_remove_entry(hal_list_t * entry) 2353 { 2354 int entry_n; 2355 hal_list_t *prev, *next; 2356 2357 /* messiness needed because of memory mapping */ 2358 entry_n = SHMOFF(entry); 2359 prev = SHMPTR(entry->prev); 2360 next = SHMPTR(entry->next); 2361 /* remove the entry */ 2362 prev->next = entry->next; 2363 next->prev = entry->prev; 2364 entry->next = entry_n; 2365 entry->prev = entry_n; 2366 return next; 2367 } 2368 2369 hal_comp_t *halpr_find_comp_by_name(const char *name) 2370 { 2371 int next; 2372 hal_comp_t *comp; 2373 2374 /* search component list for 'name' */ 2375 next = hal_data->comp_list_ptr; 2376 while (next != 0) { 2377 comp = SHMPTR(next); 2378 if (strcmp(comp->name, name) == 0) { 2379 /* found a match */ 2380 return comp; 2381 } 2382 /* didn't find it yet, look at next one */ 2383 next = comp->next_ptr; 2384 } 2385 /* if loop terminates, we reached end of list with no match */ 2386 return 0; 2387 } 2388 2389 hal_pin_t *halpr_find_pin_by_name(const char *name) 2390 { 2391 int next; 2392 hal_pin_t *pin; 2393 hal_oldname_t *oldname; 2394 2395 /* search pin list for 'name' */ 2396 next = hal_data->pin_list_ptr; 2397 while (next != 0) { 2398 pin = SHMPTR(next); 2399 if (strcmp(pin->name, name) == 0) { 2400 /* found a match */ 2401 return pin; 2402 } 2403 if (pin->oldname != 0 ) { 2404 oldname = SHMPTR(pin->oldname); 2405 if (strcmp(oldname->name, name) == 0) { 2406 /* found a match */ 2407 return pin; 2408 } 2409 } 2410 /* didn't find it yet, look at next one */ 2411 next = pin->next_ptr; 2412 } 2413 /* if loop terminates, we reached end of list with no match */ 2414 return 0; 2415 } 2416 2417 hal_sig_t *halpr_find_sig_by_name(const char *name) 2418 { 2419 int next; 2420 hal_sig_t *sig; 2421 2422 /* search signal list for 'name' */ 2423 next = hal_data->sig_list_ptr; 2424 while (next != 0) { 2425 sig = SHMPTR(next); 2426 if (strcmp(sig->name, name) == 0) { 2427 /* found a match */ 2428 return sig; 2429 } 2430 /* didn't find it yet, look at next one */ 2431 next = sig->next_ptr; 2432 } 2433 /* if loop terminates, we reached end of list with no match */ 2434 return 0; 2435 } 2436 2437 hal_param_t *halpr_find_param_by_name(const char *name) 2438 { 2439 int next; 2440 hal_param_t *param; 2441 hal_oldname_t *oldname; 2442 2443 /* search parameter list for 'name' */ 2444 next = hal_data->param_list_ptr; 2445 while (next != 0) { 2446 param = SHMPTR(next); 2447 if (strcmp(param->name, name) == 0) { 2448 /* found a match */ 2449 return param; 2450 } 2451 if (param->oldname != 0 ) { 2452 oldname = SHMPTR(param->oldname); 2453 if (strcmp(oldname->name, name) == 0) { 2454 /* found a match */ 2455 return param; 2456 } 2457 } 2458 /* didn't find it yet, look at next one */ 2459 next = param->next_ptr; 2460 } 2461 /* if loop terminates, we reached end of list with no match */ 2462 return 0; 2463 } 2464 2465 hal_thread_t *halpr_find_thread_by_name(const char *name) 2466 { 2467 int next; 2468 hal_thread_t *thread; 2469 2470 /* search thread list for 'name' */ 2471 next = hal_data->thread_list_ptr; 2472 while (next != 0) { 2473 thread = SHMPTR(next); 2474 if (strcmp(thread->name, name) == 0) { 2475 /* found a match */ 2476 return thread; 2477 } 2478 /* didn't find it yet, look at next one */ 2479 next = thread->next_ptr; 2480 } 2481 /* if loop terminates, we reached end of list with no match */ 2482 return 0; 2483 } 2484 2485 hal_funct_t *halpr_find_funct_by_name(const char *name) 2486 { 2487 int next; 2488 hal_funct_t *funct; 2489 2490 /* search function list for 'name' */ 2491 next = hal_data->funct_list_ptr; 2492 while (next != 0) { 2493 funct = SHMPTR(next); 2494 if (strcmp(funct->name, name) == 0) { 2495 /* found a match */ 2496 return funct; 2497 } 2498 /* didn't find it yet, look at next one */ 2499 next = funct->next_ptr; 2500 } 2501 /* if loop terminates, we reached end of list with no match */ 2502 return 0; 2503 } 2504 2505 hal_comp_t *halpr_find_comp_by_id(int id) 2506 { 2507 int next; 2508 hal_comp_t *comp; 2509 2510 /* search list for 'comp_id' */ 2511 next = hal_data->comp_list_ptr; 2512 while (next != 0) { 2513 comp = SHMPTR(next); 2514 if (comp->comp_id == id) { 2515 /* found a match */ 2516 return comp; 2517 } 2518 /* didn't find it yet, look at next one */ 2519 next = comp->next_ptr; 2520 } 2521 /* if loop terminates, we reached end of list without finding a match */ 2522 return 0; 2523 } 2524 2525 hal_pin_t *halpr_find_pin_by_owner(hal_comp_t * owner, hal_pin_t * start) 2526 { 2527 int owner_ptr, next; 2528 hal_pin_t *pin; 2529 2530 /* get offset of 'owner' component */ 2531 owner_ptr = SHMOFF(owner); 2532 /* is this the first call? */ 2533 if (start == 0) { 2534 /* yes, start at beginning of pin list */ 2535 next = hal_data->pin_list_ptr; 2536 } else { 2537 /* no, start at next pin */ 2538 next = start->next_ptr; 2539 } 2540 while (next != 0) { 2541 pin = SHMPTR(next); 2542 if (pin->owner_ptr == owner_ptr) { 2543 /* found a match */ 2544 return pin; 2545 } 2546 /* didn't find it yet, look at next one */ 2547 next = pin->next_ptr; 2548 } 2549 /* if loop terminates, we reached end of list without finding a match */ 2550 return 0; 2551 } 2552 2553 hal_param_t *halpr_find_param_by_owner(hal_comp_t * owner, 2554 hal_param_t * start) 2555 { 2556 int owner_ptr, next; 2557 hal_param_t *param; 2558 2559 /* get offset of 'owner' component */ 2560 owner_ptr = SHMOFF(owner); 2561 /* is this the first call? */ 2562 if (start == 0) { 2563 /* yes, start at beginning of param list */ 2564 next = hal_data->param_list_ptr; 2565 } else { 2566 /* no, start at next param */ 2567 next = start->next_ptr; 2568 } 2569 while (next != 0) { 2570 param = SHMPTR(next); 2571 if (param->owner_ptr == owner_ptr) { 2572 /* found a match */ 2573 return param; 2574 } 2575 /* didn't find it yet, look at next one */ 2576 next = param->next_ptr; 2577 } 2578 /* if loop terminates, we reached end of list without finding a match */ 2579 return 0; 2580 } 2581 2582 hal_funct_t *halpr_find_funct_by_owner(hal_comp_t * owner, 2583 hal_funct_t * start) 2584 { 2585 int owner_ptr, next; 2586 hal_funct_t *funct; 2587 2588 /* get offset of 'owner' component */ 2589 owner_ptr = SHMOFF(owner); 2590 /* is this the first call? */ 2591 if (start == 0) { 2592 /* yes, start at beginning of function list */ 2593 next = hal_data->funct_list_ptr; 2594 } else { 2595 /* no, start at next function */ 2596 next = start->next_ptr; 2597 } 2598 while (next != 0) { 2599 funct = SHMPTR(next); 2600 if (funct->owner_ptr == owner_ptr) { 2601 /* found a match */ 2602 return funct; 2603 } 2604 /* didn't find it yet, look at next one */ 2605 next = funct->next_ptr; 2606 } 2607 /* if loop terminates, we reached end of list without finding a match */ 2608 return 0; 2609 } 2610 2611 hal_pin_t *halpr_find_pin_by_sig(hal_sig_t * sig, hal_pin_t * start) 2612 { 2613 int sig_ptr, next; 2614 hal_pin_t *pin; 2615 2616 /* get offset of 'sig' component */ 2617 sig_ptr = SHMOFF(sig); 2618 /* is this the first call? */ 2619 if (start == 0) { 2620 /* yes, start at beginning of pin list */ 2621 next = hal_data->pin_list_ptr; 2622 } else { 2623 /* no, start at next pin */ 2624 next = start->next_ptr; 2625 } 2626 while (next != 0) { 2627 pin = SHMPTR(next); 2628 if (pin->signal == sig_ptr) { 2629 /* found a match */ 2630 return pin; 2631 } 2632 /* didn't find it yet, look at next one */ 2633 next = pin->next_ptr; 2634 } 2635 /* if loop terminates, we reached end of list without finding a match */ 2636 return 0; 2637 } 2638 2639 /*********************************************************************** 2640 * LOCAL FUNCTION CODE * 2641 ************************************************************************/ 2642 2643 #ifdef RTAPI 2644 /* these functions are called when the hal_lib module is insmod'ed 2645 or rmmod'ed. 2646 */ 2647 2648 #if defined(__KERNEL__) && defined( CONFIG_PROC_FS ) && LINUX_VERSION_CODE < KERNEL_VERSION(3,10,0) 2649 #include <linux/proc_fs.h> 2650 extern struct proc_dir_entry *rtapi_dir; 2651 static struct proc_dir_entry *hal_dir = 0; 2652 static struct proc_dir_entry *hal_newinst_file = 0; 2653 2654 static int proc_write_newinst(struct file *file, 2655 const char *buffer, unsigned long count, void *data) 2656 { 2657 if(hal_data->pending_constructor) { 2658 rtapi_print_msg(RTAPI_MSG_DBG, 2659 "HAL: running constructor for %s %s\n", 2660 hal_data->constructor_prefix, 2661 hal_data->constructor_arg); 2662 hal_data->pending_constructor(hal_data->constructor_prefix, 2663 hal_data->constructor_arg); 2664 hal_data->pending_constructor = 0; 2665 } 2666 return count; 2667 } 2668 2669 static void hal_proc_clean(void) { 2670 if(hal_newinst_file) 2671 remove_proc_entry("newinst", hal_dir); 2672 if(hal_dir) 2673 remove_proc_entry("hal", rtapi_dir); 2674 hal_newinst_file = hal_dir = 0; 2675 } 2676 static int hal_proc_init(void) { 2677 if(!rtapi_dir) return 0; 2678 hal_dir = create_proc_entry("hal", S_IFDIR, rtapi_dir); 2679 if(!hal_dir) { hal_proc_clean(); return -1; } 2680 hal_newinst_file = create_proc_entry("newinst", 0666, hal_dir); 2681 if(!hal_newinst_file) { hal_proc_clean(); return -1; } 2682 hal_newinst_file->data = NULL; 2683 hal_newinst_file->read_proc = NULL; 2684 hal_newinst_file->write_proc = proc_write_newinst; 2685 return 0; 2686 } 2687 #else 2688 static int hal_proc_clean(void) { return 0; } 2689 static int hal_proc_init(void) { return 0; } 2690 #endif 2691 2692 int rtapi_app_main(void) 2693 { 2694 int retval; 2695 void *mem; 2696 2697 rtapi_print_msg(RTAPI_MSG_DBG, "HAL_LIB: loading kernel lib\n"); 2698 /* do RTAPI init */ 2699 lib_module_id = rtapi_init("HAL_LIB"); 2700 if (lib_module_id < 0) { 2701 rtapi_print_msg(RTAPI_MSG_ERR, "HAL_LIB: ERROR: rtapi init failed\n"); 2702 return -EINVAL; 2703 } 2704 /* get HAL shared memory block from RTAPI */ 2705 lib_mem_id = rtapi_shmem_new(HAL_KEY, lib_module_id, HAL_SIZE); 2706 if (lib_mem_id < 0) { 2707 rtapi_print_msg(RTAPI_MSG_ERR, 2708 "HAL_LIB: ERROR: could not open shared memory\n"); 2709 rtapi_exit(lib_module_id); 2710 return -EINVAL; 2711 } 2712 /* get address of shared memory area */ 2713 retval = rtapi_shmem_getptr(lib_mem_id, &mem); 2714 if (retval < 0) { 2715 rtapi_print_msg(RTAPI_MSG_ERR, 2716 "HAL_LIB: ERROR: could not access shared memory\n"); 2717 rtapi_exit(lib_module_id); 2718 return -EINVAL; 2719 } 2720 /* set up internal pointers to shared mem and data structure */ 2721 hal_shmem_base = (char *) mem; 2722 hal_data = (hal_data_t *) mem; 2723 /* perform a global init if needed */ 2724 retval = init_hal_data(); 2725 if ( retval ) { 2726 rtapi_print_msg(RTAPI_MSG_ERR, 2727 "HAL_LIB: ERROR: could not init shared memory\n"); 2728 rtapi_exit(lib_module_id); 2729 return -EINVAL; 2730 } 2731 retval = hal_proc_init(); 2732 if ( retval ) { 2733 rtapi_print_msg(RTAPI_MSG_ERR, 2734 "HAL_LIB: ERROR: could not init /proc files\n"); 2735 rtapi_exit(lib_module_id); 2736 return -EINVAL; 2737 } 2738 /* done */ 2739 rtapi_print_msg(RTAPI_MSG_DBG, 2740 "HAL_LIB: kernel lib installed successfully\n"); 2741 return 0; 2742 } 2743 2744 void rtapi_app_exit(void) 2745 { 2746 hal_thread_t *thread; 2747 2748 rtapi_print_msg(RTAPI_MSG_DBG, "HAL_LIB: removing kernel lib\n"); 2749 hal_proc_clean(); 2750 /* grab mutex before manipulating list */ 2751 rtapi_mutex_get(&(hal_data->mutex)); 2752 /* must remove all threads before unloading this module */ 2753 while (hal_data->thread_list_ptr != 0) { 2754 /* point to a thread */ 2755 thread = SHMPTR(hal_data->thread_list_ptr); 2756 /* unlink from list */ 2757 hal_data->thread_list_ptr = thread->next_ptr; 2758 /* and delete it */ 2759 free_thread_struct(thread); 2760 } 2761 /* release mutex */ 2762 rtapi_mutex_give(&(hal_data->mutex)); 2763 /* release RTAPI resources */ 2764 rtapi_shmem_delete(lib_mem_id, lib_module_id); 2765 rtapi_exit(lib_module_id); 2766 /* done */ 2767 rtapi_print_msg(RTAPI_MSG_DBG, 2768 "HAL_LIB: kernel lib removed successfully\n"); 2769 } 2770 2771 /* this is the task function that implements threads in realtime */ 2772 2773 static void thread_task(void *arg) 2774 { 2775 hal_thread_t *thread; 2776 hal_funct_t *funct; 2777 hal_funct_entry_t *funct_root, *funct_entry; 2778 long long int start_time, end_time; 2779 long long int thread_start_time; 2780 2781 thread = arg; 2782 while (1) { 2783 if (hal_data->threads_running > 0) { 2784 /* point at first function on function list */ 2785 funct_root = (hal_funct_entry_t *) & (thread->funct_list); 2786 funct_entry = SHMPTR(funct_root->links.next); 2787 /* execution time logging */ 2788 start_time = rtapi_get_clocks(); 2789 end_time = start_time; 2790 thread_start_time = start_time; 2791 /* run thru function list */ 2792 while (funct_entry != funct_root) { 2793 /* call the function */ 2794 funct_entry->funct(funct_entry->arg, thread->period); 2795 /* capture execution time */ 2796 end_time = rtapi_get_clocks(); 2797 /* point to function structure */ 2798 funct = SHMPTR(funct_entry->funct_ptr); 2799 /* update execution time data */ 2800 *(funct->runtime) = (hal_s32_t)(end_time - start_time); 2801 if ( *(funct->runtime) > funct->maxtime) { 2802 funct->maxtime = *(funct->runtime); 2803 funct->maxtime_increased = 1; 2804 } else { 2805 funct->maxtime_increased = 0; 2806 } 2807 /* point to next next entry in list */ 2808 funct_entry = SHMPTR(funct_entry->links.next); 2809 /* prepare to measure time for next funct */ 2810 start_time = end_time; 2811 } 2812 /* update thread execution time */ 2813 *(thread->runtime) = (hal_s32_t)(end_time - thread_start_time); 2814 if ( *(thread->runtime) > thread->maxtime) { 2815 thread->maxtime = *(thread->runtime); 2816 } 2817 } 2818 /* wait until next period */ 2819 rtapi_wait(); 2820 } 2821 } 2822 #endif /* RTAPI */ 2823 2824 /* see the declarations of these functions (near top of file) for 2825 a description of what they do. 2826 */ 2827 2828 static int init_hal_data(void) 2829 { 2830 /* has the block already been initialized? */ 2831 if (hal_data->version != 0) { 2832 /* yes, verify version code */ 2833 if (hal_data->version == HAL_VER) { 2834 return 0; 2835 } else { 2836 rtapi_print_msg(RTAPI_MSG_ERR, 2837 "HAL: ERROR: version code mismatch\n"); 2838 return -1; 2839 } 2840 } 2841 /* no, we need to init it, grab the mutex unconditionally */ 2842 rtapi_mutex_try(&(hal_data->mutex)); 2843 /* set version code so nobody else init's the block */ 2844 hal_data->version = HAL_VER; 2845 /* initialize everything */ 2846 hal_data->comp_list_ptr = 0; 2847 hal_data->pin_list_ptr = 0; 2848 hal_data->sig_list_ptr = 0; 2849 hal_data->param_list_ptr = 0; 2850 hal_data->funct_list_ptr = 0; 2851 hal_data->thread_list_ptr = 0; 2852 hal_data->base_period = 0; 2853 hal_data->threads_running = 0; 2854 hal_data->oldname_free_ptr = 0; 2855 hal_data->comp_free_ptr = 0; 2856 hal_data->pin_free_ptr = 0; 2857 hal_data->sig_free_ptr = 0; 2858 hal_data->param_free_ptr = 0; 2859 hal_data->funct_free_ptr = 0; 2860 hal_data->pending_constructor = 0; 2861 hal_data->constructor_prefix[0] = 0; 2862 list_init_entry(&(hal_data->funct_entry_free)); 2863 hal_data->thread_free_ptr = 0; 2864 hal_data->exact_base_period = 0; 2865 /* set up for shmalloc_xx() */ 2866 hal_data->shmem_bot = sizeof(hal_data_t); 2867 hal_data->shmem_top = HAL_SIZE; 2868 hal_data->lock = HAL_LOCK_NONE; 2869 /* done, release mutex */ 2870 rtapi_mutex_give(&(hal_data->mutex)); 2871 return 0; 2872 } 2873 2874 static void *shmalloc_up(long int size) 2875 { 2876 long int tmp_bot; 2877 void *retval; 2878 2879 /* deal with alignment requirements */ 2880 tmp_bot = hal_data->shmem_bot; 2881 if (size >= 8) { 2882 /* align on 8 byte boundary */ 2883 tmp_bot = (tmp_bot + 7) & (~7); 2884 } else if (size >= 4) { 2885 /* align on 4 byte boundary */ 2886 tmp_bot = (tmp_bot + 3) & (~3); 2887 } else if (size == 2) { 2888 /* align on 2 byte boundary */ 2889 tmp_bot = (tmp_bot + 1) & (~1); 2890 } 2891 /* is there enough memory available? */ 2892 if ((hal_data->shmem_top - tmp_bot) < size) { 2893 /* no */ 2894 return 0; 2895 } 2896 /* memory is available, allocate it */ 2897 retval = SHMPTR(tmp_bot); 2898 hal_data->shmem_bot = tmp_bot + size; 2899 hal_data->shmem_avail = hal_data->shmem_top - hal_data->shmem_bot; 2900 return retval; 2901 } 2902 2903 static void *shmalloc_dn(long int size) 2904 { 2905 long int tmp_top; 2906 void *retval; 2907 2908 /* tentatively allocate memory */ 2909 tmp_top = hal_data->shmem_top - size; 2910 /* deal with alignment requirements */ 2911 if (size >= 8) { 2912 /* align on 8 byte boundary */ 2913 tmp_top &= (~7); 2914 } else if (size >= 4) { 2915 /* align on 4 byte boundary */ 2916 tmp_top &= (~3); 2917 } else if (size == 2) { 2918 /* align on 2 byte boundary */ 2919 tmp_top &= (~1); 2920 } 2921 /* is there enough memory available? */ 2922 if (tmp_top < hal_data->shmem_bot) { 2923 /* no */ 2924 return 0; 2925 } 2926 /* memory is available, allocate it */ 2927 retval = SHMPTR(tmp_top); 2928 hal_data->shmem_top = tmp_top; 2929 hal_data->shmem_avail = hal_data->shmem_top - hal_data->shmem_bot; 2930 return retval; 2931 } 2932 2933 hal_comp_t *halpr_alloc_comp_struct(void) 2934 { 2935 hal_comp_t *p; 2936 2937 /* check the free list */ 2938 if (hal_data->comp_free_ptr != 0) { 2939 /* found a free structure, point to it */ 2940 p = SHMPTR(hal_data->comp_free_ptr); 2941 /* unlink it from the free list */ 2942 hal_data->comp_free_ptr = p->next_ptr; 2943 p->next_ptr = 0; 2944 } else { 2945 /* nothing on free list, allocate a brand new one */ 2946 p = shmalloc_dn(sizeof(hal_comp_t)); 2947 } 2948 if (p) { 2949 /* make sure it's empty */ 2950 p->next_ptr = 0; 2951 p->comp_id = 0; 2952 p->mem_id = 0; 2953 p->type = 0; 2954 p->shmem_base = 0; 2955 p->name[0] = '\0'; 2956 } 2957 return p; 2958 } 2959 2960 static hal_pin_t *alloc_pin_struct(void) 2961 { 2962 hal_pin_t *p; 2963 2964 /* check the free list */ 2965 if (hal_data->pin_free_ptr != 0) { 2966 /* found a free structure, point to it */ 2967 p = SHMPTR(hal_data->pin_free_ptr); 2968 /* unlink it from the free list */ 2969 hal_data->pin_free_ptr = p->next_ptr; 2970 p->next_ptr = 0; 2971 } else { 2972 /* nothing on free list, allocate a brand new one */ 2973 p = shmalloc_dn(sizeof(hal_pin_t)); 2974 } 2975 if (p) { 2976 /* make sure it's empty */ 2977 p->next_ptr = 0; 2978 p->data_ptr_addr = 0; 2979 p->owner_ptr = 0; 2980 p->type = 0; 2981 p->dir = 0; 2982 p->signal = 0; 2983 memset(&p->dummysig, 0, sizeof(hal_data_u)); 2984 p->name[0] = '\0'; 2985 } 2986 return p; 2987 } 2988 2989 static hal_sig_t *alloc_sig_struct(void) 2990 { 2991 hal_sig_t *p; 2992 2993 /* check the free list */ 2994 if (hal_data->sig_free_ptr != 0) { 2995 /* found a free structure, point to it */ 2996 p = SHMPTR(hal_data->sig_free_ptr); 2997 /* unlink it from the free list */ 2998 hal_data->sig_free_ptr = p->next_ptr; 2999 p->next_ptr = 0; 3000 } else { 3001 /* nothing on free list, allocate a brand new one */ 3002 p = shmalloc_dn(sizeof(hal_sig_t)); 3003 } 3004 if (p) { 3005 /* make sure it's empty */ 3006 p->next_ptr = 0; 3007 p->data_ptr = 0; 3008 p->type = 0; 3009 p->readers = 0; 3010 p->writers = 0; 3011 p->bidirs = 0; 3012 p->name[0] = '\0'; 3013 } 3014 return p; 3015 } 3016 3017 static hal_param_t *alloc_param_struct(void) 3018 { 3019 hal_param_t *p; 3020 3021 /* check the free list */ 3022 if (hal_data->param_free_ptr != 0) { 3023 /* found a free structure, point to it */ 3024 p = SHMPTR(hal_data->param_free_ptr); 3025 /* unlink it from the free list */ 3026 hal_data->param_free_ptr = p->next_ptr; 3027 p->next_ptr = 0; 3028 } else { 3029 /* nothing on free list, allocate a brand new one */ 3030 p = shmalloc_dn(sizeof(hal_param_t)); 3031 } 3032 if (p) { 3033 /* make sure it's empty */ 3034 p->next_ptr = 0; 3035 p->data_ptr = 0; 3036 p->owner_ptr = 0; 3037 p->type = 0; 3038 p->name[0] = '\0'; 3039 } 3040 return p; 3041 } 3042 3043 static hal_oldname_t *halpr_alloc_oldname_struct(void) 3044 { 3045 hal_oldname_t *p; 3046 3047 /* check the free list */ 3048 if (hal_data->oldname_free_ptr != 0) { 3049 /* found a free structure, point to it */ 3050 p = SHMPTR(hal_data->oldname_free_ptr); 3051 /* unlink it from the free list */ 3052 hal_data->oldname_free_ptr = p->next_ptr; 3053 p->next_ptr = 0; 3054 } else { 3055 /* nothing on free list, allocate a brand new one */ 3056 p = shmalloc_dn(sizeof(hal_oldname_t)); 3057 } 3058 if (p) { 3059 /* make sure it's empty */ 3060 p->next_ptr = 0; 3061 p->name[0] = '\0'; 3062 } 3063 return p; 3064 } 3065 3066 #ifdef RTAPI 3067 static hal_funct_t *alloc_funct_struct(void) 3068 { 3069 hal_funct_t *p; 3070 3071 /* check the free list */ 3072 if (hal_data->funct_free_ptr != 0) { 3073 /* found a free structure, point to it */ 3074 p = SHMPTR(hal_data->funct_free_ptr); 3075 /* unlink it from the free list */ 3076 hal_data->funct_free_ptr = p->next_ptr; 3077 p->next_ptr = 0; 3078 } else { 3079 /* nothing on free list, allocate a brand new one */ 3080 p = shmalloc_dn(sizeof(hal_funct_t)); 3081 } 3082 if (p) { 3083 /* make sure it's empty */ 3084 p->next_ptr = 0; 3085 p->uses_fp = 0; 3086 p->owner_ptr = 0; 3087 p->reentrant = 0; 3088 p->users = 0; 3089 p->arg = 0; 3090 p->funct = 0; 3091 p->name[0] = '\0'; 3092 } 3093 return p; 3094 } 3095 #endif /* RTAPI */ 3096 3097 static hal_funct_entry_t *alloc_funct_entry_struct(void) 3098 { 3099 hal_list_t *freelist, *l; 3100 hal_funct_entry_t *p; 3101 3102 /* check the free list */ 3103 freelist = &(hal_data->funct_entry_free); 3104 l = list_next(freelist); 3105 if (l != freelist) { 3106 /* found a free structure, unlink from the free list */ 3107 list_remove_entry(l); 3108 p = (hal_funct_entry_t *) l; 3109 } else { 3110 /* nothing on free list, allocate a brand new one */ 3111 p = shmalloc_dn(sizeof(hal_funct_entry_t)); 3112 l = (hal_list_t *) p; 3113 list_init_entry(l); 3114 } 3115 if (p) { 3116 /* make sure it's empty */ 3117 p->funct_ptr = 0; 3118 p->arg = 0; 3119 p->funct = 0; 3120 } 3121 return p; 3122 } 3123 3124 #ifdef RTAPI 3125 static hal_thread_t *alloc_thread_struct(void) 3126 { 3127 hal_thread_t *p; 3128 3129 /* check the free list */ 3130 if (hal_data->thread_free_ptr != 0) { 3131 /* found a free structure, point to it */ 3132 p = SHMPTR(hal_data->thread_free_ptr); 3133 /* unlink it from the free list */ 3134 hal_data->thread_free_ptr = p->next_ptr; 3135 p->next_ptr = 0; 3136 } else { 3137 /* nothing on free list, allocate a brand new one */ 3138 p = shmalloc_dn(sizeof(hal_thread_t)); 3139 } 3140 if (p) { 3141 /* make sure it's empty */ 3142 p->next_ptr = 0; 3143 p->uses_fp = 0; 3144 p->period = 0; 3145 p->priority = 0; 3146 p->task_id = 0; 3147 list_init_entry(&(p->funct_list)); 3148 p->name[0] = '\0'; 3149 } 3150 return p; 3151 } 3152 #endif /* RTAPI */ 3153 3154 static void free_comp_struct(hal_comp_t * comp) 3155 { 3156 rtapi_intptr_t *prev, next; 3157 #ifdef RTAPI 3158 hal_funct_t *funct; 3159 #endif /* RTAPI */ 3160 hal_pin_t *pin; 3161 hal_param_t *param; 3162 3163 /* can't delete the component until we delete its "stuff" */ 3164 /* need to check for functs only if a realtime component */ 3165 #ifdef RTAPI 3166 /* search the function list for this component's functs */ 3167 prev = &(hal_data->funct_list_ptr); 3168 next = *prev; 3169 while (next != 0) { 3170 funct = SHMPTR(next); 3171 if (SHMPTR(funct->owner_ptr) == comp) { 3172 /* this function belongs to our component, unlink from list */ 3173 *prev = funct->next_ptr; 3174 /* and delete it */ 3175 free_funct_struct(funct); 3176 } else { 3177 /* no match, try the next one */ 3178 prev = &(funct->next_ptr); 3179 } 3180 next = *prev; 3181 } 3182 #endif /* RTAPI */ 3183 /* search the pin list for this component's pins */ 3184 prev = &(hal_data->pin_list_ptr); 3185 next = *prev; 3186 while (next != 0) { 3187 pin = SHMPTR(next); 3188 if (SHMPTR(pin->owner_ptr) == comp) { 3189 /* this pin belongs to our component, unlink from list */ 3190 *prev = pin->next_ptr; 3191 /* and delete it */ 3192 free_pin_struct(pin); 3193 } else { 3194 /* no match, try the next one */ 3195 prev = &(pin->next_ptr); 3196 } 3197 next = *prev; 3198 } 3199 /* search the parameter list for this component's parameters */ 3200 prev = &(hal_data->param_list_ptr); 3201 next = *prev; 3202 while (next != 0) { 3203 param = SHMPTR(next); 3204 if (SHMPTR(param->owner_ptr) == comp) { 3205 /* this param belongs to our component, unlink from list */ 3206 *prev = param->next_ptr; 3207 /* and delete it */ 3208 free_param_struct(param); 3209 } else { 3210 /* no match, try the next one */ 3211 prev = &(param->next_ptr); 3212 } 3213 next = *prev; 3214 } 3215 /* now we can delete the component itself */ 3216 /* clear contents of struct */ 3217 comp->comp_id = 0; 3218 comp->mem_id = 0; 3219 comp->type = 0; 3220 comp->shmem_base = 0; 3221 comp->name[0] = '\0'; 3222 /* add it to free list */ 3223 comp->next_ptr = hal_data->comp_free_ptr; 3224 hal_data->comp_free_ptr = SHMOFF(comp); 3225 } 3226 3227 static void unlink_pin(hal_pin_t * pin) 3228 { 3229 hal_sig_t *sig; 3230 hal_comp_t *comp; 3231 void **data_ptr_addr; 3232 hal_data_u *dummy_addr, *sig_data_addr; 3233 3234 /* is this pin linked to a signal? */ 3235 if (pin->signal != 0) { 3236 /* yes, need to unlink it */ 3237 sig = SHMPTR(pin->signal); 3238 /* make pin's 'data_ptr' point to its dummy signal */ 3239 data_ptr_addr = SHMPTR(pin->data_ptr_addr); 3240 comp = SHMPTR(pin->owner_ptr); 3241 dummy_addr = comp->shmem_base + SHMOFF(&(pin->dummysig)); 3242 *data_ptr_addr = dummy_addr; 3243 3244 /* copy current signal value to dummy */ 3245 sig_data_addr = (hal_data_u *)(hal_shmem_base + sig->data_ptr); 3246 dummy_addr = (hal_data_u *)(hal_shmem_base + SHMOFF(&(pin->dummysig))); 3247 3248 switch (pin->type) { 3249 case HAL_BIT: 3250 dummy_addr->b = sig_data_addr->b; 3251 break; 3252 case HAL_S32: 3253 dummy_addr->s = sig_data_addr->s; 3254 break; 3255 case HAL_U32: 3256 dummy_addr->u = sig_data_addr->u; 3257 break; 3258 case HAL_FLOAT: 3259 dummy_addr->f = sig_data_addr->f; 3260 break; 3261 case HAL_PORT: 3262 /*once a pin is unlinked from its signal, it gets set to the empty_port*/ 3263 dummy_addr->p = 0; 3264 break; 3265 default: 3266 rtapi_print_msg(RTAPI_MSG_ERR, 3267 "HAL: BUG: pin '%s' has invalid type %d !!\n", 3268 pin->name, pin->type); 3269 } 3270 3271 /* update the signal's reader/writer counts */ 3272 if ((pin->dir & HAL_IN) != 0) { 3273 sig->readers--; 3274 } 3275 if (pin->dir == HAL_OUT) { 3276 sig->writers--; 3277 } 3278 if (pin->dir == HAL_IO) { 3279 sig->bidirs--; 3280 } 3281 /* mark pin as unlinked */ 3282 pin->signal = 0; 3283 } 3284 } 3285 3286 static void free_pin_struct(hal_pin_t * pin) 3287 { 3288 3289 unlink_pin(pin); 3290 /* clear contents of struct */ 3291 if ( pin->oldname != 0 ) free_oldname_struct(SHMPTR(pin->oldname)); 3292 pin->data_ptr_addr = 0; 3293 pin->owner_ptr = 0; 3294 pin->type = 0; 3295 pin->dir = 0; 3296 pin->signal = 0; 3297 memset(&pin->dummysig, 0, sizeof(hal_data_u)); 3298 pin->name[0] = '\0'; 3299 /* add it to free list */ 3300 pin->next_ptr = hal_data->pin_free_ptr; 3301 hal_data->pin_free_ptr = SHMOFF(pin); 3302 } 3303 3304 static void free_sig_struct(hal_sig_t * sig) 3305 { 3306 hal_pin_t *pin; 3307 3308 /* look for pins linked to this signal */ 3309 pin = halpr_find_pin_by_sig(sig, 0); 3310 while (pin != 0) { 3311 /* found one, unlink it */ 3312 unlink_pin(pin); 3313 /* check for another pin linked to the signal */ 3314 pin = halpr_find_pin_by_sig(sig, pin); 3315 } 3316 /* clear contents of struct */ 3317 sig->data_ptr = 0; 3318 sig->type = 0; 3319 sig->readers = 0; 3320 sig->writers = 0; 3321 sig->bidirs = 0; 3322 sig->name[0] = '\0'; 3323 /* add it to free list */ 3324 sig->next_ptr = hal_data->sig_free_ptr; 3325 hal_data->sig_free_ptr = SHMOFF(sig); 3326 } 3327 3328 static void free_param_struct(hal_param_t * p) 3329 { 3330 /* clear contents of struct */ 3331 if ( p->oldname != 0 ) free_oldname_struct(SHMPTR(p->oldname)); 3332 p->data_ptr = 0; 3333 p->owner_ptr = 0; 3334 p->type = 0; 3335 p->name[0] = '\0'; 3336 /* add it to free list (params use the same struct as src vars) */ 3337 p->next_ptr = hal_data->param_free_ptr; 3338 hal_data->param_free_ptr = SHMOFF(p); 3339 } 3340 3341 static void free_oldname_struct(hal_oldname_t * oldname) 3342 { 3343 /* clear contents of struct */ 3344 oldname->name[0] = '\0'; 3345 /* add it to free list */ 3346 oldname->next_ptr = hal_data->oldname_free_ptr; 3347 hal_data->oldname_free_ptr = SHMOFF(oldname); 3348 } 3349 3350 #ifdef RTAPI 3351 static void free_funct_struct(hal_funct_t * funct) 3352 { 3353 int next_thread; 3354 hal_thread_t *thread; 3355 hal_list_t *list_root, *list_entry; 3356 hal_funct_entry_t *funct_entry; 3357 3358 /* int next_thread, next_entry;*/ 3359 3360 if (funct->users > 0) { 3361 /* We can't casually delete the function, there are thread(s) which 3362 will call it. So we must check all the threads and remove any 3363 funct_entrys that call this function */ 3364 /* start at root of thread list */ 3365 next_thread = hal_data->thread_list_ptr; 3366 /* run through thread list */ 3367 while (next_thread != 0) { 3368 /* point to thread */ 3369 thread = SHMPTR(next_thread); 3370 /* start at root of funct_entry list */ 3371 list_root = &(thread->funct_list); 3372 list_entry = list_next(list_root); 3373 /* run thru funct_entry list */ 3374 while (list_entry != list_root) { 3375 /* point to funct entry */ 3376 funct_entry = (hal_funct_entry_t *) list_entry; 3377 /* test it */ 3378 if (SHMPTR(funct_entry->funct_ptr) == funct) { 3379 /* this funct entry points to our funct, unlink */ 3380 list_entry = list_remove_entry(list_entry); 3381 /* and delete it */ 3382 free_funct_entry_struct(funct_entry); 3383 } else { 3384 /* no match, try the next one */ 3385 list_entry = list_next(list_entry); 3386 } 3387 } 3388 /* move on to the next thread */ 3389 next_thread = thread->next_ptr; 3390 } 3391 } 3392 /* clear contents of struct */ 3393 funct->uses_fp = 0; 3394 funct->owner_ptr = 0; 3395 funct->reentrant = 0; 3396 funct->users = 0; 3397 funct->arg = 0; 3398 funct->funct = 0; 3399 funct->runtime = 0; 3400 funct->name[0] = '\0'; 3401 /* add it to free list */ 3402 funct->next_ptr = hal_data->funct_free_ptr; 3403 hal_data->funct_free_ptr = SHMOFF(funct); 3404 } 3405 #endif /* RTAPI */ 3406 3407 static void free_funct_entry_struct(hal_funct_entry_t * funct_entry) 3408 { 3409 hal_funct_t *funct; 3410 3411 if (funct_entry->funct_ptr > 0) { 3412 /* entry points to a function, update the function struct */ 3413 funct = SHMPTR(funct_entry->funct_ptr); 3414 funct->users--; 3415 } 3416 /* clear contents of struct */ 3417 funct_entry->funct_ptr = 0; 3418 funct_entry->arg = 0; 3419 funct_entry->funct = 0; 3420 /* add it to free list */ 3421 list_add_after((hal_list_t *) funct_entry, &(hal_data->funct_entry_free)); 3422 } 3423 3424 #ifdef RTAPI 3425 static void free_thread_struct(hal_thread_t * thread) 3426 { 3427 hal_funct_entry_t *funct_entry; 3428 hal_list_t *list_root, *list_entry; 3429 /*! \todo Another #if 0 */ 3430 #if 0 3431 rtapi_intptr_t *prev, next; 3432 char time[HAL_NAME_LEN + 1], tmax[HAL_NAME_LEN + 1]; 3433 hal_param_t *param; 3434 #endif 3435 3436 /* if we're deleting a thread, we need to stop all threads */ 3437 hal_data->threads_running = 0; 3438 /* and stop the task associated with this thread */ 3439 rtapi_task_pause(thread->task_id); 3440 rtapi_task_delete(thread->task_id); 3441 /* clear contents of struct */ 3442 thread->uses_fp = 0; 3443 thread->period = 0; 3444 thread->priority = 0; 3445 thread->task_id = 0; 3446 /* clear the function entry list */ 3447 list_root = &(thread->funct_list); 3448 list_entry = list_next(list_root); 3449 while (list_entry != list_root) { 3450 /* entry found, save pointer to it */ 3451 funct_entry = (hal_funct_entry_t *) list_entry; 3452 /* unlink it, point to the next one */ 3453 list_entry = list_remove_entry(list_entry); 3454 /* free the removed entry */ 3455 free_funct_entry_struct(funct_entry); 3456 } 3457 /*! \todo Another #if 0 */ 3458 #if 0 3459 /* Currently these don't get created, so we don't have to worry 3460 about deleting them. They will come back when the HAL refactor 3461 is done, at that time this code or something like it will be 3462 needed. 3463 */ 3464 /* need to delete <thread>.time and <thread>.tmax params */ 3465 rtapi_snprintf(time, sizeof(time), "%s.time", thread->name); 3466 rtapi_snprintf(tmax, sizeof(tmax), "%s.tmax", thread->name); 3467 /* search the parameter list for those parameters */ 3468 prev = &(hal_data->param_list_ptr); 3469 next = *prev; 3470 while (next != 0) { 3471 param = SHMPTR(next); 3472 /* does this param match either name? */ 3473 if ((strcmp(param->name, time) == 0) 3474 || (strcmp(param->name, tmax) == 0)) { 3475 /* yes, unlink from list */ 3476 *prev = param->next_ptr; 3477 /* and delete it */ 3478 free_param_struct(param); 3479 } else { 3480 /* no match, try the next one */ 3481 prev = &(param->next_ptr); 3482 } 3483 next = *prev; 3484 } 3485 #endif 3486 thread->name[0] = '\0'; 3487 /* add thread to free list */ 3488 thread->next_ptr = hal_data->thread_free_ptr; 3489 hal_data->thread_free_ptr = SHMOFF(thread); 3490 } 3491 #endif /* RTAPI */ 3492 3493 static char *halpr_type_string(int type, char *buf, size_t nbuf) { 3494 switch(type) { 3495 case HAL_BIT: return "bit"; 3496 case HAL_FLOAT: return "float"; 3497 case HAL_S32: return "s32"; 3498 case HAL_U32: return "u32"; 3499 case HAL_PORT: return "port"; 3500 default: 3501 rtapi_snprintf(buf, nbuf, "UNK#%d", type); 3502 return buf; 3503 } 3504 } 3505 3506 3507 3508 /****************************************************************************** 3509 HAL PORT functions 3510 ******************************************************************************/ 3511 3512 static void hal_port_atomic_load(hal_port_shm_t* port_shm, unsigned* read, unsigned* write) 3513 { 3514 *read = atomic_load_explicit(&port_shm->read, memory_order_acquire); 3515 *write = atomic_load_explicit(&port_shm->write, memory_order_acquire); 3516 } 3517 3518 3519 static void hal_port_atomic_store_read(hal_port_shm_t* port_shm, unsigned read) 3520 { 3521 atomic_store_explicit(&port_shm->read, read, memory_order_release); 3522 } 3523 3524 3525 static void hal_port_atomic_store_write(hal_port_shm_t* port_shm, unsigned write) 3526 { 3527 atomic_store_explicit(&port_shm->write, write, memory_order_release); 3528 } 3529 3530 3531 static unsigned hal_port_bytes_readable(unsigned read, unsigned write, unsigned size) { 3532 if(size == 0) { 3533 return 0; 3534 } else if (read <= write) { 3535 return write - read; 3536 } else { 3537 return size - read + write; 3538 } 3539 } 3540 3541 3542 static unsigned hal_port_bytes_writable(unsigned read, unsigned write, unsigned size) { 3543 if(size == 0) { 3544 return 0; 3545 } else if(write < read) { 3546 return read - write - 1; 3547 } else { 3548 return size - write + read - 1; 3549 } 3550 3551 } 3552 3553 3554 static bool hal_port_compute_copy(unsigned read, 3555 unsigned write, 3556 unsigned size, 3557 unsigned count, 3558 unsigned* end_bytes_to_read, 3559 unsigned* beg_bytes_to_read, 3560 unsigned* final_pos) 3561 { 3562 unsigned bytes_avail, 3563 end_bytes_avail; 3564 3565 bytes_avail = hal_port_bytes_readable(read, write, size); 3566 3567 if(count > bytes_avail) { 3568 return false; 3569 } else if(read <= write) { 3570 //can read up to write position (no wraparound condition in buffer) 3571 *end_bytes_to_read = count; 3572 *beg_bytes_to_read = 0; 3573 *final_pos = read + count; 3574 } else { 3575 end_bytes_avail = size - read; 3576 3577 if(count < end_bytes_avail) { 3578 //still only reading from end of buffer 3579 *end_bytes_to_read = count; 3580 *beg_bytes_to_read = 0; 3581 *final_pos = read + count; 3582 } else { 3583 //read porition of buffer with wrap around to beginnning of buffer 3584 *end_bytes_to_read = end_bytes_avail; 3585 *beg_bytes_to_read = count - end_bytes_avail; 3586 *final_pos = *beg_bytes_to_read; 3587 } 3588 } 3589 3590 return true; 3591 } 3592 3593 3594 hal_port_t hal_port_alloc(unsigned size) { 3595 hal_port_shm_t* new_port = shmalloc_up(sizeof(hal_port_shm_t) + size); 3596 3597 memset(new_port, 0, sizeof(hal_port_shm_t)); 3598 3599 new_port->size = size; 3600 3601 return SHMOFF(new_port); 3602 } 3603 3604 3605 bool hal_port_read(hal_port_t port, char* dest, unsigned count) { 3606 unsigned read, 3607 write, 3608 end_bytes_to_read, //number of bytes to read after read position and before end of buffer 3609 beg_bytes_to_read, //number of bytes to read at beginning of buffer 3610 final_pos; //final position after read 3611 hal_port_shm_t* port_shm = SHMPTR(port); 3612 3613 3614 if(!port || !count) { 3615 return false; 3616 } else { 3617 hal_port_atomic_load(port_shm, &read, &write); 3618 3619 if(hal_port_compute_copy(read, 3620 write, 3621 port_shm->size, 3622 count, 3623 &end_bytes_to_read, 3624 &beg_bytes_to_read, 3625 &final_pos)) { 3626 3627 memcpy(dest, port_shm->buff + read, end_bytes_to_read); 3628 memcpy(dest+end_bytes_to_read, port_shm->buff, beg_bytes_to_read); 3629 hal_port_atomic_store_read(port_shm, final_pos); 3630 return true; 3631 } else { 3632 return false; 3633 } 3634 } 3635 } 3636 3637 3638 bool hal_port_peek(hal_port_t port, char* dest, unsigned count) { 3639 unsigned read, 3640 write, 3641 end_bytes_to_read, //number of bytes to read after read position and before end of buffer 3642 beg_bytes_to_read, //number of bytes to read at beginning of buffer 3643 final_pos; //final position of read 3644 hal_port_shm_t* port_shm = SHMPTR(port); 3645 3646 if(!port || !count) { 3647 return false; 3648 } else { 3649 hal_port_atomic_load(port_shm, &read, &write); 3650 3651 if(hal_port_compute_copy(read, 3652 write, 3653 port_shm->size, 3654 count, 3655 &end_bytes_to_read, 3656 &beg_bytes_to_read, 3657 &final_pos)) { 3658 3659 memcpy(dest, port_shm->buff + read, end_bytes_to_read); 3660 memcpy(dest+end_bytes_to_read, port_shm->buff, beg_bytes_to_read); 3661 return true; 3662 } else { 3663 return false; 3664 } 3665 } 3666 } 3667 3668 3669 bool hal_port_peek_commit(hal_port_t port, unsigned count) { 3670 unsigned read, 3671 write, 3672 end_bytes_to_read, //number of bytes to read after read position and before end of buffer 3673 beg_bytes_to_read, //number of bytes to read at beginning of buffer 3674 final_pos; //final position of read 3675 hal_port_shm_t* port_shm = SHMPTR(port); 3676 3677 if(!port || !count) { 3678 return false; 3679 } else { 3680 hal_port_atomic_load(port_shm, &read, &write); 3681 3682 if(hal_port_compute_copy(read, 3683 write, 3684 port_shm->size, 3685 count, 3686 &end_bytes_to_read, 3687 &beg_bytes_to_read, 3688 &final_pos)) { 3689 3690 hal_port_atomic_store_read(port_shm, final_pos); 3691 return true; 3692 } else { 3693 return false; 3694 } 3695 } 3696 } 3697 3698 3699 bool hal_port_write(hal_port_t port, const char* src, unsigned count) { 3700 unsigned read, 3701 write, 3702 bytes_avail, 3703 end_bytes_avail, 3704 end_bytes_to_write, 3705 beg_bytes_to_write, 3706 final_pos; 3707 3708 hal_port_shm_t* port_shm = SHMPTR(port); 3709 3710 if(!port || !count) { 3711 return false; 3712 } else { 3713 hal_port_atomic_load(port_shm, &read, &write); 3714 bytes_avail = hal_port_bytes_writable(read, write, port_shm->size); 3715 3716 if(count > bytes_avail) { 3717 return false; 3718 } else { 3719 if (write < read) { 3720 //we can write up to one byte behind read 3721 end_bytes_to_write = count; 3722 beg_bytes_to_write = 0; 3723 final_pos = write + count; 3724 } else { 3725 if(read == 0) { 3726 end_bytes_avail = bytes_avail; 3727 } else { 3728 end_bytes_avail = port_shm->size - write; 3729 } 3730 3731 if (count < end_bytes_avail) { 3732 end_bytes_to_write = count; 3733 beg_bytes_to_write = 0; 3734 final_pos = write + count; 3735 } else { 3736 end_bytes_to_write = end_bytes_avail; 3737 beg_bytes_to_write = count - end_bytes_to_write; 3738 final_pos = beg_bytes_to_write; 3739 } 3740 } 3741 3742 memcpy(port_shm->buff+write, src, end_bytes_to_write); 3743 memcpy(port_shm->buff, src+end_bytes_to_write, beg_bytes_to_write); 3744 3745 hal_port_atomic_store_write(port_shm, final_pos); 3746 return true; 3747 } 3748 } 3749 } 3750 3751 3752 unsigned hal_port_readable(hal_port_t port) { 3753 hal_port_shm_t* port_shm = SHMPTR(port); 3754 3755 if(!port) { 3756 return 0; 3757 } else { 3758 return hal_port_bytes_readable(port_shm->read, port_shm->write, port_shm->size); 3759 } 3760 } 3761 3762 3763 unsigned hal_port_writable(hal_port_t port) { 3764 hal_port_shm_t* port_shm = SHMPTR(port); 3765 3766 if(!port) { 3767 return 0; 3768 } else { 3769 return hal_port_bytes_writable(port_shm->read, port_shm->write, port_shm->size); 3770 } 3771 } 3772 3773 3774 unsigned hal_port_buffer_size(hal_port_t port) { 3775 if(!port) { 3776 return 0; 3777 } else { 3778 return ((hal_port_shm_t*)SHMPTR(port))->size; 3779 } 3780 } 3781 3782 3783 void hal_port_clear(hal_port_t port) { 3784 unsigned read,write; 3785 hal_port_shm_t* port_shm = SHMPTR(port); 3786 3787 if(port) { 3788 hal_port_atomic_load(port_shm, &read, &write); 3789 hal_port_atomic_store_read(port_shm, write); 3790 } 3791 } 3792 3793 3794 #ifdef ULAPI 3795 void hal_port_wait_readable(hal_port_t** port, unsigned count, sig_atomic_t* stop) { 3796 while((hal_port_readable(**port) < count) && (!stop || !*stop)) { 3797 rtapi_delay(10000000); 3798 } 3799 } 3800 3801 3802 void hal_port_wait_writable(hal_port_t** port, unsigned count, sig_atomic_t* stop) { 3803 while((hal_port_writable(**port) < count) && (!stop || !*stop)) { 3804 rtapi_delay(10000000); 3805 } 3806 } 3807 #endif 3808 3809 3810 3811 3812 /* 3813 hal stream implementation 3814 */ 3815 int halpr_parse_types(hal_type_t type[HAL_STREAM_MAX_PINS], const char *cfg) 3816 { 3817 const char *c; 3818 int n; 3819 3820 c = cfg; 3821 n = 0; 3822 while (( n < HAL_STREAM_MAX_PINS ) && ( *c != '\0' )) { 3823 switch (*c) { 3824 case 'f': 3825 case 'F': 3826 type[n++] = HAL_FLOAT; 3827 c++; 3828 break; 3829 case 'b': 3830 case 'B': 3831 type[n++] = HAL_BIT; 3832 c++; 3833 break; 3834 case 'u': 3835 case 'U': 3836 type[n++] = HAL_U32; 3837 c++; 3838 break; 3839 case 's': 3840 case 'S': 3841 type[n++] = HAL_S32; 3842 c++; 3843 break; 3844 default: 3845 rtapi_print_msg(RTAPI_MSG_ERR, 3846 "stream: ERROR: unknown type '%c', must be F, B, U, or S\n", *c); 3847 return 0; 3848 } 3849 } 3850 if ( *c != '\0' ) { 3851 /* didn't reach end of cfg string */ 3852 rtapi_print_msg(RTAPI_MSG_ERR, 3853 "stream: ERROR: more than %d items\n", HAL_STREAM_MAX_PINS); 3854 return 0; 3855 } 3856 return n; 3857 } 3858 3859 int hal_stream_create(hal_stream_t *stream, int comp, int key, int depth, const char *typestring) 3860 { 3861 int result = 0; 3862 hal_type_t type[HAL_STREAM_MAX_PINS]; 3863 result = halpr_parse_types(type, typestring); 3864 if(result < 0) return result; 3865 int pin_count = result; 3866 3867 size_t size = sizeof(struct hal_stream_shm) + sizeof(union hal_stream_data) * depth * (1+pin_count); 3868 result = rtapi_shmem_new(key, comp, size); 3869 if(result < 0) return result; 3870 stream->shmem_id = result; 3871 3872 result = rtapi_shmem_getptr(stream->shmem_id, (void**)&stream->fifo); 3873 if(result < 0) { 3874 rtapi_shmem_delete(key, comp); 3875 return result; 3876 } 3877 memset(stream->fifo, 0, sizeof(*stream->fifo)); 3878 3879 stream->fifo->depth = depth; 3880 stream->fifo->num_pins = pin_count; 3881 memcpy(stream->fifo->type, type, sizeof(type)); 3882 stream->comp_id = comp; 3883 stream->fifo->magic = HAL_STREAM_MAGIC_NUM; 3884 return 0; 3885 } 3886 3887 extern void hal_stream_destroy(hal_stream_t *stream) 3888 { 3889 hal_stream_detach(stream); 3890 } 3891 3892 static int hal_stream_advance(hal_stream_t *stream, int n) { 3893 n = n + 1; 3894 if(n >= stream->fifo->depth) n = 0; 3895 return n; 3896 } 3897 3898 static int hal_stream_newin(hal_stream_t *stream) { 3899 return hal_stream_advance(stream, stream->fifo->in); 3900 } 3901 3902 bool hal_stream_writable(hal_stream_t *stream) { 3903 return hal_stream_newin(stream) != stream->fifo->out; 3904 } 3905 3906 bool hal_stream_readable(hal_stream_t *stream) { 3907 return stream->fifo->in != stream->fifo->out; 3908 } 3909 3910 int hal_stream_depth(hal_stream_t *stream) { 3911 int out = stream->fifo->out; 3912 int in = stream->fifo->in; 3913 int result = in - out; 3914 if(result < 0) result += stream->fifo->depth - 1; 3915 return result; 3916 } 3917 3918 int hal_stream_maxdepth(hal_stream_t *stream) { 3919 return stream->fifo->depth; 3920 } 3921 3922 #ifdef ULAPI 3923 void hal_stream_wait_writable(hal_stream_t *stream, sig_atomic_t *stop) { 3924 while(!hal_stream_writable(stream) && (!stop || !*stop)) { 3925 /* fifo full, sleep for 10ms */ 3926 rtapi_delay(10000000); 3927 } 3928 } 3929 3930 void hal_stream_wait_readable(hal_stream_t *stream, sig_atomic_t *stop) { 3931 while(!hal_stream_readable(stream) && (!stop || !*stop)) { 3932 /* fifo full, sleep for 10ms */ 3933 rtapi_delay(10000000); 3934 } 3935 } 3936 #endif 3937 3938 static int hal_stream_atomic_load_in(hal_stream_t *stream) 3939 { 3940 return atomic_load_explicit(&stream->fifo->in, memory_order_acquire); 3941 } 3942 3943 static int hal_stream_atomic_load_out(hal_stream_t *stream) 3944 { 3945 return atomic_load_explicit(&stream->fifo->out, memory_order_acquire); 3946 } 3947 3948 3949 static void hal_stream_atomic_store_in(hal_stream_t *stream, int newin) 3950 { 3951 atomic_store_explicit(&stream->fifo->in, newin, memory_order_release); 3952 } 3953 3954 static void hal_stream_atomic_store_out(hal_stream_t *stream, int newout) 3955 { 3956 atomic_store_explicit(&stream->fifo->out, newout, memory_order_release); 3957 } 3958 3959 int hal_stream_write(hal_stream_t *stream, union hal_stream_data *buf) { 3960 if(!hal_stream_writable(stream)) { 3961 stream->fifo->num_overruns++; 3962 return -ENOSPC; 3963 } 3964 int in = hal_stream_atomic_load_in(stream), 3965 newin = hal_stream_advance(stream, in); 3966 int num_pins = stream->fifo->num_pins; 3967 int stride = num_pins + 1; 3968 union hal_stream_data *dptr = &stream->fifo->data[in * stride]; 3969 memcpy(dptr, buf, sizeof(union hal_stream_data) * num_pins); 3970 dptr[num_pins].s = ++stream->fifo->this_sample; 3971 hal_stream_atomic_store_in(stream, newin); 3972 return 0; 3973 } 3974 3975 int hal_stream_read(hal_stream_t *stream, union hal_stream_data *buf, unsigned *this_sample) { 3976 if(!hal_stream_readable(stream)) { 3977 stream->fifo->num_underruns ++; 3978 return -ENOSPC; 3979 } 3980 int out = hal_stream_atomic_load_out(stream), 3981 newout = hal_stream_advance(stream, out); 3982 int num_pins = stream->fifo->num_pins; 3983 int stride = num_pins + 1; 3984 union hal_stream_data *dptr = &stream->fifo->data[out * stride]; 3985 memcpy(buf, dptr, sizeof(union hal_stream_data) * num_pins); 3986 if(this_sample) *this_sample = dptr[num_pins].s; 3987 hal_stream_atomic_store_out(stream, newout); 3988 return 0; 3989 } 3990 3991 int hal_stream_attach(hal_stream_t *stream, int comp_id, int key, const char *typestring) { 3992 int i; 3993 3994 stream->shmem_id = stream->comp_id = -1; 3995 stream->fifo = NULL; 3996 memset(stream, 0, sizeof(*stream)); 3997 int result = rtapi_shmem_new(key, comp_id, sizeof(struct hal_stream_shm)); 3998 int shmem_id = result; 3999 if ( result < 0 ) goto fail; 4000 4001 void *shmem_ptr; 4002 result = rtapi_shmem_getptr(shmem_id, &shmem_ptr); 4003 if ( result < 0 ) goto fail; 4004 4005 struct hal_stream_shm *fifo = shmem_ptr; 4006 if ( fifo->magic != HAL_STREAM_MAGIC_NUM ) { 4007 result = -EINVAL; 4008 goto fail; 4009 } 4010 if(typestring) { 4011 hal_type_t type[HAL_STREAM_MAX_PINS]; 4012 result = halpr_parse_types(type, typestring); 4013 if(!result) { result = -EINVAL; goto fail; } 4014 for(i=0; i<result; i++) { 4015 if(type[i] != fifo->type[i]) { 4016 char typename0[8], typename1[8]; 4017 rtapi_print_msg(RTAPI_MSG_ERR, 4018 "Type mismatch: types[%d] = %s vs %s\n", i, 4019 halpr_type_string(fifo->type[i], 4020 typename0, sizeof(typename0)), 4021 halpr_type_string(type[i], 4022 typename1, sizeof(typename1))); 4023 result = -EINVAL; goto fail; 4024 } 4025 } 4026 } 4027 /* now use data in fifo structure to calculate proper shmem size */ 4028 int depth = fifo->depth; 4029 int pin_count = fifo->num_pins; 4030 size_t size = sizeof(struct hal_stream_shm) + sizeof(union hal_stream_data) * depth * (1+pin_count); 4031 /* close shmem, re-open with proper size */ 4032 rtapi_shmem_delete(shmem_id, comp_id); 4033 shmem_id = result = rtapi_shmem_new(key, comp_id, size); 4034 if ( result < 0 ) goto fail; 4035 result = rtapi_shmem_getptr(shmem_id, &shmem_ptr); 4036 if ( result < 0 ) goto fail; 4037 4038 stream->shmem_id = shmem_id; 4039 stream->comp_id = comp_id; 4040 stream->fifo = fifo; 4041 return 0; 4042 4043 fail: 4044 if(shmem_id >= 0) 4045 rtapi_shmem_delete(shmem_id, comp_id); 4046 return result; 4047 } 4048 4049 int hal_stream_detach(hal_stream_t *stream) { 4050 if(stream->shmem_id >= 0) { 4051 int res = rtapi_shmem_delete(stream->shmem_id, stream->comp_id); 4052 if(res < 0) 4053 rtapi_print_msg(RTAPI_MSG_ERR, 4054 "hal_stream_detach: rtapi_shmem_delete: failed with code %d\n", 4055 res); 4056 } 4057 stream->shmem_id = stream->comp_id = -1; 4058 stream->fifo = NULL; 4059 return 0; 4060 } 4061 4062 int hal_stream_element_count(hal_stream_t *stream) { 4063 return stream->fifo->num_pins; 4064 } 4065 4066 hal_type_t hal_stream_element_type(hal_stream_t *stream, int idx) { 4067 return stream->fifo->type[idx]; 4068 } 4069 4070 int hal_stream_num_overruns(hal_stream_t *stream) { 4071 return stream->fifo->num_overruns; 4072 } 4073 4074 int hal_stream_num_underruns(hal_stream_t *stream) { 4075 return stream->fifo->num_underruns; 4076 } 4077 4078 #ifdef RTAPI 4079 /* only export symbols when we're building a kernel module */ 4080 4081 EXPORT_SYMBOL(hal_init); 4082 EXPORT_SYMBOL(hal_ready); 4083 EXPORT_SYMBOL(hal_exit); 4084 EXPORT_SYMBOL(hal_malloc); 4085 EXPORT_SYMBOL(hal_comp_name); 4086 4087 EXPORT_SYMBOL(hal_pin_bit_new); 4088 EXPORT_SYMBOL(hal_pin_float_new); 4089 EXPORT_SYMBOL(hal_pin_u32_new); 4090 EXPORT_SYMBOL(hal_pin_s32_new); 4091 EXPORT_SYMBOL(hal_pin_port_new); 4092 EXPORT_SYMBOL(hal_pin_new); 4093 4094 EXPORT_SYMBOL(hal_pin_bit_newf); 4095 EXPORT_SYMBOL(hal_pin_float_newf); 4096 EXPORT_SYMBOL(hal_pin_u32_newf); 4097 EXPORT_SYMBOL(hal_pin_s32_newf); 4098 EXPORT_SYMBOL(hal_pin_port_newf); 4099 4100 4101 EXPORT_SYMBOL(hal_signal_new); 4102 EXPORT_SYMBOL(hal_signal_delete); 4103 EXPORT_SYMBOL(hal_link); 4104 EXPORT_SYMBOL(hal_unlink); 4105 4106 EXPORT_SYMBOL(hal_param_bit_new); 4107 EXPORT_SYMBOL(hal_param_float_new); 4108 EXPORT_SYMBOL(hal_param_u32_new); 4109 EXPORT_SYMBOL(hal_param_s32_new); 4110 EXPORT_SYMBOL(hal_param_new); 4111 4112 EXPORT_SYMBOL(hal_param_bit_newf); 4113 EXPORT_SYMBOL(hal_param_float_newf); 4114 EXPORT_SYMBOL(hal_param_u32_newf); 4115 EXPORT_SYMBOL(hal_param_s32_newf); 4116 4117 EXPORT_SYMBOL(hal_param_bit_set); 4118 EXPORT_SYMBOL(hal_param_float_set); 4119 EXPORT_SYMBOL(hal_param_u32_set); 4120 EXPORT_SYMBOL(hal_param_s32_set); 4121 EXPORT_SYMBOL(hal_param_set); 4122 4123 EXPORT_SYMBOL(hal_set_constructor); 4124 4125 EXPORT_SYMBOL(hal_export_funct); 4126 4127 EXPORT_SYMBOL(hal_create_thread); 4128 4129 EXPORT_SYMBOL(hal_add_funct_to_thread); 4130 EXPORT_SYMBOL(hal_del_funct_from_thread); 4131 4132 EXPORT_SYMBOL(hal_start_threads); 4133 EXPORT_SYMBOL(hal_stop_threads); 4134 4135 EXPORT_SYMBOL(hal_shmem_base); 4136 EXPORT_SYMBOL(halpr_find_comp_by_name); 4137 EXPORT_SYMBOL(halpr_find_pin_by_name); 4138 EXPORT_SYMBOL(halpr_find_sig_by_name); 4139 EXPORT_SYMBOL(halpr_find_param_by_name); 4140 EXPORT_SYMBOL(halpr_find_thread_by_name); 4141 EXPORT_SYMBOL(halpr_find_funct_by_name); 4142 EXPORT_SYMBOL(halpr_find_comp_by_id); 4143 4144 EXPORT_SYMBOL(halpr_find_pin_by_owner); 4145 EXPORT_SYMBOL(halpr_find_param_by_owner); 4146 EXPORT_SYMBOL(halpr_find_funct_by_owner); 4147 4148 EXPORT_SYMBOL(halpr_find_pin_by_sig); 4149 4150 EXPORT_SYMBOL(hal_pin_alias); 4151 EXPORT_SYMBOL(hal_param_alias); 4152 4153 EXPORT_SYMBOL(hal_port_alloc); 4154 EXPORT_SYMBOL(hal_port_read); 4155 EXPORT_SYMBOL(hal_port_peek); 4156 EXPORT_SYMBOL(hal_port_peek_commit); 4157 EXPORT_SYMBOL(hal_port_write); 4158 EXPORT_SYMBOL(hal_port_readable); 4159 EXPORT_SYMBOL(hal_port_writable); 4160 EXPORT_SYMBOL(hal_port_buffer_size); 4161 EXPORT_SYMBOL(hal_port_clear); 4162 4163 EXPORT_SYMBOL_GPL(hal_stream_create); 4164 EXPORT_SYMBOL_GPL(hal_stream_destroy); 4165 EXPORT_SYMBOL_GPL(hal_stream_readable); 4166 EXPORT_SYMBOL_GPL(hal_stream_writable); 4167 EXPORT_SYMBOL_GPL(hal_stream_depth); 4168 EXPORT_SYMBOL_GPL(hal_stream_maxdepth); 4169 EXPORT_SYMBOL_GPL(hal_stream_write); 4170 EXPORT_SYMBOL_GPL(hal_stream_read); 4171 EXPORT_SYMBOL_GPL(hal_stream_attach); 4172 EXPORT_SYMBOL_GPL(hal_stream_detach); 4173 EXPORT_SYMBOL_GPL(hal_stream_element_count); 4174 EXPORT_SYMBOL_GPL(hal_stream_element_type); 4175 EXPORT_SYMBOL_GPL(hal_stream_num_overruns); 4176 EXPORT_SYMBOL_GPL(hal_stream_num_underruns); 4177 #endif /* rtapi */