int rtapi_app_main(void)
{
comp_id = hal_xinit(TYPE_RT, 0, 0, instantiate_lutn, NULL, compname);
if (comp_id < 0)
return -1;
hal_ready(comp_id);
return 0;
}
int hal_xinitf(const int type,
const int userarg1,
const int userarg2,
const hal_constructor_t ctor,
const hal_destructor_t dtor,
const char *fmt, ...)
{
va_list ap;
char hal_name[HAL_NAME_LEN + 1];
CHECK_NULL(fmt);
va_start(ap, fmt);
int sz = rtapi_vsnprintf(hal_name, sizeof(hal_name), fmt, ap);
if(sz == -1 || sz > HAL_NAME_LEN) {
HALERR("invalid length %d for name starting with '%s'",
sz, hal_name);
return -EINVAL;
}
return hal_xinit(type, userarg1, userarg2, ctor, dtor, hal_name);
}
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 rtapi_app_main(void)
{
int retval;
dlist_init_entry(&head);
if ((comp_id = hal_xinit(TYPE_RT, 0, 0,
instantiate_encoder_pair,
delete_encoder_pair,
compname)) < 0)
return comp_id;
hal_export_xfunct_args_t u = {
.type = FS_XTHREADFUNC,
.funct.x = update,
.arg = &head,
.uses_fp = 1,
.reentrant = 0,
.owner_id = comp_id
};
if ((retval = hal_export_xfunctf(&u, "%s.update",
prefix)) < 0)
return retval;
hal_export_xfunct_args_t mp = {
.type = FS_XTHREADFUNC,
.funct.x = sample,
.arg = &head,
.uses_fp = 0,
.reentrant = 0,
.owner_id = comp_id
};
if ((retval = hal_export_xfunctf(&mp, "%s.sample",
prefix)) < 0)
return retval;
hal_ready(comp_id);
return 0;
}
void rtapi_app_exit(void)
{
hal_exit(comp_id);
}
static int instantiate_encoder_pair(const int argc, const char**argv)
{
encoder_pair_t *p;
int retval;
int msg;
const char* name;
if(argc >= 2)
name = argv[1];
else
HALFAIL_RC(EINVAL, "ERROR: insufficient args in argv");
/* This function exports a lot of stuff, which results in a lot of
logging if msg_level is at INFO or ALL. So we save the current value
of msg_level and restore it later. */
msg = rtapi_get_msg_level();
#ifndef VERBOSE_SETUP
rtapi_set_msg_level(RTAPI_MSG_WARN);
#endif
if ((retval = hal_inst_create(name, comp_id, sizeof(encoder_pair_t), (void **)&p)) < 0)
return retval;
p->inst_id = retval;
if ((retval = export_encoder_pair(name, p->inst_id, p)) != 0)
HALFAIL_RC(retval, "%s: ERROR: export(%s) failed", compname, name);
// append to instance list
dlist_init_entry(&p->list);
dlist_add_after(&p->list, &head);
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
static int instantiate_interpolate(const int argc, const char **argv)
{
const char *name = argv[1];
struct inst_data *ip;
int inst_id, i;
if ((inst_id = hal_inst_create(name, comp_id,
sizeof(struct inst_data) +
count * sizeof(struct joint),
(void **)&ip)) < 0)
return -1;
// instance-level values
ip->count = count;
// attach the command ringbuffer '<instancename>.traj' if it exists
// must be record mode
unsigned flags;
if (!hal_ring_attachf(&(ip->traj), &flags, "%s.traj", name)) {
if ((flags & RINGTYPE_MASK) != RINGTYPE_RECORD) {
HALERR("ring %s.traj not a record mode ring: mode=%d",name, flags & RINGTYPE_MASK);
return -EINVAL;
}
ip->traj.header->reader = inst_id; // we're the reader - advisory
} else {
HALERR("ring %s.traj does not exist", name);
return -EINVAL;
}
// per-instance objects
if (hal_pin_s32_newf(HAL_OUT, &(ip->serial), inst_id, "%s.serial", name) ||
hal_pin_u32_newf(HAL_IN, &(ip->degree), inst_id, "%s.degree", name) ||
hal_pin_bit_newf(HAL_IN, &(ip->jitter_correct), inst_id, "%s.jitter-correct", name) ||
hal_pin_float_newf(HAL_IN, &(ip->epsilon), inst_id, "%s.epsilon", name) ||
hal_pin_float_newf(HAL_OUT, &(ip->duration), inst_id, "%s.duration", name) ||
hal_pin_float_newf(HAL_OUT, &(ip->progress), inst_id, "%s.progress", name))
return -1;
*(ip->serial) = 0;
*(ip->degree) = 1;
*(ip->jitter_correct) = 0;
*(ip->epsilon) = 0;
*(ip->duration) = 0;
*(ip->progress) = 0;
ip->time_from_start = 0;
// per-joint objects
for (i = 0; i < ip->count; i++) {
struct joint *jp = &ip->joints[i];
if (hal_pin_float_newf(HAL_OUT, &(jp->end_pos), inst_id, "%s.%d.end-pos", name, i) ||
hal_pin_float_newf(HAL_OUT, &(jp->end_vel), inst_id, "%s.%d.end-vel", name, i) ||
hal_pin_float_newf(HAL_OUT, &(jp->end_acc), inst_id, "%s.%d.end-acc", name, i) ||
hal_pin_float_newf(HAL_OUT, &(jp->curr_pos), inst_id, "%s.%d.curr-pos", name, i) ||
hal_pin_float_newf(HAL_OUT, &(jp->curr_vel), inst_id, "%s.%d.curr-vel", name, i) ||
hal_pin_float_newf(HAL_OUT, &(jp->curr_acc), inst_id, "%s.%d.curr-acc", name, i) ||
hal_pin_bit_newf(HAL_OUT, &(jp->traj_busy), inst_id, "%s.%d.traj-busy", name, i))
return -1;
// set all pin values to zero
*(jp->end_pos) = 0;
*(jp->end_vel) = 0;
*(jp->end_acc) = 0;
*(jp->curr_pos) = 0;
*(jp->curr_vel) = 0;
*(jp->curr_acc) = 0;
*(jp->traj_busy) = false;
}
hal_export_xfunct_args_t xfunct_args = {
.type = FS_XTHREADFUNC,
.funct.x = update,
.arg = ip,
.uses_fp = 0,
.reentrant = 0,
.owner_id = inst_id
};
return hal_export_xfunctf(&xfunct_args, "%s.update", name);
}
static int delete_interpolate(const char *name, void *inst, const int inst_size)
{
struct inst_data *ip = (struct inst_data *) inst;
int retval;
if (ringbuffer_attached(&ip->traj)) {
if ((retval = hal_ring_detach(&ip->traj)) < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: hal_ring_detach(%s.traj) failed: %d\n",
compname, name, retval);
return retval;
}
ip->traj.header->reader = 0;
}
return 0;
}
int rtapi_app_main(void)
{
comp_id = hal_xinit(TYPE_RT, 0, 0,
(hal_constructor_t)instantiate_interpolate,
delete_interpolate,
compname);
if (comp_id < 0)
return comp_id;
hal_ready(comp_id);
return 0;
}
