// part of public API
void *halg_malloc(const int use_hal_mutex, size_t size)
{
PCHECK_NULL(hal_data);
{
WITH_HAL_MUTEX_IF(use_hal_mutex);
void *retval = shmalloc_rt(size);
if (retval == NULL)
HALERR("out of rt memory - allocating %zu bytes", size);
hal_data->hal_malloced += size; // beancounting
return retval;
}
}
// use only for owner_ids of pins, params or functs
// may return NULL if buggy using code
hal_comp_t *halpr_find_owning_comp(const int owner_id)
{
hal_comp_t *comp = halpr_find_comp_by_id(owner_id);
if (comp != NULL)
return comp; // legacy case: the owner_id refers to a comp
// nope, so it better be an instance
hal_inst_t *inst = halpr_find_inst_by_id(owner_id);
if (inst == NULL) {
HALERR("BUG: owner_id %d refers neither to a hal_comp_t nor an hal_inst_t",
owner_id);
return NULL;
}
// found the instance. Retrieve its owning comp:
comp = halpr_find_comp_by_id(inst->comp_id);
if (comp == NULL) {
// really bad. an instance which has no owning comp?
HALERR("BUG: instance %s/%d's comp_id %d refers to a non-existant comp",
inst->name, inst->inst_id, inst->comp_id);
}
return comp;
}
// instantiation handlers
static int create_instance(const hal_funct_args_t *fa)
{
const int argc = fa_argc(fa);
const char **argv = fa_argv(fa);
#if 0
HALDBG("'%s' called, arg=%p argc=%d",
fa_funct_name(fa), fa_arg(fa), argc);
int i;
for (i = 0; i < argc; i++)
HALDBG(" argv[%d] = \"%s\"", i,argv[i]);
#endif
if (argc < 2) {
HALERR("need component name and instance name");
return -EINVAL;
}
const char *cname = argv[0];
const char *iname = argv[1];
hal_comp_t *comp = halpr_find_comp_by_name(cname);
if (!comp) {
HALERR("no such component '%s'", cname);
return -EINVAL;
}
if (!comp->ctor) {
HALERR("component '%s' not instantiable", cname);
return -EINVAL;
}
hal_inst_t *inst = halpr_find_inst_by_name(iname);
if (inst) {
HALERR("instance '%s' already exists", iname);
return -EBUSY;
}
return comp->ctor(iname, argc - 2, &argv[2]);
}
static int delete_instance(const hal_funct_args_t *fa)
{
const int argc = fa_argc(fa);
const char **argv = fa_argv(fa);
HALDBG("'%s' called, arg=%p argc=%d",
fa_funct_name(fa), fa_arg(fa), argc);
int i;
for (i = 0; i < argc; i++)
HALDBG(" argv[%d] = \"%s\"", i, argv[i]);
if (argc < 1) {
HALERR("no instance name given");
return -EINVAL;
}
return hal_inst_delete(argv[0]);
}
int hal_heap_addmem(size_t click)
{
size_t actual = RTAPI_ALIGN(click, RTAPI_CACHELINE);
HALDBG("extending arena by %zu bytes", actual);
if (hal_freemem() < HAL_HEAP_MINFREE) {
HALERR("can't extend arena - below minfree: %zu", hal_freemem());
return 0;
}
if (rtapi_heap_addmem(&hal_data->heap,
SHMPTR(hal_data->shmem_bot),
actual)) {
HALFAIL_RC(ENOMEM, "rtapi_heap_addmem(%zu) failed", actual);
}
hal_data->shmem_bot += actual;
return 0;
}
int hal_inst_delete(const char *name)
{
CHECK_HALDATA();
CHECK_STR(name);
{
hal_inst_t *inst __attribute__((cleanup(halpr_autorelease_mutex)));
rtapi_mutex_get(&(hal_data->mutex));
// inst must exist
if ((inst = halpr_find_inst_by_name(name)) == NULL) {
HALERR("instance '%s' does not exist", name);
return -ENOENT;
}
// this does most of the heavy lifting
free_inst_struct(inst);
}
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);
}
// part of public API
void *hal_malloc(long int size)
{
void *retval;
if (hal_data == 0) {
HALERR("hal_malloc called before init");
return 0;
}
/* get the mutex */
rtapi_mutex_get(&(hal_data->mutex));
/* allocate memory */
retval = shmalloc_up(size);
/* release the mutex */
rtapi_mutex_give(&(hal_data->mutex));
/* check return value */
if (retval == 0) {
HALDBG("hal_malloc() can't allocate %ld bytes", size);
}
return retval;
}
// init HAL objects
static int export_halobjs(struct inst_data *ip, int owner_id, const char *name)
{
if (hal_pin_float_newf(HAL_OUT, &ip->out, owner_id, "%s.out", name))
return -1;
if (hal_pin_float_newf(HAL_IN, &ip->in, owner_id, "%s.in", name))
return -1;
if (hal_param_s32_newf(HAL_RO, &ip->iter,owner_id, "%s.iter", name))
return -1;
// unittest observer pins, per instance
if (hal_pin_s32_newf(HAL_OUT, &ip->repeat_value, owner_id, "%s.repeat_value", name))
return -1;
if (hal_pin_s32_newf(HAL_OUT, &ip->prefix_len, owner_id, "%s.prefix_len", name))
return -1;
// exporting '<instname>.funct' as an extended thread function
// see lutn.c for a discussion of advantages
hal_export_xfunct_args_t xfunct_args = {
.type = FS_XTHREADFUNC,
.funct.x = funct,
.arg = ip, // the instance's HAL memory blob
.uses_fp = 1,
.reentrant = 0,
.owner_id = owner_id
};
return hal_export_xfunctf(&xfunct_args, "%s.funct", name);
}
// constructor - init all HAL pins, params, funct etc here
static int instantiate(const int argc, const char**argv)
{
// argv[0]: component name
const char *name = argv[1]; // instance name
struct inst_data *ip;
// allocate a named instance, and some HAL memory for the instance data
int inst_id = hal_inst_create(name, comp_id,
sizeof(struct inst_data),
(void **)&ip);
if (inst_id < 0)
return -1;
// here ip is guaranteed to point to a blob of HAL memory
// of size sizeof(struct inst_data).
HALINFO("inst=%s argc=%d\n", name, argc);
HALINFO("instance parms: repeat=%d prefix='%s'", repeat, prefix);
HALINFO("module parms: answer=%d text='%s'", answer, text);
// example - parse newinst arguments getopt-style:
// straight from: http://www.informit.com/articles/article.aspx?p=175771&seqNum=3
int do_all, do_help, do_verbose; /* flag variables */
char *myfile;
struct option longopts[] = {
{ "all", no_argument, & do_all, 1 },
{ "file", required_argument, NULL, 'f' },
{ "help", no_argument, & do_help, 1 },
{ "verbose", no_argument, & do_verbose, 1 },
{ 0, 0, 0, 0 }
};
int c;
while ((c = getopt_long(argc, argv, ":f:", longopts, NULL)) != -1) {
switch (c) {
case 'f':
myfile = optarg;
break;
case 0: // getopt_long() set a variable, just keep going
break;
case ':': // missing option argument
HALERR("%s: option `-%c' requires an argument\n",
argv[1], optopt);
break;
case '?':
default: // invalid option
HALERR("%s: option `-%c' is invalid, ignored",
argv[1], optopt);
break;
}
}
HALINFO("do_all=%d do_help=%d do_verbose=%d myfile=%s",
do_all, do_help, do_verbose, myfile);
// these pins/params/functs will be owned by the instance,
// and can be separately exited via 'halcmd delinst <instancename>'
int retval = export_halobjs(ip, inst_id, name);
// unittest: echo instance parameters into observer pins
if (!retval) {
*(ip->repeat_value) = repeat;
*(ip->prefix_len) = strlen(prefix);
}
return retval;
}
// custom destructor
// pins, params, and functs are automatically deallocated by hal_exit(comp_id)
// regardless if a destructor is used or not (see below), so usually it is not
// needed
//
// however, some objects like vtables, rings, threads, signals are not owned by
// a component, hence cannot be automatically exited by hal_exit() even if desired
//
// interaction with such objects may require a custom destructor like below for
// cleanup actions
// NB: if a customer destructor is used, it is called
// - after the instance's functs have been removed from their respective threads
// (so a thread funct call cannot interact with the destructor any more)
// - any pins and params of this instance are still intact when the destructor is
// called, and they are automatically destroyed by the HAL library once the
// destructor returns
static int delete(const char *name, void *inst, const int inst_size)
{
HALERR("inst=%s size=%d %p\n", name, inst_size, inst);
HALERR("instance parms: repeat=%d prefix='%s'", repeat, prefix);
HALERR("module parms: answer=%d text='%s'", answer, text);
return 0;
}
int rtapi_app_main(void)
{
HALERR("instance parms: repeat=%d prefix='%s'", repeat, prefix);
HALERR("module parms: answer=%d text='%s'", answer, text);
// to use default destructor, use NULL instead of delete
comp_id = hal_xinit(TYPE_RT, 0, 0, instantiate, delete, compname);
if (comp_id < 0)
return comp_id;
#if 0
// this is how an 'instance' would have been done in the legacy way
struct inst_data *ip = hal_malloc(sizeof(struct inst_data));
// these pins/params/functs will be owned by the component
// NB: this 'instance' cannot be exited, and no new one created on the fly
if (export_halobjs(ip, comp_id, "foo"))
return -1;
#endif
// unittest only, see nosetests/unittest_instbindings.py and
// nosetests/unittest_icomp.py
// purpose: echo module params into observer pins
// (cant set pins to strings, so just echo the string length)
if ((cd = export_observer_pins(comp_id, compname)) == NULL)
return -1;
*(cd->answer_value) = answer;
*(cd->text_len) = strlen(text);
hal_ready(comp_id);
return 0;
}
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 hal_ring_delete(const char *name)
{
int retval;
CHECK_HALDATA();
CHECK_STRLEN(name, HAL_NAME_LEN);
CHECK_LOCK(HAL_LOCK_LOAD);
{
hal_ring_t *hrptr __attribute__((cleanup(halpr_autorelease_mutex)));
rtapi_mutex_get(&(hal_data->mutex));
// ring must exist
if ((hrptr = halpr_find_ring_by_name(name)) == NULL) {
HALERR("ring '%s' not found", name);
return -ENOENT;
}
ringheader_t *rhptr;
int shmid = -1;
if (hrptr->flags & ALLOC_HALMEM) {
// ring exists as HAL memory.
rhptr = SHMPTR(hrptr->ring_offset);
} else {
// ring exists as shm segment. Retrieve shared memory address.
if ((shmid = rtapi_shmem_new_inst(hrptr->ring_shmkey,
rtapi_instance, lib_module_id,
0 )) < 0) {
if (shmid != -EEXIST) {
HALERR("ring '%s': rtapi_shmem_new_inst() failed %d",
name, shmid);
return shmid;
}
}
if ((retval = rtapi_shmem_getptr(shmid, (void **)&rhptr, 0))) {
HALERR("ring '%s': rtapi_shmem_getptr %d failed %d",
name, shmid, retval);
return -ENOMEM;
}
}
// assure attach/detach balance is zero:
if (rhptr->refcount) {
HALERR("ring '%s' still attached - refcount=%d",
name, rhptr->refcount);
return -EBUSY;
}
HALDBG("deleting ring '%s'", name);
if (hrptr->flags & ALLOC_HALMEM) {
; // if there were a HAL memory free function, call it here
} else {
if ((retval = rtapi_shmem_delete(shmid, lib_module_id)) < 0) {
HALERR("ring '%s': rtapi_shmem_delete(%d,%d) failed: %d",
name, shmid, lib_module_id, retval);
return retval;
}
}
// search for the ring (again..)
int *prev = &(hal_data->ring_list_ptr);
int next = *prev;
while (next != 0) {
hrptr = SHMPTR(next);
if (strcmp(hrptr->name, name) == 0) {
// this is the right ring
// unlink from list
*prev = hrptr->next_ptr;
// and delete it, linking it on the free list
free_ring_struct(hrptr);
return 0;
}
// no match, try the next one
prev = &(hrptr->next_ptr);
next = *prev;
}
HALERR("BUG: deleting ring '%s'; not found in ring_list?",
name);
return -ENOENT;
}
}
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;
}
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_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_ring_attach(const char *name, ringbuffer_t *rbptr,unsigned *flags)
{
hal_ring_t *rbdesc;
ringheader_t *rhptr;
CHECK_HALDATA();
CHECK_STRLEN(name, HAL_NAME_LEN);
// no mutex(es) held up to here
{
int retval __attribute__((cleanup(halpr_autorelease_mutex)));
rtapi_mutex_get(&(hal_data->mutex));
if ((rbdesc = halpr_find_ring_by_name(name)) == NULL) {
HALERR("no such ring '%s'", name);
return -ENOENT;
}
// calling hal_ring_attach(name, NULL, NULL) is a way to determine
// if a given ring exists.
// hal_ring_attach(name, NULL, &flags) is a way to inspect the flags
// of an existing ring without actually attaching it.
if (rbptr == NULL) {
if (flags)
*flags = rbdesc->flags;
return 0;
}
if (rbdesc->flags & ALLOC_HALMEM) {
rhptr = SHMPTR(rbdesc->ring_offset);
} else {
int shmid;
// map in the shm segment - size 0 means 'must exist'
if ((retval = rtapi_shmem_new_inst(rbdesc->ring_shmkey,
rtapi_instance, lib_module_id,
0 )) < 0) {
if (retval != -EEXIST) {
HALERR("ring '%s': rtapi_shmem_new_inst() failed %d",
name, retval);
return retval;
}
// tried to map shm again. May happen in halcmd_commands:print_ring_info().
// harmless.
}
shmid = retval;
// make it accessible
if ((retval = rtapi_shmem_getptr(shmid, (void **)&rhptr, 0))) {
HALERR("ring '%s': rtapi_shmem_getptr %d failed %d",
name, shmid, retval);
return -ENOMEM;
}
}
// record usage in ringheader
rhptr->refcount++;
// fill in ringbuffer_t
ringbuffer_init(rhptr, rbptr);
if (flags)
*flags = rbdesc->flags;
// hal mutex unlock happens automatically on scope exit
}
return 0;
}
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
}
}
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;
}
int hal_compile_comp(const char *name, hal_compiled_comp_t **ccomp)
{
hal_compiled_comp_t *tc;
int pincount = 0;
CHECK_HALDATA();
CHECK_STRLEN(name, HAL_NAME_LEN);
{
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) {
HALERR("no such component '%s'", 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 = halpr_find_owning_comp(pin->owner_id);
if (owner->comp_id == comp->comp_id) {
if (!(pin->flags & PIN_DO_NOT_TRACK))
n++;
pincount++;
}
next = pin->next_ptr;
}
if (n == 0) {
HALERR("component %s has no pins to watch for changes",
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 = halpr_find_owning_comp(pin->owner_id);
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;
}
HALDBG("ccomp '%s': %d pins, %d tracked", name, pincount, tc->n_pins);
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;
CHECK_HALDATA();
CHECK_STRLEN(comp_name, HAL_NAME_LEN);
{
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) {
HALERR("no such component '%s'", 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 = halpr_find_owning_comp(pin->owner_id);
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;
}
HALDBG("hal_retrieve_pinstate: visited %d pins", nvisited);
/* if we get here, we ran through all the pins, so return count */
return nvisited;
}
}
/** init_hal_data() initializes the entire HAL data structure,
by the RT hal_lib component
*/
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 {
HALERR("version code mismatch");
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;
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->inst_list_ptr = 0;
hal_data->group_free_ptr = 0;
hal_data->member_free_ptr = 0;
hal_data->ring_free_ptr = 0;
hal_data->inst_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_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;
}