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;
}
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 init_streamer(int num, streamer_t *str)
{
int retval, n, usefp;
pin_data_t *pptr;
char buf[HAL_NAME_LEN + 1];
/* export "standard" pins and params */
retval = hal_pin_bit_newf(HAL_OUT, &(str->empty), comp_id,
"streamer.%d.empty", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: 'empty' pin export failed\n");
return -EIO;
}
retval = hal_pin_bit_newf(HAL_IN, &(str->enable), comp_id,
"streamer.%d.enable", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: 'enable' pin export failed\n");
return -EIO;
}
retval = hal_pin_s32_newf(HAL_OUT, &(str->curr_depth), comp_id,
"streamer.%d.curr-depth", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: 'curr_depth' pin export failed\n");
return -EIO;
}
retval = hal_pin_s32_newf(HAL_IO, &(str->underruns), comp_id,
"streamer.%d.underruns", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: 'underruns' pin export failed\n");
return -EIO;
}
retval = hal_pin_bit_newf(HAL_IN, &(str->clock), comp_id,
"streamer.%d.clock", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: 'clock' pin export failed\n");
return -EIO;
}
retval = hal_pin_s32_newf(HAL_IN, &(str->clock_mode), comp_id,
"streamer.%d.clock-mode", num);
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: 'clock_mode' pin export failed\n");
return -EIO;
}
/* init the standard pins and params */
*(str->empty) = 1;
*(str->enable) = 1;
*(str->curr_depth) = 0;
*(str->underruns) = 0;
*(str->clock_mode) = 0;
pptr = str->pins;
usefp = 0;
/* export user specified pins (the ones that stream data) */
for ( n = 0 ; n < hal_stream_element_count(&str->fifo); n++ ) {
rtapi_snprintf(buf, sizeof(buf), "streamer.%d.pin.%d", num, n);
retval = hal_pin_new(buf, hal_stream_element_type(&str->fifo, n), HAL_OUT, (void **)pptr, comp_id );
if (retval != 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: pin '%s' export failed\n", buf);
return -EIO;
}
/* init the pin value */
switch ( hal_stream_element_type(&str->fifo, 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, sizeof(buf), "streamer.%d", num);
retval = hal_export_funct(buf, update, str, usefp, 0, comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"STREAMER: ERROR: function export failed\n");
return retval;
}
return 0;
}
