/*********************************************************************** * 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; }