/***********************************************************************
* LOCAL FUNCTION DEFINITIONS *
* these are functions used for exporting various HAL pins/prams *
************************************************************************/
static int export_counter(int num, vti_struct * addr)
{
int retval, msg;
char buf[HAL_NAME_LEN + 2];
/* 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. If you actually need to log this
function's actions, change the second line below */
msg = rtapi_get_msg_level();
rtapi_set_msg_level(RTAPI_MSG_WARN);
/* export pin for counts captured by update() */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.counts", num);
retval = hal_pin_s32_new(buf, HAL_OUT, &addr->count[num], comp_id);
if (retval != 0) {
return retval;
}
/* export pin for scaled position captured by update() */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.position", num);
retval = hal_pin_float_new(buf, HAL_OUT, &addr->pos[num], comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for scaling */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.position-scale", num);
retval = hal_param_float_new(buf, HAL_RW, &addr->pos_scale[num], comp_id);
if (retval != 0) {
return retval;
}
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
static int export_dac(int num, vti_struct * addr)
{
int retval, msg;
char buf[HAL_NAME_LEN + 2];
/* 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. If you actually need to log this
function's actions, change the second line below */
msg = rtapi_get_msg_level();
rtapi_set_msg_level(RTAPI_MSG_WARN);
/* export pin for voltage received by the board() */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.dac-value", num);
retval = hal_pin_float_new(buf, HAL_IN, &addr->dac_value[num], comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for offset */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.dac-offset", num);
retval =
hal_param_float_new(buf, HAL_RW, &addr->dac_offset[num], comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for gain */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.dac-gain", num);
retval = hal_param_float_new(buf, HAL_RW, &addr->dac_gain[num], comp_id);
if (retval != 0) {
return retval;
}
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
static int export_delay(int num, bit_delay_t * addr)
{
int retval;
char buf[HAL_NAME_LEN + 2];
/* export pin for input bit */
rtapi_snprintf(buf, HAL_NAME_LEN, "delay.%d.in", num);
retval = hal_pin_bit_new(buf, HAL_IN, &(addr->in), comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"TIMEDELAY: ERROR: '%s' pin export failed\n", buf);
return retval;
}
/* export pin for output bit */
rtapi_snprintf(buf, HAL_NAME_LEN, "delay.%d.out", num);
retval = hal_pin_bit_new(buf, HAL_OUT, &(addr->out), comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"TIMEDELAY: ERROR: '%s' pin export failed\n", buf);
return retval;
}
/* export off delay parameter */
rtapi_snprintf(buf, HAL_NAME_LEN, "delay.%d.off_delay", num);
retval = hal_param_float_new(buf, HAL_RW, &(addr->off_delay), comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"TIMEDELAY: ERROR: '%s' parameter export failed\n", buf);
return retval;
}
/* export on delay parameter */
rtapi_snprintf(buf, HAL_NAME_LEN, "delay.%d.on_delay", num);
retval = hal_param_float_new(buf, HAL_RW, &(addr->on_delay), comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"TIMEDELAY: ERROR: '%s' parameter export failed\n", buf);
return retval;
}
/* export elapsed time parameter */
rtapi_snprintf(buf, HAL_NAME_LEN, "delay.%d.elapsed", num);
retval = hal_param_float_new(buf, HAL_RO, &(addr->elapsed), comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"TIMEDELAY: ERROR: '%s' parameter export failed\n", buf);
return retval;
}
/* set initial parameter and pin values */
*(addr->in) = 0;
*(addr->out) = 0;
addr->timer = 0.0;
addr->off_delay = DEFAULT_DELAY;
addr->on_delay = DEFAULT_DELAY;
addr->elapsed = 0.0;
return 0;
}
int rtapi_app_main(void)
{
char name[HAL_NAME_LEN + 2];
int n, retval;
/* only one port at the moment */
num_ports = 1;
n = 0; /* port number */
/* STEP 1: initialise the driver */
comp_id = hal_init("hal_skeleton");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SKELETON: ERROR: hal_init() failed\n");
return -1;
}
/* STEP 2: allocate shared memory for skeleton data */
port_data_array = hal_malloc(num_ports * sizeof(skeleton_t));
if (port_data_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SKELETON: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/* STEP 3: export the pin(s) */
rtapi_snprintf(name, HAL_NAME_LEN, "skeleton.%d.pin-%02d-out", n, 1);
retval =
hal_pin_u32_new(name, HAL_IN, &(port_data_array->data_out), comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SKELETON: ERROR: port %d var export failed with err=%i\n", n,
retval);
hal_exit(comp_id);
return -1;
}
/* STEP 4: export write function */
rtapi_snprintf(name, HAL_NAME_LEN, "skeleton.%d.write", n);
retval =
hal_export_funct(name, write_port, &(port_data_array[n]), 0, 0,
comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SKELETON: ERROR: port %d write funct export failed\n", n);
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"SKELETON: installed driver for %d ports\n", num_ports);
hal_ready(comp_id);
return 0;
}
int assure_module_loaded(const char *module)
{
FILE *fd;
char line[100];
int len = strlen(module);
int retval;
fd = fopen("/proc/modules", "r");
if (fd == NULL) {
HPG_ERR("ERROR: cannot read /proc/modules\n");
return -1;
}
while (fgets(line, sizeof(line), fd)) {
if (!strncmp(line, module, len)) {
HPG_DBG("module '%s' already loaded\n", module);
fclose(fd);
return 0;
}
}
fclose(fd);
HPG_DBG("loading module '%s'\n", module);
rtapi_snprintf(line, sizeof(line), "/sbin/modprobe %s", module);
if ((retval = system(line))) {
HPG_ERR("ERROR: executing '%s' %d - %s\n", line, errno, strerror(errno));
return -1;
}
return 0;
}
int rtapi_app_main(void)
{
int n, retval, i;
if(num_chan && names[0]) {
rtapi_print_msg(RTAPI_MSG_ERR,"num_chan= and names= are mutually exclusive\n");
return -EINVAL;
}
if(!num_chan && !names[0]) num_chan = default_num_chan;
if(num_chan) {
howmany = num_chan;
} else {
for(i=0; names[i]; i++) {howmany = i+1;}
}
/* test for number of channels */
if ((howmany <= 0) || (howmany > MAX_CHAN)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SIGGEN: ERROR: invalid number of channels: %d\n", howmany);
return -1;
}
/* have good config info, connect to the HAL */
comp_id = hal_init("siggen");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "SIGGEN: ERROR: hal_init() failed\n");
return -1;
}
/* allocate shared memory for siggen data */
siggen_array = hal_malloc(howmany * sizeof(hal_siggen_t));
if (siggen_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SIGGEN: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/* export variables and functions for each siggen */
i = 0; // for names= items
for (n = 0; n < howmany; n++) {
/* export everything for this loop */
if(num_chan) {
char buf[HAL_NAME_LEN + 1];
rtapi_snprintf(buf, sizeof(buf), "siggen.%d", n);
retval = export_siggen(n, &(siggen_array[n]),buf);
} else {
retval = export_siggen(n, &(siggen_array[n]),names[i++]);
}
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SIGGEN: ERROR: siggen %d var export failed\n", n);
hal_exit(comp_id);
return -1;
}
}
rtapi_print_msg(RTAPI_MSG_INFO,
"SIGGEN: installed %d signal generators\n", howmany);
hal_ready(comp_id);
return 0;
}
static int module_delete(int module_id) {
module_data *module;
char name[RTAPI_NAME_LEN + 1];
int n;
rtapi_print_msg(RTAPI_MSG_DBG, "RTAPI:%d module %d exiting\n",
rtapi_instance, module_id);
/* validate module ID */
if ((module_id < 1) || (module_id > RTAPI_MAX_MODULES)) {
return -EINVAL;
}
/* point to the module's data */
module = &(module_array[module_id]);
/* check module status */
if (module->state != REALTIME) {
/* not an active realtime module */
return -EINVAL;
}
/* clean up any mess left behind by the module */
for (n = 1; n <= RTAPI_MAX_TASKS; n++) {
if ((task_array[n].state != EMPTY)
&& (task_array[n].owner == module_id)) {
rtapi_print_msg(RTAPI_MSG_WARN,
"RTAPI:%d WARNING: module '%s' failed to delete task %02d\n",
rtapi_instance, module->name, n);
task_array[n].state = DELETE_LOCKED;
_rtapi_task_delete(n);
}
}
for (n = 1; n <= RTAPI_MAX_SHMEMS; n++) {
if (rtapi_test_bit(module_id, shmem_array[n].bitmap)) {
rtapi_print_msg(RTAPI_MSG_WARN,
"RTAPI:%d WARNING: module '%s' failed to delete shmem %02d\n",
rtapi_instance, module->name, n);
// mark block as ready for delete, lock already held
shmem_array[n].magic = SHMEM_MAGIC_DEL_LOCKED;
_rtapi_shmem_delete(n, module_id);
}
}
rtapi_snprintf(name, RTAPI_NAME_LEN, "%s", module->name);
/* update module data */
module->state = NO_MODULE;
module->name[0] = '\0';
rtapi_data->rt_module_count--;
if (rtapi_data->rt_module_count == 0) {
if (rtapi_data->timer_running != 0) {
#ifdef HAVE_RTAPI_MODULE_TIMER_STOP
_rtapi_module_timer_stop();
#endif
rtapi_data->timer_period = 0;
timer_counts = 0;
max_delay = DEFAULT_MAX_DELAY;
rtapi_data->timer_running = 0;
}
}
rtapi_print_msg(RTAPI_MSG_DBG, "RTAPI:%d module %d exited, name: '%s'\n",
rtapi_instance, module_id, name);
return 0;
}
static int export_output_pin(int pinnum, io_pin * pin)
{
char buf[HAL_NAME_LEN + 2];
int retval;
/* export read only HAL pin for output data */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.out-%02d", pinnum);
retval = hal_pin_bit_new(buf, HAL_IN, &(pin->data), comp_id);
if (retval != 0)
return retval;
/* export parameter for polarity */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.out-%02d-invert", pinnum);
retval = hal_param_bit_new(buf, HAL_RW, &(pin->io.invert), comp_id);
/* initialize HAL pin and param */
*(pin->data) = 0;
pin->io.invert = 0;
return retval;
}
static int export_input_pin(int pinnum, io_pin * pin)
{
char buf[HAL_NAME_LEN + 2];
int retval;
/* export read only HAL pin for input data */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.in-%02d", pinnum);
retval = hal_pin_bit_new(buf, HAL_OUT, &(pin->data), comp_id);
if (retval != 0)
return retval;
/* export additional pin for inverted input data */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.in-%02d-not", pinnum);
retval = hal_pin_bit_new(buf, HAL_OUT, &(pin->io.not), comp_id);
/* initialize HAL pins */
*(pin->data) = 0;
*(pin->io.not) = 1;
return retval;
}
int export_pwmgen(hal_pru_generic_t *hpg, int i)
{
char name[HAL_NAME_LEN + 1];
int r, j;
// HAL values common to all outputs in this instance
rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.pwm_period", hpg->config.halname, i);
r = hal_param_u32_new(name, HAL_RW, &(hpg->pwmgen.instance[i].hal.param.pwm_period), hpg->config.comp_id);
if (r != 0) { return r; }
hpg->pwmgen.instance[i].hal.param.pwm_period = 10000000; // Default to 10 mS period, or 100 Hz
for (j=0; j < hpg->pwmgen.instance[i].num_outputs; j++) {
// Export HAL Pins
rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.out.%02d.enable", hpg->config.halname, i, j);
r = hal_pin_bit_new(name, HAL_IN, &(hpg->pwmgen.instance[i].out[j].hal.pin.enable), hpg->config.comp_id);
if (r != 0) { return r; }
rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.out.%02d.value", hpg->config.halname, i, j);
r = hal_pin_float_new(name, HAL_IN, &(hpg->pwmgen.instance[i].out[j].hal.pin.value), hpg->config.comp_id);
if (r != 0) { return r; }
// Export HAL Parameters
rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.out.%02d.scale", hpg->config.halname, i, j);
r = hal_param_float_new(name, HAL_RW, &(hpg->pwmgen.instance[i].out[j].hal.param.scale), hpg->config.comp_id);
if (r != 0) { return r; }
rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.out.%02d.pin", hpg->config.halname, i, j);
r = hal_param_u32_new(name, HAL_RW, &(hpg->pwmgen.instance[i].out[j].hal.param.pin), hpg->config.comp_id);
if (r != 0) { return r; }
// Initialize HAL Pins
*(hpg->pwmgen.instance[i].out[j].hal.pin.enable) = 0;
*(hpg->pwmgen.instance[i].out[j].hal.pin.value) = 0.0;
// Initialize HAL Parameters
hpg->pwmgen.instance[i].out[j].hal.param.pin = PRU_DEFAULT_PIN;
hpg->pwmgen.instance[i].out[j].hal.param.scale = 1.0;
}
return 0;
}
static void rtapi_format_pose(char *buf, unsigned long int size, pb_EmcPose *p)
{
unsigned sz = size;
char *b = buf; // FIXME guard against buffer overflow
int n;
pb_PmCartesian *c = &p->tran;
if (c->has_x) { n = rtapi_snprintf(b, sz, "x=%f ", c->x); b += n; }
if (c->has_y) { n = rtapi_snprintf(b, sz, "y=%f ", c->y); b += n; }
if (c->has_z) { n = rtapi_snprintf(b, sz, "z=%f ", c->z); b += n; }
if (p->has_a) { n = rtapi_snprintf(b, sz, "a=%f ", p->a); b += n; }
if (p->has_b) { n = rtapi_snprintf(b, sz, "b=%f ", p->b); b += n; }
if (p->has_c) { n = rtapi_snprintf(b, sz, "c=%f ", p->c); b += n; }
if (p->has_u) { n = rtapi_snprintf(b, sz, "u=%f ", p->u); b += n; }
if (p->has_v) { n = rtapi_snprintf(b, sz, "v=%f ", p->v); b += n; }
if (p->has_w) { n = rtapi_snprintf(b, sz, "w=%f ", p->w); b += n; }
}
// 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_init(const char *modname) {
int n, module_id = -ENOMEM;
module_data *module;
/* say hello */
rtapi_print_msg(RTAPI_MSG_DBG, "RTAPI: initing module %s\n", modname);
/* get the mutex */
rtapi_mutex_get(&(rtapi_data->mutex));
/* find empty spot in module array */
n = 1;
while ((n <= RTAPI_MAX_MODULES) && (module_array[n].state != NO_MODULE)) {
n++;
}
if (n > RTAPI_MAX_MODULES) {
/* no room */
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: reached module limit %d\n",
n);
return -EMFILE;
}
/* we have space for the module */
module_id = n + MODULE_OFFSET;
module = &(module_array[n]);
/* update module data */
module->state = REALTIME;
if (modname != NULL) {
/* use name supplied by caller, truncating if needed */
rtapi_snprintf(module->name, RTAPI_NAME_LEN, "%s", modname);
} else {
/* make up a name */
rtapi_snprintf(module->name, RTAPI_NAME_LEN, "ULMOD%03d", module_id);
}
rtapi_data->ul_module_count++;
rtapi_print_msg(RTAPI_MSG_DBG, "RTAPI: module '%s' loaded, ID: %d\n",
module->name, module_id);
rtapi_mutex_give(&(rtapi_data->mutex));
return module_id;
}
int export_pru(hal_pru_generic_t *hpg)
{
int r;
char name[HAL_NAME_LEN + 1];
// Export functions
rtapi_snprintf(name, sizeof(name), "%s.update", halname);
r = hal_export_funct(name, hpg_write, hpg, 1, 0, comp_id);
if (r != 0) {
HPG_ERR("ERROR: function export failed: %s\n", name);
hal_exit(comp_id);
return -1;
}
rtapi_snprintf(name, sizeof(name), "%s.capture-position", halname);
r = hal_export_funct(name, hpg_read, hpg, 1, 0, comp_id);
if (r != 0) {
HPG_ERR("ERROR: function export failed: %s\n", name);
hal_exit(comp_id);
return -1;
}
return 0;
}
void rtapi_print_loc(const int level,
const char *func,
const int line,
const char *topic,
const char *fmt, ...)
{
va_list args;
va_start(args, fmt);
const char *pfmt = "%d %s: %s ";
rtapi_snprintf(_rtapi_logmsg, RTAPIPRINTBUFFERLEN, pfmt, line,
func == NULL ? "(nil)" : func,
topic == NULL ? "" : topic);
int n = strlen(_rtapi_logmsg);
vsnprintf(_rtapi_logmsg + n, RTAPIPRINTBUFFERLEN - n, fmt, args);
rtapi_print_msg(level, _rtapi_logmsg);
va_end(args);
}
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;
}
// FIXME: the static automatic string makes this function non-reentrant
static const char* hm2_get_pin_secondary_name(hm2_pin_t *pin) {
static char unknown[100];
int sec_pin = pin->sec_pin & 0x7F; // turn off the "pin is an output" bit
//FIXME: some pins use the same sec_tag but different meanings depending on
//direction.
switch (pin->sec_tag) {
case HM2_GTAG_MUXED_ENCODER:
switch (sec_pin) {
case 1: return "Muxed A";
case 2: return "Muxed B";
case 3: return "Muxed Index";
case 4: return "Muxed IndexMask";
}
break;
case HM2_GTAG_MUXED_ENCODER_SEL:
switch (sec_pin) {
case 1: return "Mux Select 0";
case 2: return "Mux Select 1";
}
break;
case HM2_GTAG_ENCODER:
switch (sec_pin) {
case 1: return "A";
case 2: return "B";
case 3: return "Index";
case 4: return "IndexMask";
case 5: return "Probe";
}
break;
case HM2_GTAG_SSI:
switch (sec_pin) {
case 1: return "Clock";
case 2: return "ClockEnable";
case 3: return "Data";
}
break;
case HM2_GTAG_SPI: // Not supported yet
switch (sec_pin) {
case 1: return "Frame";
case 2: return "Out";
case 3: return "Clock";
case 4: return "In";
}
break;
case HM2_GTAG_RESOLVER:
switch (sec_pin) {
case 1: return "NC";
case 2: return "REFPDM+";
case 3: return "REFPDM-";
case 4: return "AMUX0";
case 5: return "AMUX1";
case 6: return "AMUX2";
case 7: return "SPICS";
case 8: return "SPICLK";
case 9: return "SPIDO0";
case 10: return "SPIDO1";
}
break;
case HM2_GTAG_PWMGEN:
// FIXME: these depend on the pwmgen mode
switch (sec_pin) {
case 1: return "Out0 (PWM or Up)";
case 2: return "Out1 (Dir or Down)";
case 3: return "Not-Enable";
}
break;
case HM2_GTAG_TPPWM:
switch (sec_pin) {
case 1: return "PWM A";
case 2: return "PWM B";
case 3: return "PWM C";
case 4: return "PWM /A";
case 5: return "PWM /B";
case 6: return "PWM /C";
case 7: return "Enable";
case 8: return "Fault";
}
break;
case HM2_GTAG_STEPGEN:
// FIXME: these depend on the stepgen mode
switch (sec_pin) {
case 1: return "Step";
case 2: return "Direction";
case 3: return "Table2Pin";
case 4: return "Table3Pin";
case 5: return "Table4Pin";
case 6: return "Table5Pin";
case 7: return "Table6Pin";
case 8: return "Table7Pin";
}
break;
case HM2_GTAG_SMARTSERIAL:
if (pin->sec_pin & 0x80){ // Output pin codes
switch (sec_pin) {
case 0x1: return "TxData0";
case 0x2: return "TxData1";
case 0x3: return "TxData2";
case 0x4: return "TxData3";
case 0x5: return "TxData4";
case 0x6: return "TxData5";
case 0x7: return "TxData6";
case 0x8: return "TxData7";
case 0x11: return "TxEn0 ";
case 0x12: return "TxEn1 ";
case 0x13: return "TxEn2 ";
case 0x14: return "TxEn3 ";
case 0x15: return "TxEn4 ";
case 0x16: return "TxEn5 ";
case 0x17: return "TxEn6 ";
case 0x18: return "TxEn7 ";
}
break;
}else{ // INput Pin Codes
switch (sec_pin) {
case 0x1: return "RxData0";
case 0x2: return "RxData1";
case 0x3: return "RxData2";
case 0x4: return "RxData3";
case 0x5: return "RxData4";
case 0x6: return "RxData5";
case 0x7: return "RxData6";
case 0x8: return "RxData7";
}
break;
}
case HM2_GTAG_BSPI:
switch (sec_pin) {
case 0x1: return "/Frame";
case 0x2: return "Serial Out";
case 0x3: return "Clock";
case 0x4: return "Serial In";
case 0x5: return "CS0";
case 0x6: return "CS1";
case 0x7: return "CS2";
case 0x8: return "CS3";
case 0x9: return "CS4";
case 0xA: return "CS5";
case 0xB: return "CS6";
case 0xC: return "CS7";
}
break;
case HM2_GTAG_DBSPI: // Not Supported Currently
switch (sec_pin) {
case 0x2: return "Serial Out";
case 0x3: return "Clock";
case 0x4: return "Serial In";
case 0x5: return "CS0";
case 0x6: return "CS1";
case 0x7: return "CS2";
case 0x8: return "CS3";
case 0x9: return "CS4";
case 0xA: return "CS5";
case 0xB: return "CS6";
case 0xC: return "CS7";
}
break;
case HM2_GTAG_UART_RX:
switch (sec_pin) {
case 0x1: return "RX Data";
}
break;
case HM2_GTAG_UART_TX:
switch (sec_pin) {
case 0x1: return "TX Data";
case 0x2: return "Drv Enable";
}
break;
case HM2_GTAG_DPLL: // Not Supported Currently
switch (sec_pin) {
case 0x1: return "SynchIn";
case 0x2: return "MSBOut";
case 0x3: return "Fout";
case 0x4: return "PostOut";
case 0x5: return "SynchTog";
}
break;
case HM2_GTAG_WAVEGEN: // Not Supported Currently
switch (sec_pin) {
case 0x1: return "PDMAOut";
case 0x2: return "PDMBOut";
case 0x3: return "Trigger0";
case 0x4: return "Trigger1";
case 0x5: return "Trigger2";
case 0x6: return "Trigger3";
}
break;
case HM2_GTAG_DAQFIFO: // Not Supported Currently
switch (sec_pin) {
case 0x41: return "Strobe";
default:
sprintf(unknown, "Data%02x",sec_pin - 1);
return unknown;
}
break;
case HM2_GTAG_BINOSC: // Not Supported Currently
sprintf(unknown, "Out%02x",sec_pin -1);
return unknown;
break;
case HM2_GTAG_BISS: // Not Supported Currently
switch (sec_pin) {
case 0x1: return "Clck";
case 0x2: return "Data";
}
break;
case HM2_GTAG_FABS: // Not Supported Currently
switch (sec_pin) {
case 0x1: return "Rq";
case 0x2: return "RqEn";
case 0x3: return "Data";
case 0x4: return "TestClk";
}
break;
case HM2_GTAG_HM2DPLL:
switch (sec_pin) {
case 0x1: return "Sync In Pin";
case 0x2: return "Ref Out Pin";
case 0x3: return "Timer 1 Pin";
case 0x4: return "Timer 2 Pin";
case 0x5: return "Timer 3 Pin";
case 0x6: return "Timer 4 Pin";
}
break;
case HM2_GTAG_TWIDDLER: // Not Supported Currently
if (sec_pin < 0x20){
sprintf(unknown, "In%02x", sec_pin - 1);
} else if (sec_pin > 0xC0){
sprintf(unknown, "IO%02x", sec_pin - 1);
} else {
sprintf(unknown, "Out%02x", sec_pin - 1);
}
return unknown;
break;
}
rtapi_snprintf(unknown, sizeof(unknown), "unknown-pin-%d", sec_pin & 0x7F);
return unknown;
}
int export_encoder(hal_pru_generic_t *hpg, int i)
{
char name[HAL_NAME_LEN + 1];
int r, j;
// HAL values common to all channels in this instance
// ...nothing to do here...
// HAL values for individual channels
for (j=0; j < hpg->encoder.instance[i].num_channels; j++) {
// Export HAL Pins
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.rawcounts", hpg->config.name, i, j);
r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.rawcounts), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.rawlatch", hpg->config.name, i, j);
r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.rawlatch), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.count", hpg->config.name, i, j);
r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.count), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.count-latched", hpg->config.name, i, j);
r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.count_latch), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.position", hpg->config.name, i, j);
r = hal_pin_float_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.position), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.position-latched", hpg->config.name, i, j);
r = hal_pin_float_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.position_latch), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.velocity", hpg->config.name, i, j);
r = hal_pin_float_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.velocity), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.reset", hpg->config.name, i, j);
r = hal_pin_bit_new(name, HAL_IN, &(hpg->encoder.instance[i].chan[j].hal.pin.reset), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-enable", hpg->config.name, i, j);
r = hal_pin_bit_new(name, HAL_IO, &(hpg->encoder.instance[i].chan[j].hal.pin.index_enable), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.latch-enable", hpg->config.name, i, j);
r = hal_pin_bit_new(name, HAL_IN, &(hpg->encoder.instance[i].chan[j].hal.pin.latch_enable), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.latch-polarity", hpg->config.name, i, j);
r = hal_pin_bit_new(name, HAL_IN, &(hpg->encoder.instance[i].chan[j].hal.pin.latch_polarity), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.quadrature-error", hpg->config.name, i, j);
r = hal_pin_bit_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.quadrature_error), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
// Export HAL Parameters
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.scale", hpg->config.name, i, j);
r = hal_param_float_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.scale), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.A-pin", hpg->config.name, i, j);
r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.A_pin), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.A-invert", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.A_invert), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.B-pin", hpg->config.name, i, j);
r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.B_pin), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.B-invert", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.B_invert), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-pin", hpg->config.name, i, j);
r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_pin), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-invert", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_invert), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-mask", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_mask), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-mask-invert", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_mask_invert), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.counter-mode", hpg->config.name, i, j);
r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.counter_mode), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.filter", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.filter), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.vel-timeout", hpg->config.name, i, j);
r = hal_param_float_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.vel_timeout), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
//
// init the hal objects that need it
//
hpg->encoder.instance[i].chan[j].hal.param.scale = 1.0;
hpg->encoder.instance[i].chan[j].hal.param.index_invert = 0;
hpg->encoder.instance[i].chan[j].hal.param.index_mask = 0;
hpg->encoder.instance[i].chan[j].hal.param.index_mask_invert = 0;
hpg->encoder.instance[i].chan[j].hal.param.counter_mode = 0;
// hpg->encoder.instance[i].chan[j].hal.param.filter = 1;
hpg->encoder.instance[i].chan[j].hal.param.vel_timeout = 0.5;
*hpg->encoder.instance[i].chan[j].hal.pin.rawcounts = 0;
*hpg->encoder.instance[i].chan[j].hal.pin.rawlatch = 0;
*hpg->encoder.instance[i].chan[j].hal.pin.count = 0;
*hpg->encoder.instance[i].chan[j].hal.pin.count_latch = 0;
*hpg->encoder.instance[i].chan[j].hal.pin.position = 0.0;
*hpg->encoder.instance[i].chan[j].hal.pin.position_latch = 0.0;
*hpg->encoder.instance[i].chan[j].hal.pin.velocity = 0.0;
*hpg->encoder.instance[i].chan[j].hal.pin.quadrature_error = 0;
hpg->encoder.instance[i].chan[j].zero_offset = 0;
hpg->encoder.instance[i].chan[j].prev_reg_count = 0;
hpg->encoder.instance[i].chan[j].state = HM2_ENCODER_STOPPED;
}
return 0;
}
static int export_freqgen(int num, freqgen_t * addr, int step_type)
{
int n, retval, msg;
char buf[HAL_NAME_LEN + 2];
/* 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. If you actually need to log this
function's actions, change the second line below */
msg = rtapi_get_msg_level();
rtapi_set_msg_level(RTAPI_MSG_WARN);
/* export param variable for raw counts */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.rawcounts", num);
retval = hal_param_s32_new(buf, HAL_RO, &(addr->rawcount), comp_id);
if (retval != 0) {
return retval;
}
/* export pin for counts captured by update() */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.counts", num);
retval = hal_pin_s32_new(buf, HAL_OUT, &(addr->count), comp_id);
if (retval != 0) {
return retval;
}
/* export pin for scaled position captured by update() */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.position-fb", num);
retval = hal_pin_float_new(buf, HAL_OUT, &(addr->pos), comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for position scaling */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.position-scale", num);
retval = hal_param_float_new(buf, HAL_RW, &(addr->pos_scale), comp_id);
if (retval != 0) {
return retval;
}
/* export pin for frequency command */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.velocity", num);
retval = hal_pin_float_new(buf, HAL_IN, &(addr->vel), comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for frequency scaling */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.velocity-scale", num);
retval = hal_param_float_new(buf, HAL_RW, &(addr->vel_scale), comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for max frequency */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.maxfreq", num);
retval = hal_param_float_new(buf, HAL_RW, &(addr->maxfreq), comp_id);
if (retval != 0) {
return retval;
}
/* export param for scaled velocity (frequency in Hz) */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.frequency", num);
retval = hal_param_float_new(buf, HAL_RO, &(addr->freq), comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for max accel/decel */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.maxaccel", num);
retval = hal_param_float_new(buf, HAL_RW, &(addr->maxaccel), comp_id);
if (retval != 0) {
return retval;
}
/* set default parameter values */
addr->pos_scale = 1.0;
addr->vel_scale = 1.0;
/* set maxfreq very high - let update_freq() fix it */
addr->maxfreq = 1e15;
addr->maxaccel = 0.0;
addr->wd.st0.step_type = step_type;
/* init the step generator core to zero output */
addr->accum = 0;
addr->addval = 0;
addr->newaddval = 0;
addr->deltalim = 0;
addr->rawcount = 0;
addr->printed_error = 0;
if (step_type == 0) {
/* setup for stepping type 0 - step/dir */
addr->wd.st0.need_step = 0;
addr->wd.st0.setup_timer = 0;
addr->wd.st0.hold_timer = 0;
addr->wd.st0.space_timer = 0;
addr->wd.st0.len_timer = 0;
/* export parameters for step/dir pulse timing */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.dirsetup", num);
retval =
hal_param_u32_new(buf, HAL_RW, &(addr->wd.st0.dir_setup), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.dirhold", num);
retval =
hal_param_u32_new(buf, HAL_RW, &(addr->wd.st0.dir_hold), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.steplen", num);
retval =
hal_param_u32_new(buf, HAL_RW, &(addr->wd.st0.step_len), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.stepspace", num);
retval =
hal_param_u32_new(buf, HAL_RW, &(addr->wd.st0.step_space),
comp_id);
if (retval != 0) {
return retval;
}
/* init the parameters */
addr->wd.st0.dir_setup = 1;
addr->wd.st0.dir_hold = 1;
addr->wd.st0.step_len = 1;
addr->wd.st0.step_space = 1;
/* export pins for step and direction */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.step", num);
retval =
hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[STEP_PIN]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[STEP_PIN]) = 0;
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.dir", num);
retval =
hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[DIR_PIN]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[DIR_PIN]) = 0;
} else if (step_type == 1) {
/* setup for stepping type 1 - pseudo-PWM */
/* export pins for up and down */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.up", num);
retval =
hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[UP_PIN]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[UP_PIN]) = 0;
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.down", num);
retval =
hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[DOWN_PIN]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[DOWN_PIN]) = 0;
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.count", num);
retval =
hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[COUNT_PIN]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[COUNT_PIN]) = 0;
} else {
/* setup for stepping types 2 and higher */
addr->wd.st2.state = 0;
addr->wd.st2.cycle_max = cycle_len_lut[step_type - 2] - 1;
addr->wd.st2.num_phases = num_phases_lut[step_type - 2];
addr->wd.st2.lut = &(master_lut[step_type - 2][0]);
/* export pins for output phases */
for (n = 0; n < addr->wd.st2.num_phases; n++) {
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.phase-%c", num,
n + 'A');
retval = hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[n]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[n]) = 0;
}
}
/* set initial pin values */
*(addr->count) = 0;
*(addr->pos) = 0.0;
*(addr->vel) = 0.0;
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
static int init_sampler(int num, fifo_t *tmp_fifo)
{
int size, retval, n, usefp;
void *shmem_ptr;
sampler_t *str;
pin_data_t *pptr;
fifo_t *fifo;
char buf[HAL_NAME_LEN + 2];
/* alloc shmem for base sampler data and user specified pins */
size = sizeof(sampler_t) + tmp_fifo->num_pins * sizeof(pin_data_t);
str = hal_malloc(size);
if (str == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: couldn't allocate HAL shared memory\n");
return -ENOMEM;
}
/* export "standard" pins and params */
retval = hal_pin_bit_newf(HAL_OUT, &(str->full), comp_id,
"sampler.%d.full", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: 'full' pin export failed\n");
return -EIO;
}
retval = hal_pin_bit_newf(HAL_IN, &(str->enable), comp_id,
"sampler.%d.enable", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: 'enable' pin export failed\n");
return -EIO;
}
retval = hal_pin_s32_newf(HAL_OUT, &(str->curr_depth), comp_id,
"sampler.%d.curr-depth", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLEr: ERROR: 'curr_depth' pin export failed\n");
return -EIO;
}
retval = hal_param_s32_newf(HAL_RW, &(str->overruns), comp_id,
"sampler.%d.overruns", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: 'overruns' parameter export failed\n");
return -EIO;
}
retval = hal_param_s32_newf(HAL_RW, &(str->sample_num), comp_id,
"sampler.%d.sample-num", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: 'sample-num' parameter export failed\n");
return -EIO;
}
/* init the standard pins and params */
*(str->full) = 0;
*(str->enable) = 1;
*(str->curr_depth) = 0;
str->overruns = 0;
str->sample_num = 0;
/* HAL pins are right after the sampler_t struct in HAL shmem */
pptr = (pin_data_t *)(str+1);
usefp = 0;
/* export user specified pins (the ones that sample data) */
for ( n = 0 ; n < tmp_fifo->num_pins ; n++ ) {
rtapi_snprintf(buf, HAL_NAME_LEN, "sampler.%d.pin.%d", num, n);
retval = hal_pin_new(buf, tmp_fifo->type[n], HAL_IN, (void **)pptr, comp_id );
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: pin '%s' export failed\n", buf);
return -EIO;
}
/* init the pin value */
switch ( tmp_fifo->type[n] ) {
case HAL_FLOAT:
*(pptr->hfloat) = 0.0;
usefp = 1;
break;
case HAL_BIT:
*(pptr->hbit) = 0;
break;
case HAL_U32:
*(pptr->hu32) = 0;
break;
case HAL_S32:
*(pptr->hs32) = 0;
break;
default:
break;
}
pptr++;
}
/* export update function */
rtapi_snprintf(buf, HAL_NAME_LEN, "sampler.%d", num);
retval = hal_export_funct(buf, sample, str, usefp, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: function export failed\n");
return retval;
}
/* alloc shmem for user/RT comms (fifo) */
size = sizeof(fifo_t) + (tmp_fifo->num_pins + 1) * tmp_fifo->depth * sizeof(shmem_data_t);
shmem_id[num] = rtapi_shmem_new(SAMPLER_SHMEM_KEY+num, comp_id, size);
if ( shmem_id[num] < 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLEr: ERROR: couldn't allocate user/RT shared memory\n");
return -ENOMEM;
}
retval = rtapi_shmem_getptr(shmem_id[num], &shmem_ptr);
if ( retval < 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SAMPLER: ERROR: couldn't map user/RT shared memory\n");
return -ENOMEM;
}
fifo = shmem_ptr;
str->fifo = fifo;
/* copy data from temp_fifo */
*fifo = *tmp_fifo;
/* init fields */
fifo->in = 0;
fifo->out = 0;
fifo->last_sample = 0;
fifo->last_sample--;
/* mark it inited for user program */
fifo->magic = FIFO_MAGIC_NUM;
return 0;
}
static int export_siggen(int num, hal_siggen_t * addr,char* prefix)
{
int retval;
char buf[HAL_NAME_LEN + 1];
/* export pins */
retval = hal_pin_float_newf(HAL_OUT, &(addr->square), comp_id,
"%s.square", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_OUT, &(addr->sawtooth), comp_id,
"%s.sawtooth", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_OUT, &(addr->triangle), comp_id,
"%s.triangle", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_OUT, &(addr->sine), comp_id,
"%s.sine", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_OUT, &(addr->cosine), comp_id,
"%s.cosine", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->clock), comp_id,
"%s.clock", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->enable), comp_id,
"%s.enable", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IN, &(addr->frequency), comp_id,
"%s.frequency", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IN, &(addr->amplitude), comp_id,
"%s.amplitude", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IN, &(addr->offset), comp_id,
"%s.offset", prefix);
if (retval != 0) {
return retval;
}
/* init all structure members */
*(addr->square) = 0.0;
*(addr->sawtooth) = 0.0;
*(addr->triangle) = 0.0;
*(addr->sine) = 0.0;
*(addr->cosine) = 0.0;
*(addr->clock) = 0;
*(addr->enable) = 0;
*(addr->frequency) = 1.0;
*(addr->amplitude) = 1.0;
*(addr->offset) = 0.0;
addr->index = 0.0;
/* export function for this loop */
rtapi_snprintf(buf, sizeof(buf), "%s.update", prefix);
retval =
hal_export_funct(buf, calc_siggen, &(siggen_array[num]), 1, 0,
comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"SIGGEN: ERROR: update funct export failed\n");
hal_exit(comp_id);
return -1;
}
return 0;
}
int rtapi_app_main(void)
{
int retval, i;
int in_cnt = 0, out_cnt = 0;
char buf[128];
char *pc = axes_conf;
/* Parsing axes configuration */
while(*pc != 0)
{
int idx = -1;
switch(*pc)
{
case 'X':
case 'x':
idx = 0;
break;
case 'Y':
case 'y':
idx = 1;
break;
case 'Z':
case 'z':
idx = 2;
break;
case 'A':
case 'a':
idx = 3;
break;
case 'B':
case 'b':
idx = 4;
break;
case 'C':
case 'c':
idx = 5;
break;
default: break;
}
if(idx >= 0)
axis_map[num_axis++] = idx;
pc++;
}
fprintf(stderr, "num_axis=%d, fifo_size=%d\n", num_axis, fifo_deep);
/* test for number of channels */
if ((num_axis <= 0) || (num_axis > MAX_AXIS)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"miniemcdrv: ERROR: invalid num_chan: %d\n", num_axis);
return -EINVAL;
}
/* have good config info, connect to the HAL */
comp_id = hal_init("miniemcdrv");
if (comp_id < 0)
{
rtapi_print_msg(RTAPI_MSG_ERR, "miniemcdrv: ERROR: hal_init() failed\n");
rtapi_app_exit();
return -EINVAL;
}
pgpio = hal_malloc(sizeof(gpio_t));
memset(pgpio, 0, sizeof(gpio_t));
pgpio->fd = open("/dev/miniemc", O_RDWR | O_SYNC);
if(pgpio->fd < 0)
{
rtapi_print_msg(RTAPI_MSG_ERR,
"miniemcdrv: ERROR: unble to create access to stepgen module\n");
rtapi_app_exit();
return -EIO;
}
pgpio->pfiq = mmap(0, sizeof(struct fiq_ipc_shared), PROT_READ | PROT_WRITE, MAP_FILE | MAP_SHARED, pgpio->fd, 0);
if(pgpio->pfiq == MAP_FAILED)
{
rtapi_print_msg(RTAPI_MSG_ERR,
"miniemcdrv: ERROR: unable to mmap stepgen ringbuffer\n");
rtapi_app_exit();
return -EIO;
}
/* Setup ringbuff size */
fiq_static.rb_size = fifo_deep;
memset(&cmd_pos_prev, 0, sizeof(cmd_pos_prev));
memset(&cmd_pos_accum, 0, sizeof(cmd_pos_accum));
//Configure PWM pins and create HAL inputs
for( i = 0; i < MAX_PWM; i++)
{
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pwm-in", i);
retval = hal_pin_float_new(buf, HAL_IN, &(pgpio->pwm_duty[i]), comp_id);
if (retval != 0)
{
rtapi_app_exit();
return retval;
}
if( pwm_pin_num[i] >= 0 )
{
emcConfigureDefault( pwm_pin_num[i] );
emcReservePin( pwm_pin_num[i] );
fiq_static.pwm_pin_addr[i] = GPIOS[pwm_pin_num[i]].port_index;
fiq_static.pwm_pin_mask[i] = 1L << GPIOS[pwm_pin_num[i]].offset;
pgpio->pfiq->pwm_duty_cycle[i] = 0;
} else
{
fiq_static.pwm_pin_mask[i] = 0;
fiq_static.pwm_pin_addr[i] = 0;
}
}
// Create axis step and dir pins
for(i = 0; i < num_axis; i++)
{
if(step_pins[i] >=0 && dir_pins[i] >= 0)
{
if( emcGetPinMode( step_pins[i]) == egpIn || emcGetPinMode( dir_pins[i]) == egpIn )
{
rtapi_print_msg(RTAPI_MSG_ERR, "WARN: can't create axis[%d] stepgen, invalid pin\n", i);
continue;
}
fiq_static.axis[i].configured = 0;
fiq_static.axis[i].step_pin_addr = GPIOS[step_pins[i]].port_index;
fiq_static.axis[i].step_pin_mask = 1L << GPIOS[step_pins[i]].offset;
emcConfigureDefault( step_pins[i] );
emcReservePin( step_pins[i] );
fiq_static.axis[i].dir_pin_addr = GPIOS[dir_pins[i]].port_index;
fiq_static.axis[i].dir_pin_mask = 1L << GPIOS[dir_pins[i]].offset;
emcConfigureDefault( dir_pins[i] );
emcReservePin( dir_pins[i] );
fiq_static.axis[i].dir_pin_pol = dir_polarity[i];
fiq_static.axis[i].configured = 1;
} else
{
rtapi_print_msg(RTAPI_MSG_ERR,
"miniemcdrv: WARNING: axis[%d] step and/or dir pin(s) not properly configured, skipping\n", i);
}
fiq_static.scan_pin_num = -1;
}
ioctl(pgpio->fd, AXIS_SET_IOCTL, &fiq_static );
/*
* Create IO pins
*/
for( i=0; i < ARR_SZ(GPIOS); i++ )
{
if( emcGetPinMode( i ) == egpRsv )
continue;
if( emcGetPinMode( i ) == egpIn )
{
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-in", in_cnt);
hal_pin_bit_new(buf, HAL_OUT, &(pgpio->io_pin[i]), comp_id);
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-in-inv", in_cnt);
hal_pin_bit_new(buf, HAL_IN, &(pgpio->io_invert[i]), comp_id);
in_cnt++;
} else
{
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-out", out_cnt);
hal_pin_bit_new(buf, HAL_IN, &(pgpio->io_pin[i]), comp_id);
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.pin-out-inv", out_cnt);
hal_pin_bit_new(buf, HAL_IN, &(pgpio->io_invert[i]), comp_id);
out_cnt++;
}
emcConfigureDefault( i );
}
// Trajectory wait output
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.traj-wait-out");
hal_pin_bit_new(buf, HAL_OUT, &(pgpio->traj_wait), comp_id);
*(pgpio->traj_wait) = 1;
// Scaner sync
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.scan-sync-in");
hal_pin_bit_new(buf, HAL_IN, &(pgpio->scan_sync), comp_id);
for(i=0; i < num_axis; i++)
{
// Check if pin already added
char contin = 0;
int j;
for(j=0; j < i; j++)
if(axis_map[j] == axis_map[i])
{
contin = 1;
break;
}
if(contin) continue;
// commanded position pin
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.cmd-pos", axis_map[i]);
retval = hal_pin_float_new(buf, HAL_IN, &(pgpio->cmd_pos[axis_map[i]]), comp_id);
if (retval != 0)
{
rtapi_app_exit();
return retval;
}
//feedback position pin
rtapi_snprintf(buf, HAL_NAME_LEN, "miniemcdrv.%d.fb-pos", axis_map[i]);
retval = hal_pin_float_new(buf, HAL_OUT, &(pgpio->fb_pos[axis_map[i]]), comp_id);
if (retval != 0)
{
rtapi_app_exit();
return retval;
}
}
/* export functions */
retval = hal_export_funct("update-miniemcdrv", update,
pgpio, 0, 0, comp_id);
if (retval != 0)
{
rtapi_print_msg(RTAPI_MSG_ERR,
"miniemcdrv: ERROR: count funct export failed\n");
rtapi_app_exit();
return -EIO;
}
ioctl(pgpio->fd, SCAN_PIN_SETUP_IOCTL, NULL);
//emcConfigurePin(11, egpOut );
//emcSetPin(11, 1 );
hal_ready(comp_id);
return 0;
}
int hm2_led_parse_md(hostmot2_t *hm2, int md_index) {
hm2_module_descriptor_t *md = &hm2->md[md_index];
int r;
//
// some standard sanity checks
//
if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 1, 4, 0x0000)) {
HM2_ERR("inconsistent Module Descriptor!\n");
return -EINVAL;
}
// LEDs were enumerated during llio setup
if (hm2->llio->num_leds == 0 || hm2->config.num_leds == 0) return 0;
if (hm2->config.num_leds > hm2->llio->num_leds) {
hm2->config.num_leds = hm2->llio->num_leds;
HM2_ERR( "There are only %d LEDs on this board type, defaulting to %d\n",
hm2->llio->num_leds, hm2->config.num_leds );
}
else if (hm2->config.num_leds == -1) {
hm2->config.num_leds = hm2->llio->num_leds;
}
//
// looks good, start initializing
//
// allocate the module-global HAL shared memory
hm2->led.instance = (hm2_led_instance_t *)hal_malloc(hm2->config.num_leds * sizeof(hm2_led_instance_t));
if (hm2->led.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->led.led_reg = (u32 *)kmalloc( sizeof(u32), GFP_KERNEL);
if (hm2->led.led_reg == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->led.led_addr = md->base_address;
// export to HAL
{
int i;
char name[HAL_NAME_LEN+1];
for (i = 0 ; i < hm2->config.num_leds ; i++) {
rtapi_snprintf(name, sizeof(name), "%s.led.CR%02d", hm2->llio->name, i + 1 );
r = hal_pin_bit_new(name, HAL_IN, &(hm2->led.instance[i].led), hm2->llio->comp_id);
if (r < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
}
return 1;
fail1:
kfree(hm2->led.led_reg);
fail0:
return r;
}
}
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 rtapi_app_main(void){
int retval;
int i, f;
char hal_name[HAL_NAME_LEN];
char *types[5] = {"invalid", "bit", "float", "s32", "u32"};
if (!config[0]) {
rtapi_print_msg(RTAPI_MSG_ERR, "The mux_generic component requires at least"
" one valid format string\n");
return -EINVAL;
}
comp_id = hal_init("mux_generic");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: ERROR: hal_init() failed\n");
return -1;
}
// allocate shared memory for the base struct
mux = hal_malloc(sizeof(mux_t));
if (mux == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"mux_generic component: Out of Memory\n");
hal_exit(comp_id);
return -1;
}
// Count the instances.
for (mux->num_insts = 0; config[mux->num_insts];mux->num_insts++) {}
mux->insts = hal_malloc(mux->num_insts * sizeof(mux_inst_t));
// Parse the config string
for (i = 0; i < mux->num_insts; i++) {
char c;
int s, p = 0;
mux_inst_t *inst = &mux->insts[i];
inst->in_type = -1;
inst->out_type = -1;
for (f = 0; (c = config[i][f]); f++) {
int type;
type = 0;
switch (c) {
case '0':
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
inst->size = (inst->size * 10) + (c - '0');
if (inst->size > MAX_SIZE) inst->size = MAX_SIZE;
break;
case 'b':
case 'B':
type = HAL_BIT;
break;
case 'f':
case 'F':
type = HAL_FLOAT;
break;
case 's':
case 'S':
type = HAL_S32;
break;
case 'u':
case 'U':
type = HAL_U32;
break;
default:
rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: invalid character in "
"fmt string\n");
goto fail0;
}
if (type) {
if (inst->in_type == -1) {
inst->in_type = type;
}
else if (inst->out_type == -1) {
inst->out_type = type;
}
else
{
rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: too many type "
"specifiers in fmt string\n");
goto fail0;
}
}
}
if (inst->size < 1) {
rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: No entry count given\n");
goto fail0;
}
else if (inst->size < 2) {
rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: A one-element mux makes "
"no sense\n");
goto fail0;
}
if (inst->in_type == -1) {
rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: No type specifiers in "
"fmt string\n");
goto fail0;
}
else if (inst->out_type == -1) {
inst->out_type = inst->in_type;
}
retval = rtapi_snprintf(hal_name, HAL_NAME_LEN, "mux-gen.%02i", i);
if (retval >= HAL_NAME_LEN) {
goto fail0;
}
if (inst->in_type == HAL_FLOAT || inst->out_type == HAL_FLOAT) {
retval = hal_export_funct(hal_name, write_fp, inst, 1, 0, comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: ERROR: function export"
" failed\n");
goto fail0;
}
}
else
{
retval = hal_export_funct(hal_name, write_nofp, inst, 0, 0, comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: ERROR: function export"
" failed\n");
goto fail0;
}
}
// Input pins
// if the mux size is a power of 2 then create the bit inputs
s = inst->size;
for(inst->num_bits = 1; (!((s >>= 1) & 1)); inst->num_bits++);
if (s == 1) { //make the bit pins
inst->sel_bit = hal_malloc(inst->num_bits * sizeof(hal_bit_t*));
for (p = 0; p < inst->num_bits; p++) {
retval = hal_pin_bit_newf(HAL_IN, &inst->sel_bit[p], comp_id,
"mux-gen.%02i.sel-bit-%02i", i, p);
if (retval != 0) {
goto fail0;
}
}
}
retval = hal_pin_u32_newf(HAL_IN, &(inst->sel_int), comp_id,
"mux-gen.%02i.sel-int", i);
if (retval != 0) {
goto fail0;
}
inst->inputs = hal_malloc(inst->size * sizeof(hal_data_u*));
for (p = 0; p < inst->size; p++) {
retval = rtapi_snprintf(hal_name, HAL_NAME_LEN,
"mux-gen.%02i.in-%s-%02i", i, types[inst->in_type], p);
if (retval >= HAL_NAME_LEN) {
goto fail0;
}
retval = hal_pin_new(hal_name, inst->in_type, HAL_IN,
(void**)&(inst->inputs[p]), comp_id);
if (retval != 0) {
goto fail0;
}
}
// Behaviour-modifiers
retval = hal_pin_bit_newf(HAL_IN, &inst->suppress, comp_id,
"mux-gen.%02i.suppress-no-input", i);
if (retval != 0) {
goto fail0;
}
retval = hal_pin_u32_newf(HAL_IN, &inst->debounce, comp_id,
"mux-gen.%02i.debounce-us", i);
if (retval != 0) {
goto fail0;
}
retval = hal_param_u32_newf(HAL_RO, &inst->timer, comp_id,
"mux-gen.%02i.elapsed", i);
if (retval != 0) {
goto fail0;
}
retval = hal_param_u32_newf(HAL_RO, &inst->selection, comp_id,
"mux-gen.%02i.selected", i);
if (retval != 0) {
goto fail0;
}
//output pins
retval = rtapi_snprintf(hal_name, HAL_NAME_LEN,
"mux-gen.%02i.out-%s", i, types[inst->out_type]);
if (retval >= HAL_NAME_LEN) {
goto fail0;
}
retval = hal_pin_new(hal_name, inst->out_type, HAL_OUT,
(void**)&(inst->output), comp_id);
if (retval != 0) {
goto fail0;
}
}
hal_ready(comp_id);
return 0;
fail0:
hal_exit(comp_id);
return -1;
}
int _rtapi_task_new(const rtapi_task_args_t *args) {
int task_id;
int __attribute__((__unused__)) retval = 0;
task_data *task;
/* get the mutex */
rtapi_mutex_get(&(rtapi_data->mutex));
#ifdef MODULE
/* validate owner */
if ((args->owner < 1) || (args->owner > RTAPI_MAX_MODULES)) {
rtapi_mutex_give(&(rtapi_data->mutex));
return -EINVAL;
}
if (module_array[args->owner].state != REALTIME) {
rtapi_mutex_give(&(rtapi_data->mutex));
return -EINVAL;
}
if ((args->flags & (TF_NONRT|TF_NOWAIT)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"task '%s' : nowait/posix flags not supported with kthreads\n",
args->name);
rtapi_mutex_give(&(rtapi_data->mutex));
return -EINVAL;
}
#endif
/* find an empty entry in the task array */
task_id = 1; // tasks start at one!
// go through task_array until an empty task slot is found
while ((task_id < RTAPI_MAX_TASKS) &&
(task_array[task_id].magic == TASK_MAGIC))
task_id++;
// if task_array is full, release lock and return error
if (task_id == RTAPI_MAX_TASKS) {
rtapi_mutex_give(&(rtapi_data->mutex));
return -ENOMEM;
}
task = &(task_array[task_id]);
// if requested priority is invalid, release lock and return error
if (PRIO_LT(args->prio,_rtapi_prio_lowest()) ||
PRIO_GT(args->prio,_rtapi_prio_highest())) {
rtapi_print_msg(RTAPI_MSG_ERR,
"New task %d '%s:%d': invalid priority %d "
"(highest=%d lowest=%d)\n",
task_id, args->name, rtapi_instance, args->prio,
_rtapi_prio_highest(),
_rtapi_prio_lowest());
rtapi_mutex_give(&(rtapi_data->mutex));
return -EINVAL;
}
if ((args->flags & (TF_NOWAIT|TF_NONRT)) == TF_NOWAIT) {
rtapi_print_msg(RTAPI_MSG_ERR,"task '%s' : nowait flag invalid for RT thread\n",
args->name);
rtapi_mutex_give(&(rtapi_data->mutex));
return -EINVAL;
}
// task slot found; reserve it and release lock
rtapi_print_msg(RTAPI_MSG_DBG,
"Creating new task %d '%s:%d': "
"req prio %d (highest=%d lowest=%d) stack=%lu fp=%d flags=%d\n",
task_id, args->name, rtapi_instance, args->prio,
_rtapi_prio_highest(),
_rtapi_prio_lowest(),
args->stacksize, args->uses_fp, args->flags);
task->magic = TASK_MAGIC;
/* fill out task structure */
task->owner = args->owner;
task->arg = args->arg;
task->stacksize = (args->stacksize < MIN_STACKSIZE) ? MIN_STACKSIZE : args->stacksize;
task->taskcode = args->taskcode;
task->prio = args->prio;
task->flags = args->flags;
task->uses_fp = args->uses_fp;
task->cpu = args->cpu_id > -1 ? args->cpu_id : rtapi_data->rt_cpu;
rtapi_print_msg(RTAPI_MSG_DBG, "Task CPU: %d\n", task->cpu);
rtapi_snprintf(task->name, sizeof(task->name),
"%s:%d", args->name, rtapi_instance);
task->name[sizeof(task->name) - 1] = '\0';
#ifdef MODULE
/* get space for the OS's task data - this is around 900 bytes, */
/* so we don't want to statically allocate it for unused tasks. */
ostask_array[task_id] = kmalloc(sizeof(RT_TASK), GFP_USER);
if (ostask_array[task_id] == NULL) {
rtapi_mutex_give(&(rtapi_data->mutex));
return -ENOMEM;
}
#ifdef HAVE_RTAPI_TASK_NEW_HOOK
/* kernel threads: rtapi_task_new_hook() should call OS to
initialize the task - use predetermined or explicitly assigned
CPU */
retval = _rtapi_task_new_hook(task, task_id);
if (retval) {
rtapi_print_msg(RTAPI_MSG_ERR,
"rt_task_create failed, rc = %d\n", retval );
/* couldn't create task, free task data memory */
kfree(ostask_array[task_id]);
rtapi_mutex_give(&(rtapi_data->mutex));
if (retval == ENOMEM) {
/* not enough space for stack */
return -ENOMEM;
}
/* unknown error */
return -EINVAL;
}
#endif
/* the task has been created, update data */
task->state = PAUSED;
retval = task_id;
#else /* userland thread */
/* userland threads: rtapi_task_new_hook() should perform any
thread system-specific tasks, and return task_id or an error
code back to the caller (how do we know the diff between an
error and a task_id???). */
task->state = USERLAND; // userland threads don't track this
# ifdef HAVE_RTAPI_TASK_NEW_HOOK
retval = _rtapi_task_new_hook(task,task_id);
# else
retval = task_id;
# endif
#endif /* userland thread */
rtapi_data->task_count++;
rtapi_mutex_give(&(rtapi_data->mutex));
/* announce the birth of a brand new baby task */
rtapi_print_msg(RTAPI_MSG_DBG,
"RTAPI: task %02d installed by module %02d, priority %d, code: %p\n",
task_id, task->owner, task->prio, args->taskcode);
return task_id;
}
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;
}
int rtapi_app_main(void)
{
char name[HAL_NAME_LEN + 1];
int n,i , retval, num_dac, num_enc;
unsigned int base=0x300;
/* only one port at the moment */
num_ports = 1;
n = 0;
#define ISA_BASE 0xC9000
#define ISA_MAX 0x100000 /* allgemeiner Speicherzugriff */
/* STEP 1: initialise the driver */
comp_id = hal_init("hal_evoreg");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: hal_init() failed\n");
return -1;
}
/* STEP 2: allocate shared memory for EVOREG data */
port_data_array = hal_malloc(num_ports * sizeof(evoreg_t));
if (port_data_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/*! \todo FIXME: Test memory area and setup the card */
port_data_array->io_base = ioremap(ISA_BASE, ISA_MAX - ISA_BASE);
rtapi_print_msg(RTAPI_MSG_ERR,"EVOREG: io_base: %p \n", port_data_array->io_base);
outw(0x82c9,base); /* set indexregister */
/* Set all outputs to zero */
writew(0, port_data_array->io_base + 0x20); /* digital out 0-15 */
writew(0, port_data_array->io_base + 0x40); /* digital out 16-23 */
writew(0, port_data_array->io_base + 0x60); /* DAC 1 */
writew(0, port_data_array->io_base + 0x80); /* DAC 2 */
writew(0, port_data_array->io_base + 0xa0); /* DAC 3 */
/* Reset Encoder's */
writew(0, port_data_array->io_base + 0x02); /* ENCODER 1 */
writew(0, port_data_array->io_base + 0x0a); /* ENCODER 2 */
writew(0, port_data_array->io_base + 0x12); /* ENCODER 3 */
/* STEP 3: export the pin(s) */
/* Export DAC pin's */
for ( num_dac=1; num_dac<=MAX_DAC; num_dac++) {
retval = hal_pin_float_newf(HAL_IN, &(port_data_array->dac_out[num_dac-1]),
comp_id, "evoreg.%d.dac-%02d-out", 1, num_dac);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: port %d var export failed with err=%i\n", n + 1,
retval);
hal_exit(comp_id);
return -1;
}
}
/* Export Encoder pin's */
for ( num_enc=1; num_enc<=MAX_ENC; num_enc++) {
retval = hal_pin_float_newf(HAL_OUT, &(port_data_array->position[num_enc - 1]),
comp_id, "evoreg.%d.position-%02d-in", 1, num_enc);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: port %d var export failed with err=%i\n", n + 1,
retval);
hal_exit(comp_id);
return -1;
}
}
/* Export IO pin's */
/* export write only HAL pin's for the input bit */
for ( i=0; i<=45;i++) {
retval += hal_pin_bit_newf(HAL_OUT, &(port_data_array->digital_in[i]),
comp_id, "evoreg.%d.pin-%02d-in", 1, i);
/* export another write only HAL pin for the same bit inverted */
/*
retval += hal_pin_bit_newf(HAL_OUT, &(port_data_array->digital_in[(2*i)+1]),
comp_id, "evoreg.%d.pin-%02d-in-not", 1, i); */
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: port %d var export failed with err=%i\n", n + 1,
retval);
hal_exit(comp_id);
return -1;
}
}
/* export read only HAL pin's for the output bit */
for ( i=0; i<=23;i++) {
retval += hal_pin_bit_newf(HAL_IN, &(port_data_array->digital_out[i]),
comp_id, "evoreg.%d.pin-%02d-out", 1, i);
/* export another read only HAL pin for the same bit inverted */
/*
retval += hal_pin_bit_newf(HAL_IN, &(port_data_array->digital_out[(2*i)+1]),
comp_id, "evoreg.%d.pin-%02d-out-not", 1, i)); */
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: port %d var export failed with err=%i\n", n + 1,
retval);
hal_exit(comp_id);
return -1;
}
}
/* export parameter for scaling */
retval = hal_param_float_newf(HAL_RW, &(port_data_array->pos_scale),
comp_id, "evoreg.%d.position-scale", 1);
if (retval != 0) {
return retval;
}
/* STEP 4: export function */
rtapi_snprintf(name, sizeof(name), "evoreg.%d.update", n + 1);
retval = hal_export_funct(name, update_port, &(port_data_array[n]), 1, 0,
comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: port %d write funct export failed\n", n + 1);
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"EVOREG: installed driver for %d card(s)\n", num_ports);
hal_ready(comp_id);
return 0;
}
int hm2_resolver_parse_md(hostmot2_t *hm2, int md_index) {
hm2_module_descriptor_t *md = &hm2->md[md_index];
int i, r = 0;
int resolvers_per_instance;
//
// some standard sanity checks
//
if ( ! hm2_md_is_consistent_or_complain(hm2, md_index, 0, 5, 4, 0x001F)) {
HM2_ERR("inconsistent resolver Module Descriptor!\n");
return -EINVAL;
}
if (hm2->resolver.num_instances != 0) {
HM2_ERR(
"found duplicate Module Descriptor for %s (inconsistent firmware), not loading driver\n",
hm2_get_general_function_name(md->gtag)
);
return -EINVAL;
}
resolvers_per_instance = hm2_resolver_get_param(2); // just returns 6 at the moment
if (hm2->config.num_resolvers > (md->instances * resolvers_per_instance)) {
HM2_ERR(
"config.num_resolvers=%d, but only %d are available, not loading driver\n",
hm2->config.num_resolvers,
md->instances * resolvers_per_instance);
return -EINVAL;
}
if (hm2->config.num_resolvers == 0) {
return 0;
}
//
// looks good, start initializing
//
/*At the moment it is not clear if there will ever be more than one resolver
instance. If there were to be more than one then they would need to have
a different base-address, and this code would need to be re-enterable.
A bridge to cross when we come to it */
if (hm2->config.num_resolvers == -1) {
hm2->resolver.num_resolvers = md->instances * resolvers_per_instance;
hm2->resolver.num_instances = md->instances;
} else {
hm2->resolver.num_resolvers = hm2->config.num_resolvers;
hm2->resolver.num_instances = md->instances;
}
hm2->resolver.hal = (hm2_resolver_global_t *)hal_malloc(
sizeof(hm2_resolver_global_t));
if (hm2->resolver.hal == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->resolver.instance = (hm2_resolver_instance_t *)hal_malloc(
hm2->resolver.num_resolvers * sizeof(hm2_resolver_instance_t));
if (hm2->resolver.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
for (i = 0 ; i < hm2->resolver.num_instances ; i++ ){
hm2->resolver.stride = md->register_stride;
hm2->resolver.clock_frequency = md->clock_freq;
hm2->resolver.version = md->version;
hm2->resolver.command_addr = md->base_address + (0 * md->register_stride);
hm2->resolver.data_addr = md->base_address + (1 * md->register_stride);
hm2->resolver.status_addr = md->base_address + (2 * md->register_stride);
hm2->resolver.velocity_addr = md->base_address + (3 * md->register_stride);
hm2->resolver.position_addr = md->base_address + (4 * md->register_stride);
// If there were multiple resolver function instances, this would need
// to be the number of resolvers for that particular instance
r = hm2_register_tram_read_region(hm2, hm2->resolver.status_addr,
sizeof(u32),
&hm2->resolver.status_reg);
r += hm2_register_tram_read_region(hm2, hm2->resolver.position_addr,
(hm2->resolver.num_resolvers * sizeof(u32)),
&hm2->resolver.position_reg);
r += hm2_register_tram_read_region(hm2, hm2->resolver.velocity_addr,
(hm2->resolver.num_resolvers * sizeof(u32)),
(u32**)&hm2->resolver.velocity_reg);
if (r < 0) {
HM2_ERR("error registering tram read region for Resolver "
"register (%d)\n", i);
goto fail1;
}
}
// export the resolvers to HAL
{
int i;
int ret;
char name[HAL_NAME_LEN + 1];
rtapi_snprintf(name, sizeof(name), "%s.resolver.excitation-khz",
hm2->llio->name);
ret= hal_pin_float_new(name, HAL_IO,
&(hm2->resolver.hal->pin.excitation_khz),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
for (i = 0; i < hm2->resolver.num_resolvers; i ++) {
// pins
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.position",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.position),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.angle",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.angle),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.velocity",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.velocity),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.count",
hm2->llio->name, i);
ret= hal_pin_s32_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.count),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.rawcounts",
hm2->llio->name, i);
ret= hal_pin_s32_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.rawcounts),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.reset",
hm2->llio->name, i);
ret= hal_pin_bit_new(name, HAL_IN,
&(hm2->resolver.instance[i].hal.pin.reset),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.index-enable",
hm2->llio->name, i);
ret= hal_pin_bit_new(name, HAL_IO,
&(hm2->resolver.instance[i].hal.pin.index_enable),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.error",
hm2->llio->name, i);
ret= hal_pin_bit_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.error),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
// parameters
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.scale",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_IO,
&(hm2->resolver.instance[i].hal.pin.scale),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.velocity-scale",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_IO,
&(hm2->resolver.instance[i].hal.pin.vel_scale),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.index-divisor",
hm2->llio->name, i);
ret= hal_pin_u32_new(name, HAL_RW,
&(hm2->resolver.instance[i].hal.pin.index_div),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
//
// init the hal objects that need it
// the things not initialized here will be set by hm2_resolver_tram_init()
//
*hm2->resolver.instance[i].hal.pin.reset = 0;
*hm2->resolver.instance[i].hal.pin.scale = 1.0;
*hm2->resolver.instance[i].hal.pin.vel_scale = 1.0;
*hm2->resolver.instance[i].hal.pin.index_div = 1;
*hm2->resolver.hal->pin.excitation_khz = -1; // don't-write
hm2->resolver.kHz = (hm2->resolver.clock_frequency / 5000);
}
}
return hm2->resolver.num_instances;
fail1:
// This is where we would kfree anything kmalloced.
fail0:
hm2->resolver.num_instances = 0;
return r;
}
