int hal_ccomp_report(hal_compiled_comp_t *cc,
comp_report_callback_t report_cb,
void *cb_data, int report_all)
{
int retval, i;
hal_data_u *data_ptr;
hal_pin_t *pin;
hal_sig_t *sig;
if (!report_cb)
return 0;
if ((retval = report_cb(REPORT_BEGIN, cc, NULL, NULL, cb_data)) < 0)
return retval;
for (i = 0; i < cc->n_pins; i++) {
if (report_all || RTAPI_BIT_TEST(cc->changed, i)) {
pin = cc->pin[i];
if (pin->signal != 0) {
sig = SHMPTR(pin->signal);
data_ptr = (hal_data_u *)SHMPTR(sig->data_ptr);
} else {
data_ptr = (hal_data_u *)(hal_shmem_base + SHMOFF(&(pin->dummysig)));
}
if ((retval = report_cb(REPORT_PIN, cc, pin,
data_ptr, cb_data)) < 0)
return retval;
}
}
return report_cb(REPORT_END, cc, NULL, NULL, cb_data);
}
void free_oldname_struct(hal_oldname_t * oldname)
{
/* clear contents of struct */
oldname->name[0] = '\0';
/* add it to free list */
oldname->next_ptr = hal_data->oldname_free_ptr;
hal_data->oldname_free_ptr = SHMOFF(oldname);
}
void free_funct_struct(hal_funct_t * funct)
{
int next_thread;
hal_thread_t *thread;
hal_list_t *list_root, *list_entry;
hal_funct_entry_t *funct_entry;
if (funct->users > 0) {
/* We can't casually delete the function, there are thread(s) which
will call it. So we must check all the threads and remove any
funct_entrys that call this function */
/* start at root of thread list */
next_thread = hal_data->thread_list_ptr;
/* run through thread list */
while (next_thread != 0) {
/* point to thread */
thread = SHMPTR(next_thread);
/* start at root of funct_entry list */
list_root = &(thread->funct_list);
list_entry = list_next(list_root);
/* run thru funct_entry list */
while (list_entry != list_root) {
/* point to funct entry */
funct_entry = (hal_funct_entry_t *) list_entry;
/* test it */
if (SHMPTR(funct_entry->funct_ptr) == funct) {
/* this funct entry points to our funct, unlink */
list_entry = list_remove_entry(list_entry);
/* and delete it */
free_funct_entry_struct(funct_entry);
} else {
/* no match, try the next one */
list_entry = list_next(list_entry);
}
}
/* move on to the next thread */
next_thread = thread->next_ptr;
}
}
/* clear contents of struct */
funct->uses_fp = 0;
funct->owner_id = 0;
funct->reentrant = 0;
funct->users = 0;
funct->arg = 0;
funct->funct.l = 0;
funct->runtime = 0;
funct->name[0] = '\0';
/* add it to free list */
funct->next_ptr = hal_data->funct_free_ptr;
hal_data->funct_free_ptr = SHMOFF(funct);
}
int hal_xinit(const int type,
const int userarg1,
const int userarg2,
const hal_constructor_t ctor,
const hal_destructor_t dtor,
const char *name)
{
int comp_id, retval;
rtapi_set_logtag("hal_lib");
CHECK_STRLEN(name, HAL_NAME_LEN);
// sanity: these must have been inited before by the
// respective rtapi.so/.ko module
CHECK_NULL(rtapi_switch);
if ((dtor != NULL) && (ctor == NULL)) {
HALERR("component '%s': NULL constructor doesnt make"
" sense with non-NULL destructor", name);
return -EINVAL;
}
// RTAPI initialisation already done
HALDBG("initializing component '%s' type=%d arg1=%d arg2=%d/0x%x",
name, type, userarg1, userarg2, userarg2);
if ((lib_module_id < 0) && (type != TYPE_HALLIB)) {
// if hal_lib not inited yet, do so now - recurse
#ifdef RTAPI
retval = hal_xinit(TYPE_HALLIB, 0, 0, NULL, NULL, "hal_lib");
#else
retval = hal_xinitf(TYPE_HALLIB, 0, 0, NULL, NULL, "hal_lib%ld",
(long) getpid());
#endif
if (retval < 0)
return retval;
}
// tag message origin field since ulapi autoload re-tagged them temporarily
rtapi_set_logtag("hal_lib");
/* copy name to local vars, truncating if needed */
char rtapi_name[RTAPI_NAME_LEN + 1];
char hal_name[HAL_NAME_LEN + 1];
rtapi_snprintf(hal_name, sizeof(hal_name), "%s", name);
rtapi_snprintf(rtapi_name, RTAPI_NAME_LEN, "HAL_%s", hal_name);
/* do RTAPI init */
comp_id = rtapi_init(rtapi_name);
if (comp_id < 0) {
HALERR("rtapi init(%s) failed", rtapi_name);
return -EINVAL;
}
// recursing? init HAL shm
if ((lib_module_id < 0) && (type == TYPE_HALLIB)) {
// recursion case, we're initing hal_lib
// get HAL shared memory from RTAPI
int shm_id = rtapi_shmem_new(HAL_KEY,
comp_id,
global_data->hal_size);
if (shm_id < 0) {
HALERR("hal_lib:%d failed to allocate HAL shm %x, rc=%d",
comp_id, HAL_KEY, shm_id);
rtapi_exit(comp_id);
return -EINVAL;
}
// retrieve address of HAL shared memory segment
void *mem;
retval = rtapi_shmem_getptr(shm_id, &mem, 0);
if (retval < 0) {
HALERR("hal_lib:%d failed to acquire HAL shm %x, id=%d rc=%d",
comp_id, HAL_KEY, shm_id, retval);
rtapi_exit(comp_id);
return -EINVAL;
}
// set up internal pointers to shared mem and data structure
hal_shmem_base = (char *) mem;
hal_data = (hal_data_t *) mem;
#ifdef RTAPI
// only on RTAPI hal_lib initialization:
// initialize up the HAL shm segment
retval = init_hal_data();
if (retval) {
HALERR("could not init HAL shared memory rc=%d", retval);
rtapi_exit(lib_module_id);
lib_module_id = -1;
return -EINVAL;
}
retval = hal_proc_init();
if (retval) {
HALERR("could not init /proc files");
rtapi_exit(lib_module_id);
lib_module_id = -1;
return -EINVAL;
}
#endif
// record hal_lib comp_id
lib_module_id = comp_id;
// and the HAL shm segmed id
lib_mem_id = shm_id;
}
// global_data MUST be at hand now:
HAL_ASSERT(global_data != NULL);
// paranoia
HAL_ASSERT(hal_shmem_base != NULL);
HAL_ASSERT(hal_data != NULL);
HAL_ASSERT(lib_module_id > -1);
HAL_ASSERT(lib_mem_id > -1);
if (lib_module_id < 0) {
HALERR("giving up");
return -EINVAL;
}
{
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 */
HALERR("duplicate component name '%s'", hal_name);
rtapi_exit(comp_id);
return -EINVAL;
}
/* allocate a new component structure */
comp = halpr_alloc_comp_struct();
if (comp == 0) {
HALERR("insufficient memory for component '%s'", 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;
comp->ctor = ctor;
comp->dtor = dtor;
#ifdef RTAPI
comp->pid = 0;
#else /* ULAPI */
// a remote component starts out disowned
comp->pid = comp->type == TYPE_REMOTE ? 0 : getpid();
#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
// finish hal_lib initialisation
// in ULAPI this will happen after the recursion on hal_lib%d unwinds
if (type == TYPE_HALLIB) {
#ifdef RTAPI
// only on RTAPI hal_lib initialization:
// export the instantiation support userfuncts
hal_export_xfunct_args_t ni = {
.type = FS_USERLAND,
.funct.u = create_instance,
.arg = NULL,
.owner_id = lib_module_id
};
if ((retval = hal_export_xfunctf( &ni, "newinst")) < 0)
return retval;
hal_export_xfunct_args_t di = {
.type = FS_USERLAND,
.funct.u = delete_instance,
.arg = NULL,
.owner_id = lib_module_id
};
if ((retval = hal_export_xfunctf( &di, "delinst")) < 0)
return retval;
#endif
retval = hal_ready(lib_module_id);
if (retval)
HALERR("hal_ready(%d) failed rc=%d", lib_module_id, retval);
else
HALDBG("%s initialization complete", hal_name);
return retval;
}
HALDBG("%s component '%s' id=%d initialized%s",
(ctor != NULL) ? "instantiable" : "legacy",
hal_name, comp_id,
(dtor != NULL) ? ", has destructor" : "");
return comp_id;
}
int hal_exit(int comp_id)
{
int *prev, next, comptype;
char name[HAL_NAME_LEN + 1];
CHECK_HALDATA();
HALDBG("removing component %d", 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... */
HALERR("no components defined");
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 */
HALERR("no such component with id %d", comp_id);
return -EINVAL;
}
comp = SHMPTR(next);
}
// record type, since we're about to zap the comp in free_comp_struct()
comptype = comp->type;
/* save component name for later */
rtapi_snprintf(name, sizeof(name), "%s", comp->name);
/* get rid of the component */
free_comp_struct(comp);
// unlink the comp only now as free_comp_struct() must
// determine ownership of pins/params/functs and this
// requires access to the current comp, too
// since this is all under lock it should not matter
*prev = comp->next_ptr;
// add it to free list
comp->next_ptr = hal_data->comp_free_ptr;
hal_data->comp_free_ptr = SHMOFF(comp);
// scope exit - mutex released
}
// if unloading the hal_lib component, destroy HAL shm
if (comptype == TYPE_HALLIB) {
int retval;
/* release RTAPI resources */
retval = rtapi_shmem_delete(lib_mem_id, comp_id);
if (retval) {
HALERR("rtapi_shmem_delete(%d,%d) failed: %d",
lib_mem_id, comp_id, retval);
}
// HAL shm is history, take note ASAP
lib_mem_id = -1;
hal_shmem_base = NULL;
hal_data = NULL;;
retval = rtapi_exit(comp_id);
if (retval) {
HALERR("rtapi_exit(%d) failed: %d",
lib_module_id, retval);
}
// the hal_lib RTAPI module is history, too
// in theory we'd be back to square 1
lib_module_id = -1;
} else {
// the standard case
rtapi_exit(comp_id);
}
//HALDBG("component '%s' id=%d removed", name, comp_id);
return 0;
}
int set_channel_source(int chan_num, int type, char *name)
{
scope_vert_t *vert;
scope_chan_t *chan;
hal_pin_t *pin;
hal_sig_t *sig;
hal_param_t *param;
vert = &(ctrl_usr->vert);
chan = &(ctrl_usr->chan[chan_num - 1]);
/* locate the selected item in the HAL */
if (type == 0) {
/* search the pin list */
pin = halpr_find_pin_by_name(name);
if (pin == NULL) {
/* pin not found */
return -1;
}
chan->data_source_type = 0;
chan->data_source = SHMOFF(pin);
chan->data_type = pin->type;
chan->name = pin->name;
} else if (type == 1) {
/* search the signal list */
sig = halpr_find_sig_by_name(name);
if (sig == NULL) {
/* signal not found */
return -1;
}
chan->data_source_type = 1;
chan->data_source = SHMOFF(sig);
chan->data_type = sig->type;
chan->name = sig->name;
} else if (type == 2) {
/* search the parameter list */
param = halpr_find_param_by_name(name);
if (param == NULL) {
/* parameter not found */
return -1;
}
chan->data_source_type = 2;
chan->data_source = SHMOFF(param);
chan->data_type = param->type;
chan->name = param->name;
}
switch (chan->data_type) {
case HAL_BIT:
chan->data_len = sizeof(hal_bit_t);
chan->min_index = -2;
chan->max_index = 2;
break;
case HAL_FLOAT:
chan->data_len = sizeof(hal_float_t);
chan->min_index = -36;
chan->max_index = 36;
break;
case HAL_S32:
chan->data_len = sizeof(hal_s32_t);
chan->min_index = -2;
chan->max_index = 30;
break;
case HAL_U32:
chan->data_len = sizeof(hal_u32_t);
chan->min_index = -2;
chan->max_index = 30;
break;
default:
/* Shouldn't get here, but just in case... */
chan->data_len = 0;
chan->min_index = -1;
chan->max_index = 1;
}
/* invalidate any data in the buffer for this channel */
vert->data_offset[chan_num - 1] = -1;
/* set scale and offset to nominal values */
chan->vert_offset = 0.0;
chan->scale_index = 0;
/* return success */
return 0;
}
static void free_comp_struct(hal_comp_t * comp)
{
int *prev, next;
#ifdef RTAPI
hal_funct_t *funct;
#endif /* RTAPI */
hal_pin_t *pin;
hal_param_t *param;
/* can't delete the component until we delete its "stuff" */
/* need to check for functs only if a realtime component */
#ifdef RTAPI
/* search the function list for this component's functs */
prev = &(hal_data->funct_list_ptr);
next = *prev;
while (next != 0) {
funct = SHMPTR(next);
if (SHMPTR(funct->owner_ptr) == comp) {
/* this function belongs to our component, unlink from list */
*prev = funct->next_ptr;
/* and delete it */
free_funct_struct(funct);
} else {
/* no match, try the next one */
prev = &(funct->next_ptr);
}
next = *prev;
}
#endif /* RTAPI */
/* search the pin list for this component's pins */
prev = &(hal_data->pin_list_ptr);
next = *prev;
while (next != 0) {
pin = SHMPTR(next);
if (SHMPTR(pin->owner_ptr) == comp) {
/* this pin belongs to our component, unlink from list */
*prev = pin->next_ptr;
/* and delete it */
free_pin_struct(pin);
} else {
/* no match, try the next one */
prev = &(pin->next_ptr);
}
next = *prev;
}
/* search the parameter list for this component's parameters */
prev = &(hal_data->param_list_ptr);
next = *prev;
while (next != 0) {
param = SHMPTR(next);
if (SHMPTR(param->owner_ptr) == comp) {
/* this param belongs to our component, unlink from list */
*prev = param->next_ptr;
/* and delete it */
free_param_struct(param);
} else {
/* no match, try the next one */
prev = &(param->next_ptr);
}
next = *prev;
}
/* now we can delete the component itself */
/* clear contents of struct */
comp->comp_id = -1;
comp->type = TYPE_INVALID;
comp->state = COMP_INVALID;
comp->last_bound = 0;
comp->last_unbound = 0;
comp->last_update = 0;
comp->shmem_base = 0;
comp->name[0] = '\0';
/* add it to free list */
comp->next_ptr = hal_data->comp_free_ptr;
hal_data->comp_free_ptr = SHMOFF(comp);
}
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;
}
// almost a copy of free_comp_struct(), but based on instance
//
// unlinks and frees functs exported by this instance
// then calls any custom destructor
// then unlinks & deletes all pins owned by this instance
// then deletes params owned by this instance
void free_inst_struct(hal_inst_t * inst)
{
int *prev, next;
#ifdef RTAPI
hal_funct_t *funct;
#endif /* RTAPI */
hal_pin_t *pin;
hal_param_t *param;
/* can't delete the instance until we delete its "stuff" */
/* need to check for functs only if a realtime component */
#ifdef RTAPI
/* search the function list for this instance's functs */
prev = &(hal_data->funct_list_ptr);
next = *prev;
while (next != 0) {
funct = SHMPTR(next);
if (funct->owner_id == inst->inst_id) {
/* this function belongs to this instance, unlink from list */
*prev = funct->next_ptr;
/* and delete it */
free_funct_struct(funct);
} else {
/* no match, try the next one */
prev = &(funct->next_ptr);
}
next = *prev;
}
// now that the funct is gone, call the dtor for this instance
// get owning comp
hal_comp_t *comp = halpr_find_owning_comp(inst->comp_id);
if (comp->dtor) {
//NB - pins, params etc still intact
// this instance is owned by this comp, call destructor
HALDBG("calling custom destructor(%s,%s)", comp->name, inst->name);
comp->dtor(inst->name, SHMPTR(inst->inst_data_ptr), inst->inst_size);
}
#endif /* RTAPI */
/* search the pin list for this instance's pins */
prev = &(hal_data->pin_list_ptr);
next = *prev;
while (next != 0) {
pin = SHMPTR(next);
if (pin->owner_id == inst->inst_id) {
/* this pin belongs to our instance, unlink from list */
*prev = pin->next_ptr;
/* and delete it */
free_pin_struct(pin);
} else {
/* no match, try the next one */
prev = &(pin->next_ptr);
}
next = *prev;
}
/* search the parameter list for this instance's parameters */
prev = &(hal_data->param_list_ptr);
next = *prev;
while (next != 0) {
param = SHMPTR(next);
if (param->owner_id == inst->inst_id) {
/* this param belongs to our instance, unlink from list */
*prev = param->next_ptr;
/* and delete it */
free_param_struct(param);
} else {
/* no match, try the next one */
prev = &(param->next_ptr);
}
next = *prev;
}
/* now we can delete the instance itself */
// search the instance list and unlink instances owned by this comp
hal_inst_t *ip;
prev = &(hal_data->inst_list_ptr);
next = *prev;
while (next != 0) {
ip = SHMPTR(next);
if (ip == inst) {
// this instance is owned by this comp
*prev = ip->next_ptr;
// zap the instance structure
ip->comp_id = 0;
ip->inst_id = 0;
ip->inst_data_ptr = 0; // NB - loosing HAL memory here
ip->inst_size = 0;
ip->name[0] = '\0';
// add it to free list
ip->next_ptr = hal_data->inst_free_ptr;
hal_data->inst_free_ptr = SHMOFF(ip);
} else {
prev = &(ip->next_ptr);
}
next = *prev;
}
}
int hal_inst_create(const char *name, const int comp_id, const int size,
void **inst_data)
{
CHECK_HALDATA();
CHECK_STR(name);
{
hal_inst_t *inst __attribute__((cleanup(halpr_autorelease_mutex)));
hal_comp_t *comp;
void *m = NULL;
rtapi_mutex_get(&(hal_data->mutex));
// comp must exist
if ((comp = halpr_find_comp_by_id(comp_id)) == 0) {
HALERR("comp %d not found", comp_id);
return -ENOENT;
}
// inst may not exist
if ((inst = halpr_find_inst_by_name(name)) != NULL) {
HALERR("instance '%s' already exists", name);
return -EEXIST;
}
if (size > 0) {
m = shmalloc_up(size);
if (m == NULL)
NOMEM(" instance %s: cant allocate %d bytes", name, size);
}
memset(m, 0, size);
// allocate instance descriptor
if ((inst = alloc_inst_struct()) == NULL)
NOMEM("instance '%s'", name);
inst->comp_id = comp->comp_id;
inst->inst_id = rtapi_next_handle();
inst->inst_data_ptr = SHMOFF(m);
inst->inst_size = size;
rtapi_snprintf(inst->name, sizeof(inst->name), "%s", name);
HALDBG("%s: creating instance '%s' size %d at ^ %d/%p base=%p",
#ifdef RTAPI
"rtapi",
#else
"ulapi",
#endif
name, size, inst->inst_data_ptr, m, hal_shmem_base);
// if not NULL, pass pointer to blob
if (inst_data)
*(inst_data) = m;
// make it visible
inst->next_ptr = hal_data->inst_list_ptr;
hal_data->inst_list_ptr = SHMOFF(inst);
return inst->inst_id;
}
}
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:
HALERR("BUG: hal_ccomp_match(%s): invalid type for pin %s: %d",
cc->comp->name, pin->name, pin->type);
return -EINVAL;
}
}
return nchanged;
}
static void free_ring_struct(hal_ring_t * p)
{
/* add it to free list */
p->next_ptr = hal_data->ring_free_ptr;
hal_data->ring_free_ptr = SHMOFF(p);
}
int hal_ring_new(const char *name, int size, int sp_size, int mode)
{
hal_ring_t *rbdesc;
int *prev, next, cmp, retval;
int ring_id;
ringheader_t *rhptr;
CHECK_HALDATA();
CHECK_STRLEN(name, HAL_NAME_LEN);
CHECK_LOCK(HAL_LOCK_LOAD);
{
hal_ring_t *ptr __attribute__((cleanup(halpr_autorelease_mutex)));
rtapi_mutex_get(&(hal_data->mutex));
// make sure no such ring name already exists
if ((ptr = halpr_find_ring_by_name(name)) != 0) {
HALERR("ring '%s' already exists", name);
return -EEXIST;
}
// allocate a new ring id - needed since we dont track ring shm
// segments in RTAPI
if ((ring_id = next_ring_id()) < 0) {
HALERR("cant allocate new ring id for '%s'", name);
return -ENOMEM;
}
// allocate a new ring descriptor
if ((rbdesc = alloc_ring_struct()) == 0)
NOMEM("ring '%s'", name);
rbdesc->handle = rtapi_next_handle();
rbdesc->flags = mode;
rbdesc->ring_id = ring_id;
// make total allocation fit ringheader, ringbuffer and scratchpad
rbdesc->total_size = ring_memsize( rbdesc->flags, size, sp_size);
if (rbdesc->flags & ALLOC_HALMEM) {
void *ringmem = shmalloc_up(rbdesc->total_size);
if (ringmem == NULL)
NOMEM("ring '%s' size %d - insufficient HAL memory for ring",
name,rbdesc->total_size);
rbdesc->ring_offset = SHMOFF(ringmem);
rhptr = ringmem;
} else {
// allocate shared memory segment for ring and init
rbdesc->ring_shmkey = OS_KEY((RTAPI_RING_SHM_KEY + ring_id), rtapi_instance);
int shmid;
// allocate an RTAPI shm segment owned by HAL_LIB_xxx
if ((shmid = rtapi_shmem_new(rbdesc->ring_shmkey, lib_module_id,
rbdesc->total_size)) < 0)
NOMEM("rtapi_shmem_new(0x%8.8x,%d) failed: %d",
rbdesc->ring_shmkey, lib_module_id,
rbdesc->total_size);
// map the segment now so we can fill in the ringheader details
if ((retval = rtapi_shmem_getptr(shmid,
(void **)&rhptr, 0)) < 0)
NOMEM("rtapi_shmem_getptr for %d failed %d",
shmid, retval);
}
HALDBG("created ring '%s' in %s, total_size=%d",
name, (rbdesc->flags & ALLOC_HALMEM) ? "halmem" : "shm",
rbdesc->total_size);
ringheader_init(rhptr, rbdesc->flags, size, sp_size);
rhptr->refcount = 0; // on hal_ring_attach: increase; on hal_ring_detach: decrease
rtapi_snprintf(rbdesc->name, sizeof(rbdesc->name), "%s", name);
rbdesc->next_ptr = 0;
// search list for 'name' and insert new structure
prev = &(hal_data->ring_list_ptr);
next = *prev;
while (1) {
if (next == 0) {
/* reached end of list, insert here */
rbdesc->next_ptr = next;
*prev = SHMOFF(rbdesc);
return 0;
}
ptr = SHMPTR(next);
cmp = strcmp(ptr->name, rbdesc->name);
if (cmp > 0) {
/* found the right place for it, insert here */
rbdesc->next_ptr = next;
*prev = SHMOFF(rbdesc);
return 0;
}
/* didn't find it yet, look at next one */
prev = &(ptr->next_ptr);
next = *prev;
}
// automatic unlock by scope exit
}
}
static int hal_export_xfunctfv(const hal_export_xfunct_args_t *xf, const char *fmt, va_list ap)
{
int *prev, next, cmp, sz;
hal_funct_t *nf, *fptr;
char name[HAL_NAME_LEN + 1];
CHECK_HALDATA();
CHECK_LOCK(HAL_LOCK_LOAD);
sz = rtapi_vsnprintf(name, sizeof(name), fmt, ap);
if(sz == -1 || sz > HAL_NAME_LEN) {
HALERR("length %d invalid for name starting '%s'", sz, name);
return -ENOMEM;
}
HALDBG("exporting function '%s' type %d", name, xf->type);
{
hal_comp_t *comp __attribute__((cleanup(halpr_autorelease_mutex)));
/* get mutex before accessing shared data */
rtapi_mutex_get(&(hal_data->mutex));
comp = halpr_find_owning_comp(xf->owner_id);
if (comp == 0) {
/* bad comp_id */
HALERR("funct '%s': owning component %d not found",
name, xf->owner_id);
return -EINVAL;
}
if (comp->type == TYPE_USER) {
/* not a realtime component */
HALERR("funct '%s': component %s/%d is not realtime (%d)",
name, comp->name, comp->comp_id, comp->type);
return -EINVAL;
}
bool legacy = (halpr_find_inst_by_id(xf->owner_id) == NULL);
// instances may export functs post hal_ready
if (legacy && (comp->state > COMP_INITIALIZING)) {
HALERR("funct '%s': called after hal_ready", name);
return -EINVAL;
}
/* allocate a new function structure */
nf = alloc_funct_struct();
if (nf == 0)
NOMEM("function '%s'", name);
/* initialize the structure */
nf->uses_fp = xf->uses_fp;
nf->owner_id = xf->owner_id;
nf->reentrant = xf->reentrant;
nf->users = 0;
nf->handle = rtapi_next_handle();
nf->arg = xf->arg;
nf->type = xf->type;
nf->funct.l = xf->funct.l; // a bit of a cheat really
rtapi_snprintf(nf->name, sizeof(nf->name), "%s", name);
/* search list for 'name' and insert new structure */
prev = &(hal_data->funct_list_ptr);
next = *prev;
while (1) {
if (next == 0) {
/* reached end of list, insert here */
nf->next_ptr = next;
*prev = SHMOFF(nf);
/* break out of loop and init the new function */
break;
}
fptr = SHMPTR(next);
cmp = strcmp(fptr->name, nf->name);
if (cmp > 0) {
/* found the right place for it, insert here */
nf->next_ptr = next;
*prev = SHMOFF(nf);
/* break out of loop and init the new function */
break;
}
if (cmp == 0) {
/* name already in list, can't insert */
free_funct_struct(nf);
HALERR("duplicate function '%s'", name);
return -EINVAL;
}
/* didn't find it yet, look at next one */
prev = &(fptr->next_ptr);
next = *prev;
}
// at this point we have a new function and can
// yield the mutex by scope exit
}
/* init time logging variables */
nf->runtime = 0;
nf->maxtime = 0;
nf->maxtime_increased = 0;
/* at this point we have a new function and can yield the mutex */
rtapi_mutex_give(&(hal_data->mutex));
switch (xf->type) {
case FS_LEGACY_THREADFUNC:
case FS_XTHREADFUNC:
/* create a pin with the function's runtime in it */
if (hal_pin_s32_newf(HAL_OUT, &(nf->runtime), xf->owner_id, "%s.time",name)) {
HALERR("failed to create pin '%s.time'", name);
return -EINVAL;
}
*(nf->runtime) = 0;
/* note that failure to successfully create the following params
does not cause the "export_funct()" call to fail - they are
for debugging and testing use only */
/* create a parameter with the function's maximum runtime in it */
nf->maxtime = 0;
hal_param_s32_newf(HAL_RW, &(nf->maxtime), xf->owner_id, "%s.tmax", name);
/* create a parameter with the function's maximum runtime in it */
nf->maxtime_increased = 0;
hal_param_bit_newf(HAL_RO, &(nf->maxtime_increased), xf->owner_id,
"%s.tmax-inc", name);
break;
case FS_USERLAND: // no timing pins/params
;
}
return 0;
}
int hal_add_funct_to_thread(const char *funct_name,
const char *thread_name, int position)
{
hal_funct_t *funct;
hal_list_t *list_root, *list_entry;
int n;
hal_funct_entry_t *funct_entry;
CHECK_HALDATA();
CHECK_LOCK(HAL_LOCK_CONFIG);
CHECK_STR(funct_name);
CHECK_STR(thread_name);
HALDBG("adding function '%s' to thread '%s'", funct_name, thread_name);
{
hal_thread_t *thread __attribute__((cleanup(halpr_autorelease_mutex)));
/* get mutex before accessing data structures */
rtapi_mutex_get(&(hal_data->mutex));
/* make sure position is valid */
if (position == 0) {
/* zero is not allowed */
HALERR("bad position: 0");
return -EINVAL;
}
/* search function list for the function */
funct = halpr_find_funct_by_name(funct_name);
if (funct == 0) {
HALERR("function '%s' not found", funct_name);
return -EINVAL;
}
// type-check the functions which go onto threads
switch (funct->type) {
case FS_LEGACY_THREADFUNC:
case FS_XTHREADFUNC:
break;
default:
HALERR("cant add type %d function '%s' "
"to a thread", funct->type, funct_name);
return -EINVAL;
}
/* found the function, is it available? */
if ((funct->users > 0) && (funct->reentrant == 0)) {
HALERR("function '%s' may only be added "
"to one thread", funct_name);
return -EINVAL;
}
/* search thread list for thread_name */
thread = halpr_find_thread_by_name(thread_name);
if (thread == 0) {
/* thread not found */
HALERR("thread '%s' not found", thread_name);
return -EINVAL;
}
#if 0
/* ok, we have thread and function, are they compatible? */
if ((funct->uses_fp) && (!thread->uses_fp)) {
HALERR("function '%s' needs FP", funct_name);
return -EINVAL;
}
#endif
/* find insertion point */
list_root = &(thread->funct_list);
list_entry = list_root;
n = 0;
if (position > 0) {
/* insertion is relative to start of list */
while (++n < position) {
/* move further into list */
list_entry = list_next(list_entry);
if (list_entry == list_root) {
/* reached end of list */
HALERR("position '%d' is too high", position);
return -EINVAL;
}
}
} else {
/* insertion is relative to end of list */
while (--n > position) {
/* move further into list */
list_entry = list_prev(list_entry);
if (list_entry == list_root) {
/* reached end of list */
HALERR("position '%d' is too low", position);
return -EINVAL;
}
}
/* want to insert before list_entry, so back up one more step */
list_entry = list_prev(list_entry);
}
/* allocate a funct entry structure */
funct_entry = alloc_funct_entry_struct();
if (funct_entry == 0)
NOMEM("thread->function link");
/* init struct contents */
funct_entry->funct_ptr = SHMOFF(funct);
funct_entry->arg = funct->arg;
funct_entry->funct.l = funct->funct.l;
funct_entry->type = funct->type;
/* add the entry to the list */
list_add_after((hal_list_t *) funct_entry, list_entry);
/* update the function usage count */
funct->users++;
}
return 0;
}
void start_capture(void)
{
int n;
scope_chan_t *chan;
hal_pin_t *pin;
hal_sig_t *sig;
hal_param_t *param;
if (ctrl_shm->state != IDLE) {
/* already running! */
return;
}
for (n = 0; n < 16; n++) {
/* point to user space channel data */
chan = &(ctrl_usr->chan[n]);
/* find address of data in shared memory */
if ( chan->data_source_type == 0 ) {
/* channel source is a pin, point at it */
pin = SHMPTR(chan->data_source);
/* make sure it's still valid */
if ( pin->name[0] == '\0' ) {
/* pin has been deleted */
chan->data_source_type = -1;
chan->data_len = 0;
break;
}
/* point at pin data */
if (pin->signal == 0) {
/* pin is unlinked, get data from dummysig */
ctrl_shm->data_offset[n] = SHMOFF(&(pin->dummysig));
} else {
/* pin is linked to a signal */
sig = SHMPTR(pin->signal);
ctrl_shm->data_offset[n] = sig->data_ptr;
}
} else if ( chan->data_source_type == 1 ) {
/* channel source is a signal, point at it */
sig = SHMPTR(chan->data_source);
/* make sure it's still valid */
if ( sig->name[0] == '\0' ) {
/* signal has been deleted */
chan->data_source_type = -1;
chan->data_len = 0;
break;
}
ctrl_shm->data_offset[n] = sig->data_ptr;
} else if ( chan->data_source_type == 2 ) {
/* channel source is a parameter, point at it */
param = SHMPTR(chan->data_source);
/* make sure it's still valid */
if ( param->name[0] == '\0' ) {
/* param has been deleted */
chan->data_source_type = -1;
chan->data_len = 0;
break;
}
ctrl_shm->data_offset[n] = param->data_ptr;
} else {
/* channel source is invalid */
chan->data_len = 0;
}
/* set data type */
ctrl_shm->data_type[n] = chan->data_type;
/* set data length - zero means don't sample */
if (ctrl_usr->vert.chan_enabled[n]) {
ctrl_shm->data_len[n] = chan->data_len;
} else {
ctrl_shm->data_len[n] = 0;
}
}
ctrl_shm->pre_trig = ctrl_shm->rec_len * ctrl_usr->trig.position;
ctrl_shm->state = INIT;
}