halcmd_commands.c

CNC 的开放码,EMC2 V2.2.8版

        nanosleep(&ts, NULL);
        rtapi_mutex_get(&(hal_data->mutex));
    }
    strncpy(hal_data->constructor_prefix, inst_name, HAL_NAME_LEN);
    hal_data->constructor_prefix[HAL_NAME_LEN]=0;
    hal_data->pending_constructor = comp->make;
    rtapi_mutex_give(&(hal_data->mutex));

    if(fputc(' ', f) == EOF) {
        halcmd_error( "cannot write to proc entry: %s\n",
                strerror(errno));
        fclose(f);
        rtapi_mutex_get(&(hal_data->mutex));
        hal_data->pending_constructor = 0;
        rtapi_mutex_give(&(hal_data->mutex));
        return HAL_FAIL;
    }
    if(fclose(f) != 0) {
        halcmd_error(
                "cannot close proc entry: %s\n",
                strerror(errno));
        rtapi_mutex_get(&(hal_data->mutex));
        hal_data->pending_constructor = 0;
        rtapi_mutex_give(&(hal_data->mutex));
        return HAL_FAIL;
    }

    while(hal_data->pending_constructor) {
        struct timespec ts = {0, 100 * 1000 * 1000}; // 100ms
        nanosleep(&ts, NULL);
    }
    }
#endif
    rtapi_mutex_get(&hal_data->mutex);
    {
    hal_comp_t *inst = halpr_alloc_comp_struct();
    if (inst == 0) {
        /* couldn't allocate structure */
        rtapi_mutex_give(&(hal_data->mutex));
        halcmd_error(
            "insufficient memory for instance '%s'\n", inst_name);
        return HAL_NOMEM;
    }
    inst->comp_id = comp->comp_id | 0x10000;
    inst->mem_id = -1;
    inst->type = 2;
    inst->pid = 0;
    inst->ready = 1;
    inst->shmem_base = 0;
    rtapi_snprintf(inst->name, HAL_NAME_LEN, "%s", inst_name);
    /* insert new structure at head of list */
    inst->next_ptr = hal_data->comp_list_ptr;
    hal_data->comp_list_ptr = SHMOFF(inst);

    rtapi_mutex_give(&(hal_data->mutex));
    }
    return HAL_SUCCESS;
}
#endif /* newinst deferred */

int do_newsig_cmd(char *name, char *type)
{
    int retval;

    if (strcasecmp(type, "bit") == 0) {
	retval = hal_signal_new(name, HAL_BIT);
    } else if (strcasecmp(type, "float") == 0) {
	retval = hal_signal_new(name, HAL_FLOAT);
    } else if (strcasecmp(type, "u32") == 0) {
	retval = hal_signal_new(name, HAL_U32);
    } else if (strcasecmp(type, "s32") == 0) {
	retval = hal_signal_new(name, HAL_S32);
    } else {
	halcmd_error("Unknown signal type '%s'\n", type);
	retval = HAL_INVAL;
    }
    if (retval != HAL_SUCCESS) {
	halcmd_error("newsig failed\n");
    }
    return retval;
}

static int set_common(hal_type_t type, void *d_ptr, char *value) {
    // This function assumes that the mutex is held
    int retval = 0;
    float fval;
    long lval;
    unsigned long ulval;
    char *cp = value;

    switch (type) {
    case HAL_BIT:
	if ((strcmp("1", value) == 0) || (strcasecmp("TRUE", value) == 0)) {
	    *(hal_bit_t *) (d_ptr) = 1;
	} else if ((strcmp("0", value) == 0)
	    || (strcasecmp("FALSE", value)) == 0) {
	    *(hal_bit_t *) (d_ptr) = 0;
	} else {
	    halcmd_error("value '%s' invalid for bit\n", value);
	    retval = HAL_INVAL;
	}
	break;
    case HAL_FLOAT:
	fval = strtod ( value, &cp );
	if ((*cp != '\0') && (!isspace(*cp))) {
	    /* invalid character(s) in string */
	    halcmd_error("value '%s' invalid for float\n", value);
	    retval = HAL_INVAL;
	} else {
	    *((hal_float_t *) (d_ptr)) = fval;
	}
	break;
    case HAL_S32:
	lval = strtol(value, &cp, 0);
	if ((*cp != '\0') && (!isspace(*cp))) {
	    /* invalid chars in string */
	    halcmd_error("value '%s' invalid for S32\n", value);
	    retval = HAL_INVAL;
	} else {
	    *((hal_s32_t *) (d_ptr)) = lval;
	}
	break;
    case HAL_U32:
	ulval = strtoul(value, &cp, 0);
	if ((*cp != '\0') && (!isspace(*cp))) {
	    /* invalid chars in string */
	    halcmd_error("value '%s' invalid for U32\n", value);
	    retval = HAL_INVAL;
	} else {
	    *((hal_u32_t *) (d_ptr)) = ulval;
	}
	break;
    default:
	/* Shouldn't get here, but just in case... */
	halcmd_error("bad type %d\n", type);
	retval = HAL_INVAL;
    }
    return retval;
}

int do_setp_cmd(char *name, char *value)
{
    int retval;
    hal_param_t *param;
    hal_pin_t *pin;
    hal_type_t type;
    void *d_ptr;

    halcmd_info("setting parameter '%s' to '%s'\n", name, value);
    /* get mutex before accessing shared data */
    rtapi_mutex_get(&(hal_data->mutex));
    /* search param list for name */
    param = halpr_find_param_by_name(name);
    if (param == 0) {
        pin = halpr_find_pin_by_name(name);
        if(pin == 0) {
            rtapi_mutex_give(&(hal_data->mutex));
            halcmd_error("parameter or pin '%s' not found\n", name);
            return HAL_INVAL;
        } else {
            /* found it */
            type = pin->type;
            if(pin->dir == HAL_OUT) {
                rtapi_mutex_give(&(hal_data->mutex));
                halcmd_error("pin '%s' is not writable\n", name);
                return HAL_INVAL;
            }
            if(pin->signal != 0) {
                rtapi_mutex_give(&(hal_data->mutex));
                halcmd_error("pin '%s' is connected to a signal\n", name);
                return HAL_INVAL;
            }
            // d_ptr = (void*)SHMPTR(pin->dummysig);
            d_ptr = (void*)&pin->dummysig;
        }
    } else {
        /* found it */
        type = param->type;
        /* is it read only? */
        if (param->dir == HAL_RO) {
            rtapi_mutex_give(&(hal_data->mutex));
            halcmd_error("param '%s' is not writable\n", name);
            return HAL_INVAL;
        }
        d_ptr = SHMPTR(param->data_ptr);
    }

    retval = set_common(type, d_ptr, value);

    rtapi_mutex_give(&(hal_data->mutex));
    if (retval == 0) {
	/* print success message */
        if(param) {
            halcmd_info("Parameter '%s' set to %s\n", name, value);
        } else {
            halcmd_info("Pin '%s' set to %s\n", name, value);
	}
    } else {
	halcmd_error("setp failed\n");
    }
    return retval;

}

int do_ptype_cmd(char *name)
{
    hal_param_t *param;
    hal_pin_t *pin;
    hal_type_t type;
    
    rtapi_print_msg(RTAPI_MSG_DBG, "getting parameter '%s'\n", name);
    /* get mutex before accessing shared data */
    rtapi_mutex_get(&(hal_data->mutex));
    /* search param list for name */
    param = halpr_find_param_by_name(name);
    if (param) {
        /* found it */
        type = param->type;
        halcmd_output("%s\n", data_type2(type));
        rtapi_mutex_give(&(hal_data->mutex));
        return HAL_SUCCESS;
    }
        
    /* not found, search pin list for name */
    pin = halpr_find_pin_by_name(name);
    if(pin) {
        /* found it */
        type = pin->type;
        halcmd_output("%s\n", data_type2(type));
        rtapi_mutex_give(&(hal_data->mutex));
        return HAL_SUCCESS;
    }   
    
    rtapi_mutex_give(&(hal_data->mutex));
    halcmd_error("parameter '%s' not found\n", name);
    return HAL_INVAL;
}


int do_getp_cmd(char *name)
{
    hal_param_t *param;
    hal_pin_t *pin;
    hal_sig_t *sig;
    hal_type_t type;
    void *d_ptr;
    
    rtapi_print_msg(RTAPI_MSG_DBG, "getting parameter '%s'\n", name);
    /* get mutex before accessing shared data */
    rtapi_mutex_get(&(hal_data->mutex));
    /* search param list for name */
    param = halpr_find_param_by_name(name);
    if (param) {
        /* found it */
        type = param->type;
        d_ptr = SHMPTR(param->data_ptr);
        halcmd_output("%s\n", data_value2((int) type, d_ptr));
        rtapi_mutex_give(&(hal_data->mutex));
        return HAL_SUCCESS;
    }
        
    /* not found, search pin list for name */
    pin = halpr_find_pin_by_name(name);
    if(pin) {
        /* found it */
        type = pin->type;
        if (pin->signal != 0) {
            sig = SHMPTR(pin->signal);
            d_ptr = SHMPTR(sig->data_ptr);
        } else {
            sig = 0;
            d_ptr = &(pin->dummysig);
        }
        halcmd_output("%s\n", data_value2((int) type, d_ptr));
        rtapi_mutex_give(&(hal_data->mutex));
        return HAL_SUCCESS;
    }   
    
    rtapi_mutex_give(&(hal_data->mutex));
    halcmd_error("parameter '%s' not found\n", name);
    return HAL_INVAL;
}

int do_sets_cmd(char *name, char *value)
{
    int retval;
    hal_sig_t *sig;
    hal_type_t type;
    void *d_ptr;

    rtapi_print_msg(RTAPI_MSG_DBG, "setting signal '%s'\n", name);
    /* get mutex before accessing shared data */
    rtapi_mutex_get(&(hal_data->mutex));
    /* search signal list for name */
    sig = halpr_find_sig_by_name(name);
    if (sig == 0) {
	rtapi_mutex_give(&(hal_data->mutex));
	halcmd_error("signal '%s' not found\n", name);
	return HAL_INVAL;
    }
    /* found it - does it have a writer? */
    if (sig->writers > 0) {
	rtapi_mutex_give(&(hal_data->mutex));
	halcmd_error("signal '%s' already has writer(s)\n", name);
	return HAL_INVAL;
    }
    /* no writer, so we can safely set it */
    type = sig->type;
    d_ptr = SHMPTR(sig->data_ptr);
    retval = set_common(type, d_ptr, value);
    rtapi_mutex_give(&(hal_data->mutex));
    if (retval == 0) {
	/* print success message */
	halcmd_info("Signal '%s' set to %s\n", name, value);
    } else {
	halcmd_error("sets failed\n");
    }
    return retval;

}

int do_stype_cmd(char *name)
{
    hal_sig_t *sig;
    hal_type_t type;

    rtapi_print_msg(RTAPI_MSG_DBG, "getting signal '%s'\n", name);
    /* get mutex before accessing shared data */
    rtapi_mutex_get(&(hal_data->mutex));
    /* search signal list for name */
    sig = halpr_find_sig_by_name(name);
    if (sig == 0) {
	rtapi_mutex_give(&(hal_data->mutex));
	halcmd_error("signal '%s' not found\n", name);
	return HAL_INVAL;
    }
    /* found it */
    type = sig->type;
    halcmd_output("%s\n", data_type2(type));
    rtapi_mutex_give(&(hal_data->mutex));
    return HAL_SUCCESS;
}

int do_gets_cmd(char *name)
{
    hal_sig_t *sig;
    hal_type_t type;
    void *d_ptr;

    rtapi_print_msg(RTAPI_MSG_DBG, "getting signal '%s'\n", name);
    /* get mutex before accessing shared data */
    rtapi_mutex_get(&(hal_data->mutex));
    /* search signal list for name */
    sig = halpr_find_sig_by_name(name);
    if (sig == 0) {
	rtapi_mutex_give(&(hal_data->mutex));
	halcmd_error("signal '%s' not found\n", name);
	return HAL_INVAL;
    }
    /* found it */
    type = sig->type;
    d_ptr = SHMPTR(sig->data_ptr);
    halcmd_output("%s\n", data_value2((int) type, d_ptr));
    rtapi_mutex_give(&(hal_data->mutex));
    return HAL_SUCCESS;
}

int do_show_cmd(char *type, char **patterns)
{

    if (rtapi_get_msg_level() == RTAPI_MSG_NONE) {
	/* must be -Q, don't print anything */
	return 0;
    }
    if (!type || *type == '\0') {
	/* print everything */
	print_comp_info(NULL);
	print_pin_info(NULL);
	print_sig_info(NULL);
	print_param_info(NULL);
	print_funct_info(NULL);
	print_thread_info(NULL);
    } else if (strcmp(type, "all") == 0) {
	/* print everything, using the pattern */
	print_comp_info(patterns);
	print_pin_info(patterns);
	print_sig_info(patterns);
	print_param_info(patterns);
	print_funct_info(patterns);
	print_thread_info(patterns);
    } else if (strcmp(type, "comp") == 0) {
	print_comp_info(patterns);
    } else if (strcmp(type, "pin") == 0) {
	print_pin_info(patterns);
    } else if (strcmp(type, "sig") == 0) {
	print_sig_info(patterns);
    } else if (strcmp(type, "param") == 0) {
	print_param_info(patterns);
    } else if (strcmp(type, "funct") == 0) {
	print_funct_info(patterns);
    } else if (strcmp(type, "thread") == 0) {
	print_thread_info(patterns);
    } else {
	halcmd_error("Unknown 'show' type '%s'\n", type);
	return -1;
    }
    return 0;
}

int do_list_cmd(char *type, char **patterns)
{
    if ( !type) {
	halcmd_error("'list' requires type'\n");
	return -1;
    }
    if (rtapi_get_msg_level() == RTAPI_MSG_NONE) {
	/* must be -Q, don't print anything */
	return 0;
    }
    if (strcmp(type, "comp") == 0) {
	print_comp_names(patterns);
    } else if (strcmp(type, "pin") == 0) {
	print_pin_names(patterns);
    } else if (strcmp(type, "sig") == 0) {
	print_sig_names(patterns);
    } else if (strcmp(type, "param") == 0) {
	print_param_names(patterns);
    } else if (strcmp(type, "funct") == 0) {
	print_funct_names(patterns);
    } else if (strcmp(type, "thread") == 0) {
	print_thread_names(patterns);
    } else {
	halcmd_error("Unknown 'list' type '%s'\n", type);
	return -1;
    }
    return 0;
}

int do_status_cmd(char *type)
{

    if (rtapi_get_msg_level() == RTAPI_MSG_NONE) {
	/* must be -Q, don't print anything */
	return 0;
    }
    if ((type == NULL) || (strcmp(type, "all") == 0)) {
	/* print everything */
	/* add other status functions here if/when they are defined */
	print_lock_status();
	print_mem_status();
    } else if (strcmp(type, "lock") == 0) {
	print_lock_status();
    } else if (strcmp(type, "mem") == 0) {
	print_mem_status();
    } else {
	halcmd_error("Unknown 'status' type '%s'\n", type);
	return -1;
    }
    return 0;
}

int do_loadrt_cmd(char *mod_name, char *args[])
{
    char arg_string[MAX_CMD_LEN+1];
    int m=0, n=0, retval;
    hal_comp_t *comp;
    char *argv[MAX_TOK+3];
    char *cp1;
#if defined(RTAPI_SIM)
    argv[m++] = "-Wn";
    argv[m++] = mod_name;
    argv[m++] = EMC2_BIN_DIR "/rtapi_app";
    argv[m++] = "load";
    argv[m++] = mod_name;
    /* loop thru remaining arguments */
    while ( args[n] && args[n][0] != '\0' ) {
        argv[m++] = args[n++];