int hal_set_constructor(int comp_id, constructor make) {
int next;
hal_comp_t *comp __attribute__((cleanup(halpr_autorelease_mutex)));
rtapi_mutex_get(&(hal_data->mutex));
/* search component list for 'comp_id' */
next = hal_data->comp_list_ptr;
if (next == 0) {
/* list is empty - should never happen, but... */
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: component %d not found\n", comp_id);
return -EINVAL;
}
comp = SHMPTR(next);
while (comp->comp_id != comp_id) {
/* not a match, try the next one */
next = comp->next_ptr;
if (next == 0) {
/* reached end of list without finding component */
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: component %d not found\n", comp_id);
return -EINVAL;
}
comp = SHMPTR(next);
}
comp->make = make;
return 0;
}
int hal_bind(const char *comp_name)
{
if (hal_data == 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: hal_bind called before hal_init\n");
return -EINVAL;
}
{
hal_comp_t *comp __attribute__((cleanup(halpr_autorelease_mutex)));
rtapi_mutex_get(&(hal_data->mutex));
comp = halpr_find_comp_by_name(comp_name);
if (comp == NULL) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: hal_bind(): no such component '%s'\n", comp_name);
return -EINVAL;
}
if (comp->type != TYPE_REMOTE) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: hal_bind(%s): not a remote componet (%d)\n",
comp_name, comp->type);
return -EINVAL;
}
if (comp->state != COMP_UNBOUND) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: hal_bind(%s): state not unbound (%d)\n",
comp_name, comp->state);
return -EINVAL;
}
comp->state = COMP_BOUND;
comp->last_bound = (long int) time(NULL);; // XXX ugly
}
return 0;
}
unsigned char hal_get_lock() {
if (hal_data == 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: get_lock called before init\n");
return -EINVAL;
}
return hal_data->lock;
}
int hal_acquire(const char *comp_name, int pid)
{
if (hal_data == 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: hal_acquire called before hal_init\n");
return -EINVAL;
}
{
hal_comp_t *comp __attribute__((cleanup(halpr_autorelease_mutex)));
rtapi_mutex_get(&(hal_data->mutex));
comp = halpr_find_comp_by_name(comp_name);
if (comp == NULL) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: hal_acquire(): no such component '%s'\n",
comp_name);
return -EINVAL;
}
if (comp->type != TYPE_REMOTE) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: hal_acquire(%s): not a remote component (%d)\n",
comp_name, comp->type);
return -EINVAL;
}
if (comp->state == COMP_BOUND) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: hal_acquire(%s): cant reown a bound component (%d)\n",
comp_name, comp->state);
return -EINVAL;
}
// let a comp be 'adopted away' from the RT environment
// this is a tad hacky, should separate owner pid from RT/user distinction
if ((comp->pid !=0) &&
(comp->pid != global_data->rtapi_app_pid))
{
hal_print_msg(RTAPI_MSG_ERR,
"HAL: hal_acquire(%s): component already owned by pid %d\n",
comp_name, comp->pid);
return -EINVAL;
}
comp->pid = pid;
return comp->comp_id;
}
}
/** The 'hal_set_lock()' function sets locking based on one of the
locking types defined in hal.h
*/
int hal_set_lock(unsigned char lock_type) {
if (hal_data == 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: set_lock called before init\n");
return -EINVAL;
}
hal_data->lock = lock_type;
return 0;
}
static int instantiate_lutn(const char *name,
const int argc,
const char**argv)
{
struct inst_data *ip;
int i, inst_id;
if ((inputs < 1) || (inputs > 5)) {
hal_print_msg(RTAPI_MSG_ERR,
"%s: invalid parameter inputs=%d, valid range=1..5",
name, inputs);
return -1;
}
if ((function == 0) || (function == -1)) {
hal_print_msg(RTAPI_MSG_ERR,
"%s: function=0x%x does not make sense",
name, function);
return -1;
}
if ((inst_id = hal_inst_create(name, comp_id,
sizeof(struct inst_data) + inputs * sizeof(hal_bit_t *),
(void **)&ip)) < 0)
return -1;
hal_print_msg(RTAPI_MSG_DBG,
"%s: name='%s' inputs=%d function=0x%x ip=%p",
__FUNCTION__, name, inputs, function, ip);
// record instance params
ip->inputs = inputs;
ip->function = function;
// export per-instance HAL objects
for (i = 0; i < ip->inputs; i++)
if (hal_pin_bit_newf(HAL_IN, &(ip->in[i]), inst_id, "%s.in%d", name, i))
return -1;
if (hal_pin_bit_newf(HAL_OUT, &(ip->out), inst_id, "%s.out", name))
return -1;
if (hal_export_functf(lutn, ip, 0, 0, inst_id, "%s.funct", name))
return -1;
return 0;
}
/***********************************************************************
* Scope exit unlock helper *
* see hal_priv.h for usage hints *
************************************************************************/
void halpr_autorelease_mutex(void *variable)
{
if (hal_data != NULL)
rtapi_mutex_give(&(hal_data->mutex));
else
// programming error
hal_print_msg(RTAPI_MSG_ERR,
"HAL:%d BUG: halpr_autorelease_mutex called before hal_data inited\n",
rtapi_instance);
}
void rtapi_app_exit(void)
{
int retval;
hal_print_msg(RTAPI_MSG_DBG,
"HAL_LIB:%d removing RT support\n",rtapi_instance);
hal_proc_clean();
{
hal_thread_t *thread __attribute__((cleanup(halpr_autorelease_mutex)));
/* grab mutex before manipulating list */
rtapi_mutex_get(&(hal_data->mutex));
/* must remove all threads before unloading this module */
while (hal_data->thread_list_ptr != 0) {
/* point to a thread */
thread = SHMPTR(hal_data->thread_list_ptr);
/* unlink from list */
hal_data->thread_list_ptr = thread->next_ptr;
/* and delete it */
free_thread_struct(thread);
}
}
/* release RTAPI resources */
retval = rtapi_shmem_delete(lib_mem_id, lib_module_id);
if (retval) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL_LIB:%d rtapi_shmem_delete(%d,%d) failed: %d\n",
rtapi_instance, lib_mem_id, lib_module_id, retval);
}
retval = rtapi_exit(lib_module_id);
if (retval) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL_LIB:%d rtapi_exit(%d) failed: %d\n",
rtapi_instance, lib_module_id, retval);
}
/* done */
hal_print_msg(RTAPI_MSG_DBG,
"HAL_LIB:%d RT support removed successfully\n",
rtapi_instance);
}
int hal_release(const char *comp_name)
{
if (hal_data == 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: hal_release called before hal_init\n");
return -EINVAL;
}
{
hal_comp_t *comp __attribute__((cleanup(halpr_autorelease_mutex)));
rtapi_mutex_get(&(hal_data->mutex));
comp = halpr_find_comp_by_name(comp_name);
if (comp == NULL) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: hal_release(): no such component '%s'\n",
comp_name);
return -EINVAL;
}
if (comp->type != TYPE_REMOTE) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: hal_release(%s): not a remote componet (%d)\n",
comp_name, comp->type);
return -EINVAL;
}
if (comp->pid == 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: hal_release(%s): component already disowned\n",
comp_name);
return -EINVAL;
}
if (comp->pid != getpid()) {
hal_print_msg(RTAPI_MSG_WARN,
"HAL: hal_release(%s): component owned by pid %d\n",
comp_name, comp->pid);
// return -EINVAL;
}
comp->pid = 0;
}
return 0;
}
static int proc_write_newinst(struct file *file,
const char *buffer, unsigned long count, void *data)
{
if(hal_data->pending_constructor) {
hal_print_msg(RTAPI_MSG_DBG,
"HAL: running constructor for %s %s\n",
hal_data->constructor_prefix,
hal_data->constructor_arg);
hal_data->pending_constructor(hal_data->constructor_prefix,
hal_data->constructor_arg);
hal_data->pending_constructor = 0;
}
return count;
}
int hal_ready(int comp_id) {
int next;
hal_comp_t *comp __attribute__((cleanup(halpr_autorelease_mutex)));
rtapi_mutex_get(&(hal_data->mutex));
/* search component list for 'comp_id' */
next = hal_data->comp_list_ptr;
if (next == 0) {
/* list is empty - should never happen, but... */
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: component %d not found\n", comp_id);
return -EINVAL;
}
comp = SHMPTR(next);
while (comp->comp_id != comp_id) {
/* not a match, try the next one */
next = comp->next_ptr;
if (next == 0) {
/* reached end of list without finding component */
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: component %d not found\n", comp_id);
return -EINVAL;
}
comp = SHMPTR(next);
}
if(comp->state > COMP_INITIALIZING) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: Component '%s' already ready (%d)\n",
comp->name, comp->state);
return -EINVAL;
}
comp->state = (comp->type == TYPE_REMOTE ? COMP_UNBOUND : COMP_READY);
return 0;
}
// simple interface to hal_create_thread()/hal_thread_delete()
// through /proc/rtapi/hal/threadcmd (kernel threadstyles only)
//
// to start a thread, write 'newthread' <threadname> <period> <fp> <cpu>'
// example:
// echo newthread servo-thread 1000000 1 -1 >/proc/rtapi/hal/threadcmd
//
// to delete a thread, write 'delthread <threadname>'
// echo delthread servo-thread >/proc/rtapi/hal/threadcmd
//
// HAL return values are reflected in the return value to write()
//
static int proc_write_threadcmd(struct file *file,
const char *buffer, unsigned long count, void *data)
{
char cmd[20], name[HAL_NAME_LEN + 1];
unsigned long period;
int fp, cpu, retval;
if (!strncmp(buffer,"newthread", 9)) {
if ((retval = sscanf(buffer, "%s %s %lu %d %d",
cmd, name, &period, &fp, &cpu)) != 5) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL:newthread: expecting 5 items (s:cmd s:name d:period d:fp d:cpu), got %d\n",
retval);
return -EINVAL;
}
if ((period > 0) &&
(strlen(name) > 0)) {
retval = hal_create_thread(name, period, fp, cpu);
if (retval < 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL:newthread: could not create thread '%s' - error %d\n",
name, retval);
return retval;
} else {
hal_print_msg(RTAPI_MSG_INFO,
"HAL:newthread: created %ld uS thread '%s' fp=%d cpu=%d\n",
period / 1000, name, fp, cpu);
}
}
} else if (!strncmp(buffer, "delthread", 9)) {
if ((retval = sscanf(buffer, "%s %s", cmd, name)) != 2) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL:delthread: expecting 2 items: 'delthread <threadname>'\n");
return -EINVAL;
}
if ((retval = hal_thread_delete(name))) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL:delthread '%s' error %d\n", name, retval);
return retval;
}
hal_print_msg(RTAPI_MSG_INFO,
"HAL:delthread - thread '%s' deleted\n", name);
} else {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: unrecognized threadcmd: '%s'\n", cmd);
return -EINVAL;
}
return count;
}
// this is now delayed to first hal_init() in this process
int hal_rtapi_attach()
{
int retval;
void *mem;
char rtapi_name[RTAPI_NAME_LEN + 1];
if (lib_mem_id < 0) {
hal_print_msg(RTAPI_MSG_DBG, "HAL: initializing hal_lib\n");
rtapi_snprintf(rtapi_name, RTAPI_NAME_LEN, "HAL_LIB_%d", (int)getpid());
lib_module_id = rtapi_init(rtapi_name);
if (lib_module_id < 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: could not not initialize RTAPI - realtime not started?\n");
return -EINVAL;
}
if (global_data == NULL) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: RTAPI shutting down - exiting\n");
exit(1);
}
/* get HAL shared memory block from RTAPI */
lib_mem_id = rtapi_shmem_new(HAL_KEY, lib_module_id, global_data->hal_size);
if (lib_mem_id < 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: could not open shared memory\n");
rtapi_exit(lib_module_id);
return -EINVAL;
}
/* get address of shared memory area */
retval = rtapi_shmem_getptr(lib_mem_id, &mem, 0);
if (retval < 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: could not access shared memory\n");
rtapi_exit(lib_module_id);
return -EINVAL;
}
/* set up internal pointers to shared mem and data structure */
hal_shmem_base = (char *) mem;
hal_data = (hal_data_t *) mem;
/* perform a global init if needed */
retval = init_hal_data();
if ( retval ) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: could not init shared memory\n");
rtapi_exit(lib_module_id);
return -EINVAL;
}
}
return 0;
}
int rtapi_app_main(void)
{
int n, retval;
// determine number of instances
for (count = 0; (names[count] != NULL) && (count < MAX_INST); count++);
comp_id = hal_init(cname);
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: hal_init() failed, rc=%d\n", cname, comp_id);
return -1;
}
// check if nr of array members in sample lines configuration string
// correspond with number of instances. initiation is like:
// samples="bb,sf,fus" (bit, float, signed, unsigned)
if (!samples[0]) {
hal_print_msg(RTAPI_MSG_ERR,
"%s : ERROR: a string declaring valid pintype is needed\n",
cname);
hal_exit(comp_id);
return -1;
}
// parse the samples array for sanity
int nr_of_samples;
for (n = 0; (samples[n] != NULL) || (n < count); n++) {
nr_of_samples = return_instance_samples(n);
if (nr_of_samples <= 0) {
hal_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: erroneous number of samples (%d) for instance %d\n",
cname, nr_of_samples, n);
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_DBG,
"%s: there are %d samples for instance %d\n",
cname, nr_of_samples, n);
}
if (n != count) {
hal_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: \"samples=\" array amount mismatch with number of"
" instances\n", cname);
hal_exit(comp_id);
return -1;
}
// done with input checking, and all should be fine
// export variables and function for each DELAYLINE loop
for (n = 0; n < count; n++) {
if ((retval = export_delayline(n))) {
hal_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: loop %d var export failed\n", cname, n);
hal_exit(comp_id);
return -1;
}
}
rtapi_print_msg(RTAPI_MSG_DBG,
"%s: installed %d lines\n", cname, count);
hal_ready(comp_id);
return 0;
}
int hal_retrieve_pinstate(const char *comp_name,
hal_retrieve_pins_callback_t callback,
void *cb_data)
{
int next;
int nvisited = 0;
int result;
hal_comp_t *comp = NULL;
hal_comp_t *owner;
hal_pinstate_t pinstate;
if (hal_data == 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: hal_retrieve_pinstate called before ULAPI init\n");
return -EINVAL;
}
{
hal_pin_t *pin __attribute__((cleanup(halpr_autorelease_mutex)));
/* get mutex before accessing shared data */
rtapi_mutex_get(&(hal_data->mutex));
if (comp_name != NULL) {
comp = halpr_find_comp_by_name(comp_name);
if (comp == NULL) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: hal_retrieve_pinstate: component '%s' not found\n", comp_name);
return -EINVAL;
}
}
// either comp == NULL, so visit all pins
// or comp != NULL, in which case visit only this
// component's pins
// walk the pinlist
next = hal_data->pin_list_ptr;
while (next != 0) {
pin = SHMPTR(next);
owner = SHMPTR(pin->owner_ptr);
if (!comp_name || (owner->comp_id == comp->comp_id)) {
nvisited++;
/* this is the right comp */
if (callback) {
// fill in the details:
// NB: cover remote link case!
pinstate.value = SHMPTR(pin->data_ptr_addr);
pinstate.type = pin->type;
pinstate.dir = pin->dir;
pinstate.epsilon = hal_data->epsilon[pin->eps_index];
pinstate.flags = pin->flags;
strncpy(pinstate.name, pin->name, sizeof(pin->name));
strncpy(pinstate.owner_name, owner->name, sizeof(owner->name));
result = callback(&pinstate, cb_data);
if (result < 0) {
// callback signaled an error, pass that back up.
return result;
} else if (result > 0) {
// callback signaled 'stop iterating'.
// pass back the number of visited pins so far.
return nvisited;
} else {
// callback signaled 'OK to continue'
// fall through
}
} else {
// null callback passed in,
// just count pins
// nvisited already bumped above.
}
}
/* no match, try the next one */
next = pin->next_ptr;
}
hal_print_msg(RTAPI_MSG_DBG,
"HAL: hal_retrieve_pinstate: visited %d pins\n", nvisited);
/* if we get here, we ran through all the pins, so return count */
return nvisited;
}
}
int hal_init_mode(const char *name, int type, int userarg1, int userarg2)
{
int comp_id;
char rtapi_name[RTAPI_NAME_LEN + 1];
char hal_name[HAL_NAME_LEN + 1];
// tag message origin field
rtapi_set_logtag("hal_lib");
if (name == 0) {
hal_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: no component name\n");
return -EINVAL;
}
if (strlen(name) > HAL_NAME_LEN) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: component name '%s' is too long\n", name);
return -EINVAL;
}
// rtapi initialisation already done
// since this happens through the constructor
hal_print_msg(RTAPI_MSG_DBG,
"HAL: initializing component '%s' type=%d arg1=%d arg2=%d/0x%x\n",
name, type, userarg1, userarg2, userarg2);
/* copy name to local vars, truncating if needed */
rtapi_snprintf(rtapi_name, RTAPI_NAME_LEN, "HAL_%s", name);
rtapi_snprintf(hal_name, sizeof(hal_name), "%s", name);
/* do RTAPI init */
comp_id = rtapi_init(rtapi_name);
if (comp_id < 0) {
hal_print_msg(RTAPI_MSG_ERR, "HAL: ERROR: rtapi init failed\n");
return -EINVAL;
}
// tag message origin field since ulapi autoload re-tagged them
rtapi_set_logtag("hal_lib");
#ifdef ULAPI
hal_rtapi_attach();
#endif
{
hal_comp_t *comp __attribute__((cleanup(halpr_autorelease_mutex)));
/* get mutex before manipulating the shared data */
rtapi_mutex_get(&(hal_data->mutex));
/* make sure name is unique in the system */
if (halpr_find_comp_by_name(hal_name) != 0) {
/* a component with this name already exists */
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: duplicate component name '%s'\n", hal_name);
rtapi_exit(comp_id);
return -EINVAL;
}
/* allocate a new component structure */
comp = halpr_alloc_comp_struct();
if (comp == 0) {
/* couldn't allocate structure */
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: insufficient memory for component '%s'\n", hal_name);
rtapi_exit(comp_id);
return -ENOMEM;
}
/* initialize the comp structure */
comp->userarg1 = userarg1;
comp->userarg2 = userarg2;
comp->comp_id = comp_id;
comp->type = type;
#ifdef RTAPI
comp->pid = 0; //FIXME revisit this
#else /* ULAPI */
// a remote component starts out disowned
comp->pid = comp->type == TYPE_REMOTE ? 0 : getpid(); //FIXME revisit this
#endif
comp->state = COMP_INITIALIZING;
comp->last_update = 0;
comp->last_bound = 0;
comp->last_unbound = 0;
comp->shmem_base = hal_shmem_base;
comp->insmod_args = 0;
rtapi_snprintf(comp->name, sizeof(comp->name), "%s", hal_name);
/* insert new structure at head of list */
comp->next_ptr = hal_data->comp_list_ptr;
hal_data->comp_list_ptr = SHMOFF(comp);
}
// scope exited - mutex released
/* done */
hal_print_msg(RTAPI_MSG_DBG,
"HAL: component '%s' initialized, ID = %02d\n", hal_name, comp_id);
return comp_id;
}
int hal_exit(int comp_id)
{
int *prev, next;
char name[HAL_NAME_LEN + 1];
if (hal_data == 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: exit called before init\n");
return -EINVAL;
}
hal_print_msg(RTAPI_MSG_DBG, "HAL: removing component %02d\n", comp_id);
{
hal_comp_t *comp __attribute__((cleanup(halpr_autorelease_mutex)));
/* grab mutex before manipulating list */
rtapi_mutex_get(&(hal_data->mutex));
/* search component list for 'comp_id' */
prev = &(hal_data->comp_list_ptr);
next = *prev;
if (next == 0) {
/* list is empty - should never happen, but... */
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: component %d not found\n", comp_id);
return -EINVAL;
}
comp = SHMPTR(next);
while (comp->comp_id != comp_id) {
/* not a match, try the next one */
prev = &(comp->next_ptr);
next = *prev;
if (next == 0) {
/* reached end of list without finding component */
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: component %d not found\n", comp_id);
return -EINVAL;
}
comp = SHMPTR(next);
}
/* found our component, unlink it from the list */
*prev = comp->next_ptr;
/* save component name for later */
rtapi_snprintf(name, sizeof(name), "%s", comp->name);
/* get rid of the component */
free_comp_struct(comp);
/*! \todo Another #if 0 */
#if 0
/*! \todo FIXME - this is the beginning of a two pronged approach to managing
shared memory. Prong 1 - re-init the shared memory allocator whenever
it is known to be safe. Prong 2 - make a better allocator that can
reclaim memory allocated by components when those components are
removed. To be finished later. */
/* was that the last component? */
if (hal_data->comp_list_ptr == 0) {
/* yes, are there any signals or threads defined? */
if ((hal_data->sig_list_ptr == 0) && (hal_data->thread_list_ptr == 0)) {
/* no, invalidate "magic" number so shmem will be re-inited when
a new component is loaded */
hal_data->magic = 0;
}
}
#endif
// scope exit - mutex released
}
// the RTAPI resources are now released
// on hal_lib shared library unload
rtapi_exit(comp_id);
/* done */
hal_print_msg(RTAPI_MSG_DBG,
"HAL: component %02d removed, name = '%s'\n", comp_id, name);
return 0;
}
static int init_hal_data(void)
{
/* has the block already been initialized? */
if (hal_data->version != 0) {
/* yes, verify version code */
if (hal_data->version == HAL_VER) {
return 0;
} else {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: version code mismatch\n");
return -1;
}
}
/* no, we need to init it, grab the mutex unconditionally */
rtapi_mutex_try(&(hal_data->mutex));
// some heaps contain garbage, like xenomai
memset(hal_data, 0, global_data->hal_size);
/* set version code so nobody else init's the block */
hal_data->version = HAL_VER;
/* initialize everything */
hal_data->comp_list_ptr = 0;
hal_data->pin_list_ptr = 0;
hal_data->sig_list_ptr = 0;
hal_data->param_list_ptr = 0;
hal_data->funct_list_ptr = 0;
hal_data->thread_list_ptr = 0;
hal_data->vtable_list_ptr = 0;
hal_data->base_period = 0;
hal_data->threads_running = 0;
hal_data->oldname_free_ptr = 0;
hal_data->comp_free_ptr = 0;
hal_data->pin_free_ptr = 0;
hal_data->sig_free_ptr = 0;
hal_data->param_free_ptr = 0;
hal_data->funct_free_ptr = 0;
hal_data->vtable_free_ptr = 0;
hal_data->pending_constructor = 0;
hal_data->constructor_prefix[0] = 0;
list_init_entry(&(hal_data->funct_entry_free));
hal_data->thread_free_ptr = 0;
hal_data->exact_base_period = 0;
hal_data->group_list_ptr = 0;
hal_data->member_list_ptr = 0;
hal_data->ring_list_ptr = 0;
hal_data->group_free_ptr = 0;
hal_data->member_free_ptr = 0;
hal_data->ring_free_ptr = 0;
RTAPI_ZERO_BITMAP(&hal_data->rings, HAL_MAX_RINGS);
// silly 1-based shm segment id allocation FIXED
// yeah, 'user friendly', how could one possibly think zero might be a valid id
RTAPI_BIT_SET(hal_data->rings,0);
/* set up for shmalloc_xx() */
hal_data->shmem_bot = sizeof(hal_data_t);
hal_data->shmem_top = global_data->hal_size;
hal_data->lock = HAL_LOCK_NONE;
int i;
for (i = 0; i < MAX_EPSILON; i++)
hal_data->epsilon[i] = 0.0;
hal_data->epsilon[0] = DEFAULT_EPSILON;
/* done, release mutex */
rtapi_mutex_give(&(hal_data->mutex));
return 0;
}
int hal_compile_comp(const char *name, hal_compiled_comp_t **ccomp)
{
hal_compiled_comp_t *tc;
int pincount = 0;
if (!name) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL:%d ERROR: hal_compile_comp() called with NULL name\n",
rtapi_instance);
return -EINVAL;
}
{
hal_comp_t *comp __attribute__((cleanup(halpr_autorelease_mutex)));
int next, n;
hal_comp_t *owner;
hal_pin_t *pin;
rtapi_mutex_get(&(hal_data->mutex));
if ((comp = halpr_find_comp_by_name(name)) == NULL) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL:%d ERROR: hal_comp_compile(%s): no such comp\n",
rtapi_instance, name);
return -EINVAL;
}
// array sizing: count pins owned by this component
next = hal_data->pin_list_ptr;
n = 0;
while (next != 0) {
pin = SHMPTR(next);
owner = SHMPTR(pin->owner_ptr);
if (owner->comp_id == comp->comp_id) {
if (!(pin->flags & PIN_DO_NOT_TRACK))
n++;
pincount++;
}
next = pin->next_ptr;
}
if (n == 0) {
hal_print_msg(RTAPI_MSG_ERR,
"ERROR: component %s has no pins to watch for changes\n",
name);
return -EINVAL;
}
// a compiled comp is a userland/per process memory object
if ((tc = malloc(sizeof(hal_compiled_comp_t))) == NULL)
return -ENOMEM;
memset(tc, 0, sizeof(hal_compiled_comp_t));
tc->comp = comp;
tc->n_pins = n;
// alloc pin array
if ((tc->pin = malloc(sizeof(hal_pin_t *) * tc->n_pins)) == NULL)
return -ENOMEM;
// alloc tracking value array
if ((tc->tracking =
malloc(sizeof(hal_data_u) * tc->n_pins )) == NULL)
return -ENOMEM;
// alloc change bitmap
if ((tc->changed =
malloc(RTAPI_BITMAP_BYTES(tc->n_pins))) == NULL)
return -ENOMEM;
memset(tc->pin, 0, sizeof(hal_pin_t *) * tc->n_pins);
memset(tc->tracking, 0, sizeof(hal_data_u) * tc->n_pins);
RTAPI_ZERO_BITMAP(tc->changed,tc->n_pins);
// fill in pin array
n = 0;
next = hal_data->pin_list_ptr;
while (next != 0) {
pin = SHMPTR(next);
owner = SHMPTR(pin->owner_ptr);
if ((owner->comp_id == comp->comp_id) &&
!(pin->flags & PIN_DO_NOT_TRACK))
tc->pin[n++] = pin;
next = pin->next_ptr;
}
assert(n == tc->n_pins);
tc->magic = CCOMP_MAGIC;
*ccomp = tc;
}
hal_print_msg(RTAPI_MSG_DBG, "hal_compile_comp(%s): %d pins, %d tracked",
name, pincount, tc->n_pins);
return 0;
}
int rtapi_app_main(void)
{
int retval;
void *mem;
rtapi_switch = rtapi_get_handle();
hal_print_msg(RTAPI_MSG_DBG,
"HAL_LIB:%d loading RT support gd=%pp\n",rtapi_instance,global_data);
/* do RTAPI init */
lib_module_id = rtapi_init("HAL_LIB");
if (lib_module_id < 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL_LIB:%d ERROR: rtapi init failed\n",
rtapi_instance);
return -EINVAL;
}
// paranoia
if (global_data == NULL) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL_LIB:%d ERROR: global_data == NULL\n",
rtapi_instance);
return -EINVAL;
}
/* get HAL shared memory block from RTAPI */
lib_mem_id = rtapi_shmem_new(HAL_KEY, lib_module_id, global_data->hal_size);
if (lib_mem_id < 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL_LIB:%d ERROR: could not open shared memory\n",
rtapi_instance);
rtapi_exit(lib_module_id);
return -EINVAL;
}
/* get address of shared memory area */
retval = rtapi_shmem_getptr(lib_mem_id, &mem, 0);
if (retval < 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL_LIB:%d ERROR: could not access shared memory\n",
rtapi_instance);
rtapi_exit(lib_module_id);
return -EINVAL;
}
/* set up internal pointers to shared mem and data structure */
hal_shmem_base = (char *) mem;
hal_data = (hal_data_t *) mem;
/* perform a global init if needed */
retval = init_hal_data();
if ( retval ) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL_LIB:%d ERROR: could not init shared memory\n",
rtapi_instance);
rtapi_exit(lib_module_id);
return -EINVAL;
}
retval = hal_proc_init();
if ( retval ) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL_LIB: ERROR:%d could not init /proc files\n",
rtapi_instance);
rtapi_exit(lib_module_id);
return -EINVAL;
}
/* done */
hal_print_msg(RTAPI_MSG_DBG,
"HAL_LIB:%d kernel lib installed successfully\n",
rtapi_instance);
return 0;
}
int hal_ccomp_match(hal_compiled_comp_t *cc)
{
int i, nchanged = 0;
hal_bit_t halbit;
hal_s32_t hals32;
hal_s32_t halu32;
hal_float_t halfloat,delta;
hal_pin_t *pin;
hal_sig_t *sig;
void *data_ptr;
assert(cc->magic == CCOMP_MAGIC);
RTAPI_ZERO_BITMAP(cc->changed, cc->n_pins);
for (i = 0; i < cc->n_pins; i++) {
pin = cc->pin[i];
if (pin->signal != 0) {
sig = SHMPTR(pin->signal);
data_ptr = SHMPTR(sig->data_ptr);
} else {
data_ptr = hal_shmem_base + SHMOFF(&(pin->dummysig));
}
switch (pin->type) {
case HAL_BIT:
halbit = *((char *) data_ptr);
if (cc->tracking[i].b != halbit) {
nchanged++;
RTAPI_BIT_SET(cc->changed, i);
cc->tracking[i].b = halbit;
}
break;
case HAL_FLOAT:
halfloat = *((hal_float_t *) data_ptr);
delta = HAL_FABS(halfloat - cc->tracking[i].f);
if (delta > hal_data->epsilon[pin->eps_index]) {
nchanged++;
RTAPI_BIT_SET(cc->changed, i);
cc->tracking[i].f = halfloat;
}
break;
case HAL_S32:
hals32 = *((hal_s32_t *) data_ptr);
if (cc->tracking[i].s != hals32) {
nchanged++;
RTAPI_BIT_SET(cc->changed, i);
cc->tracking[i].s = hals32;
}
break;
case HAL_U32:
halu32 = *((hal_u32_t *) data_ptr);
if (cc->tracking[i].u != halu32) {
nchanged++;
RTAPI_BIT_SET(cc->changed, i);
cc->tracking[i].u = halu32;
}
break;
default:
hal_print_msg(RTAPI_MSG_ERR,
"HAL:%d BUG: hal_ccomp_match(%s): invalid type for pin %s: %d\n",
rtapi_instance, cc->comp->name, pin->name, pin->type);
return -EINVAL;
}
}
return nchanged;
}
static int export_delayline(int n)
{
int retval, nr_of_samples, i;
char *str_type;
// determine the required size of the ringbuffer
nr_of_samples = return_instance_samples(n);
size_t sample_size = sizeof(sample_t) + (nr_of_samples * sizeof(hal_data_u));
// add some headroom to be sure we dont overrun
size_t rbsize = record_space(sample_size) * max_delay[n] * RB_HEADROOM;
// create the delay queue
if ((retval = hal_ring_newf(rbsize, sizeof(hal_delayline_t), ALLOC_HALMEM,
"%s.samples", names[n])) < 0) {
hal_print_msg(RTAPI_MSG_ERR,
"%s: failed to create new ring '%s.samples': %d\n",
cname, names[n], retval);
return retval;
}
// use the per-using component ring access structure as the instance data,
// which will also give us a handle on the scratchpad which we use for
// HAL pins and other shared data
if ((instance[n] = hal_malloc(sizeof(ringbuffer_t))) == NULL)
return -1;
if ((retval = hal_ring_attachf(instance[n], NULL, "%s.samples", names[n]))) {
hal_print_msg(RTAPI_MSG_ERR,
"%s: attach to ring '%s.samples' failed: %d\n",
cname, names[n], retval);
return -1;
}
// fill in instance data
hal_delayline_t *hd = instance[n]->scratchpad;
strncpy(hd->name, names[n], sizeof(hd->name));
hd->nsamples = nr_of_samples;
hd->sample_size = sample_size;
hd->max_delay = max_delay[n];
// set delay standard to value of zero
hd->delay = 0;
hd->output_ts = 0;
hd->input_ts = hd->delay;
// init pins, and at the same time fill the puntype array with
// the type of pin[i] so we can later dereference proper
char character;
for (i = 0; i < hd->nsamples; i++) {
character = samples[n][i];
rtapi_print_msg(RTAPI_MSG_DBG, "\"samples=\" string = %s"
" character %d = %c \n",
samples[n], i, character);
// determine type
hal_type_t type = HAL_TYPE_UNSPECIFIED;
switch (character) {
case 'b':
case 'B':
type = HAL_BIT;
break;
case 'f':
case 'F':
type = HAL_FLOAT;
break;
case 's':
type = HAL_S32;
break;
case 'u':
type = HAL_U32;
break;
case 'S':
type = HAL_S64;
break;
case 'U':
type = HAL_U64;
break;
default:
hal_print_msg(RTAPI_MSG_ERR,
"%s: invalid type '%c' for pin %d\n",
cname, character, i);
return -EINVAL;
}
hd->pintype[i] = type;
switch (type) {
case HAL_BIT:
str_type = "bit";
break;
case HAL_FLOAT:
str_type = "flt";
break;
case HAL_U32:
str_type = "u32";
break;
case HAL_S32:
str_type = "s32";
break;
case HAL_S64:
str_type = "s64";
break;
case HAL_U64:
str_type = "u64";
break;
case HAL_TYPE_MAX:
case HAL_TYPE_UNSPECIFIED:
// do nothing
break;
}
// create pins of type as requested
if (((retval = hal_pin_newf(type,
HAL_IN,
(void **) &hd->pins_in[i],
comp_id,
"%s.in%d-%s",
hd->name, i, str_type)) < 0) ||
((retval = hal_pin_newf(type,
HAL_OUT,
(void **) &hd->pins_out[i],
comp_id,
"%s.out%d-%s",
hd->name, i, str_type)) < 0)) {
return retval;
}
// post hal_pin_new(), pins are guaranteed to be set to zero
}
// create other pins
if (((retval = hal_pin_bit_newf(HAL_IN, &(hd->enable), comp_id,
"%s.enable", hd->name))) ||
((retval = hal_pin_bit_newf(HAL_IN, &(hd->abort), comp_id,
"%s.abort", hd->name))) ||
((retval = hal_pin_u32_newf(HAL_IN, &(hd->pin_delay), comp_id,
"%s.delay", hd->name))) ||
((retval = hal_pin_u32_newf(HAL_OUT, &(hd->write_fail), comp_id,
"%s.write-fail", hd->name))) ||
((retval = hal_pin_u32_newf(HAL_OUT, &(hd->read_fail), comp_id,
"%s.too-old", hd->name))) ||
((retval = hal_pin_u32_newf(HAL_OUT, &(hd->too_old), comp_id,
"%s.read-fail", hd->name))))
return retval;
// export thread functions
if (((retval = hal_export_functf(write_sample_to_ring,
instance[n], 1, 0, comp_id,
"%s.sample", hd->name)) < 0) ||
((retval = hal_export_functf(read_sample_from_ring,
instance[n], 1, 0, comp_id,
"%s.output", hd->name)) < 0)) {
return retval;
}
return 0;
}
int hal_retrieve_compstate(const char *comp_name,
hal_retrieve_compstate_callback_t callback,
void *cb_data)
{
int next;
int nvisited = 0;
int result;
hal_compstate_t state;
if (hal_data == 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: ERROR: hal_retrieve_compstate called before ULAPI init\n");
return -EINVAL;
}
{
hal_comp_t *comp __attribute__((cleanup(halpr_autorelease_mutex)));
/* get mutex before accessing shared data */
rtapi_mutex_get(&(hal_data->mutex));
/* search for the comp */
next = hal_data->comp_list_ptr;
while (next != 0) {
comp = SHMPTR(next);
if (!comp_name || (strcmp(comp->name, comp_name)) == 0) {
nvisited++;
/* this is the right comp */
if (callback) {
// fill in the details:
state.type = comp->type;
state.state = comp->state;
state.last_update = comp->last_update;
state.last_bound = comp->last_bound;
state.last_unbound = comp->last_unbound;
state.pid = comp->pid;
state.insmod_args = comp->insmod_args;
strncpy(state.name, comp->name, sizeof(comp->name));
result = callback(&state, cb_data);
if (result < 0) {
// callback signaled an error, pass that back up.
return result;
} else if (result > 0) {
// callback signaled 'stop iterating'.
// pass back the number of visited comps so far.
return nvisited;
} else {
// callback signaled 'OK to continue'
// fall through
}
} else {
// null callback passed in,
// just count comps
// nvisited already bumped above.
}
}
/* no match, try the next one */
next = comp->next_ptr;
}
// hal_print_msg(RTAPI_MSG_DBG,
// "HAL: hal_retrieve_compstate: visited %d comps\n", nvisited);
/* if we get here, we ran through all the comps, so return count */
return nvisited;
}
}
