halcmd_commands.c

CNC 的开放码,EMC2 V2.2.8版

    }
    argv[m++] = NULL;
    retval = do_loadusr_cmd(argv);
#else
    static char *rtmod_dir = EMC2_RTLIB_DIR;
    struct stat stat_buf;
    char mod_path[MAX_CMD_LEN+1];

    if (hal_get_lock()&HAL_LOCK_LOAD) {
	halcmd_error("HAL is locked, loading of modules is not permitted\n");
	return HAL_PERM;
    }
    if ( (strlen(rtmod_dir)+strlen(mod_name)+5) > MAX_CMD_LEN ) {
	halcmd_error("Module path too long\n");
	return -1;
    }
    /* make full module name '<path>/<name>.o' */
    strcpy (mod_path, rtmod_dir);
    strcat (mod_path, "/");
    strcat (mod_path, mod_name);
    strcat (mod_path, MODULE_EXT);
    /* is there a file with that name? */
    if ( stat(mod_path, &stat_buf) != 0 ) {
        /* can't find it */
        halcmd_error("Can't find module '%s' in %s\n", mod_name, rtmod_dir);
        return -1;
    }
    
    // TODO - FIXME - remove test after 2.2.x when blocks isn't functional anymore
    if (strncmp(mod_name, "blocks", 6) == 0) {
	//usign RTAPI_MSG_ERR as that is the default warning level for halcmd
        halcmd_error(
            "blocks is deprecated, "
            "use the subcomponents generated by 'comp' instead\n");
    }

    argv[0] = EMC2_BIN_DIR "/emc_module_helper";
    argv[1] = "insert";
    argv[2] = mod_path;
    /* loop thru remaining arguments */
    n = 0;
    m = 3;
    while ( args[n] && args[n][0] != '\0' ) {
        argv[m++] = args[n++];
    }
    /* add a NULL to terminate the argv array */
    argv[m] = NULL;

    retval = hal_systemv(argv);
#endif

    if ( retval != 0 ) {
	halcmd_error("insmod failed, returned %d\n", retval );
	return -1;
    }
    /* make the args that were passed to the module into a single string */
    n = 0;
    arg_string[0] = '\0';
    while ( args[n] && args[n][0] != '\0' ) {
	strncat(arg_string, args[n++], MAX_CMD_LEN);
	strncat(arg_string, " ", MAX_CMD_LEN);
    }
    /* allocate HAL shmem for the string */
    cp1 = hal_malloc(strlen(arg_string)+1);
    if ( cp1 == NULL ) {
	halcmd_error("failed to allocate memory for module args\n");
	return -1;
    }
    /* copy string to shmem */
    strcpy (cp1, arg_string);
    /* get mutex before accessing shared data */
    rtapi_mutex_get(&(hal_data->mutex));
    /* search component list for the newly loaded component */
    comp = halpr_find_comp_by_name(mod_name);
    if (comp == 0) {
	rtapi_mutex_give(&(hal_data->mutex));
	halcmd_error("module '%s' not loaded\n", mod_name);
	return HAL_INVAL;
    }
    /* link args to comp struct */
    comp->insmod_args = SHMOFF(cp1);
    rtapi_mutex_give(&(hal_data->mutex));
    /* print success message */
    halcmd_info("Realtime module '%s' loaded\n", mod_name);
    return 0;
}

int do_delsig_cmd(char *mod_name)
{
    int next, retval, retval1, n;
    hal_sig_t *sig;
    char sigs[MAX_EXPECTED_SIGS][HAL_NAME_LEN+1];

    /* check for "all" */
    if ( strcmp(mod_name, "all" ) != 0 ) {
	retval = hal_signal_delete(mod_name);
	if (retval == 0) {
	    /* print success message */
	    halcmd_info("Signal '%s' deleted'\n", mod_name);
	}
	return retval;
    } else {
	/* build a list of signal(s) to delete */
	n = 0;
	rtapi_mutex_get(&(hal_data->mutex));

	next = hal_data->sig_list_ptr;
	while (next != 0) {
	    sig = SHMPTR(next);
	    /* we want to unload this signal, remember it's name */
	    if ( n < ( MAX_EXPECTED_SIGS - 1 ) ) {
	        strncpy(sigs[n++], sig->name, HAL_NAME_LEN );
	    }
	    next = sig->next_ptr;
	}
	rtapi_mutex_give(&(hal_data->mutex));
	sigs[n][0] = '\0';

	if ( ( sigs[0][0] == '\0' )) {
	    /* desired signals not found */
	    halcmd_error("no signals found to be deleted\n");
	    return -1;
	}
	/* we now have a list of components, unload them */
	n = 0;
	retval1 = 0;
	while ( sigs[n][0] != '\0' ) {
	    retval = hal_signal_delete(sigs[n]);
	/* check for fatal error */
	    if ( retval < -1 ) {
		return retval;
	    }
	    /* check for other error */
	    if ( retval != 0 ) {
		retval1 = retval;
	    }
	    if (retval == 0) {
		/* print success message */
		halcmd_info("Signal '%s' deleted'\n",
		sigs[n]);
	    }
	    n++;
	}
    }
    return retval1;
}

int do_unloadusr_cmd(char *mod_name)
{
    int next, all;
    hal_comp_t *comp;
    pid_t ourpid = getpid();

    /* check for "all" */
    if ( strcmp(mod_name, "all" ) == 0 ) {
	all = 1;
    } else {
	all = 0;
    }
    /* build a list of component(s) to unload */
    rtapi_mutex_get(&(hal_data->mutex));
    next = hal_data->comp_list_ptr;
    while (next != 0) {
	comp = SHMPTR(next);
	if ( comp->type == 0 && comp->pid != ourpid) {
	    /* found a userspace component besides us */
	    if ( all || ( strcmp(mod_name, comp->name) == 0 )) {
		/* we want to unload this component, send it SIGTERM */
                kill(abs(comp->pid), SIGTERM);
	    }
	}
	next = comp->next_ptr;
    }
    rtapi_mutex_give(&(hal_data->mutex));
    return 0;
}


int do_unloadrt_cmd(char *mod_name)
{
    int next, retval, retval1, n, all;
    hal_comp_t *comp;
    char comps[64][HAL_NAME_LEN+1];

    /* check for "all" */
    if ( strcmp(mod_name, "all" ) == 0 ) {
	all = 1;
    } else {
	all = 0;
    }
    /* build a list of component(s) to unload */
    n = 0;
    rtapi_mutex_get(&(hal_data->mutex));
    next = hal_data->comp_list_ptr;
    while (next != 0) {
	comp = SHMPTR(next);
	if ( comp->type == 1 ) {
	    /* found a realtime component */
	    if ( all || ( strcmp(mod_name, comp->name) == 0 )) {
		/* we want to unload this component, remember it's name */
		if ( n < 63 ) {
		    strncpy(comps[n++], comp->name, HAL_NAME_LEN );
		}
	    }
	}
	next = comp->next_ptr;
    }
    rtapi_mutex_give(&(hal_data->mutex));
    /* mark end of list */
    comps[n][0] = '\0';
    if ( !all && ( comps[0][0] == '\0' )) {
	/* desired component not found */
	halcmd_error("component '%s' is not loaded\n", mod_name);
	return -1;
    }
    /* we now have a list of components, unload them */
    n = 0;
    retval1 = 0;
    while ( comps[n][0] != '\0' ) {
	retval = unloadrt_comp(comps[n++]);
	/* check for fatal error */
	if ( retval < -1 ) {
	    return retval;
	}
	/* check for other error */
	if ( retval != 0 ) {
	    retval1 = retval;
	}
    }
    if (retval1 != HAL_SUCCESS) {
	halcmd_error("unloadrt failed\n");
    }
    return retval1;
}

static int unloadrt_comp(char *mod_name)
{
    int retval;
    char *argv[4];

#if defined(RTAPI_SIM)
    argv[0] = EMC2_BIN_DIR "/rtapi_app";
    argv[1] = "unload";
#else
    argv[0] = EMC2_BIN_DIR "/emc_module_helper";
    argv[1] = "remove";
#endif
    argv[2] = mod_name;
    /* add a NULL to terminate the argv array */
    argv[3] = NULL;

    retval = hal_systemv(argv);

    if ( retval != 0 ) {
	halcmd_error("rmmod failed, returned %d\n", retval);
	return -1;
    }
    /* print success message */
    halcmd_info("Realtime module '%s' unloaded\n",
	mod_name);
    return 0;
}

int do_unload_cmd(char *mod_name) {
    if(strcmp(mod_name, "all") == 0) {
        int res = do_unloadusr_cmd(mod_name);
        if(res) return res;
        return do_unloadrt_cmd(mod_name);
    } else {
        hal_comp_t *comp;
        int type = -1;
        rtapi_mutex_get(&(hal_data->mutex));
        comp = halpr_find_comp_by_name(mod_name);
        if(comp) type = comp->type;
        rtapi_mutex_give(&(hal_data->mutex));
        if(type == -1) {
            halcmd_error("component '%s' is not loaded\n",
                mod_name);
            return -1;
        }
        if(type == 1) return do_unloadrt_cmd(mod_name);
        else return do_unloadusr_cmd(mod_name);
    }
}

int do_loadusr_cmd(char *args[])
{
    int wait_flag, wait_comp_flag, name_flag, ignore_flag;
    char *prog_name, *new_comp_name=NULL;
    char *argv[MAX_TOK+1];
    int n, m, retval, status;
    pid_t pid;

    int argc = 0;
    while(args[argc] && *args[argc]) argc++;
    args--; argc++;

    if (hal_get_lock()&HAL_LOCK_LOAD) {
	halcmd_error("HAL is locked, loading of programs is not permitted\n");
	return HAL_PERM;
    }
    wait_flag = 0;
    wait_comp_flag = 0;
    name_flag = 0;
    ignore_flag = 0;
    prog_name = NULL;

    /* check for options (-w, -i, and/or -r) */
    optind = 0;
    while (1) {
	int c = getopt(argc, args, "+wWin:");
	if(c == -1) break;

	switch(c) {
	    case 'w':
		wait_flag = 1; break;
	    case 'W':
		wait_comp_flag = 1; break;
	    case 'i':
		ignore_flag = 1; break;
	    case 'n':
		new_comp_name = optarg; break;
	    default:
		return HAL_INVAL;
		break;
	}
    }
    /* get program and component name */
    args += optind;
    prog_name = *args++;
    if(!new_comp_name) {
	new_comp_name = prog_name;
    }
    /* prepare to exec() the program */
    argv[0] = prog_name;
    /* loop thru remaining arguments */
    n = 0;
    m = 1;
    while ( args[n] && args[n][0] != '\0' ) {
        argv[m++] = args[n++];
    }
    /* add a NULL to terminate the argv array */
    argv[m] = NULL;
    /* start the child process */
    pid = hal_systemv_nowait(argv);
    /* make sure we reconnected to the HAL */
    if (comp_id < 0) {
	fprintf(stderr, "halcmd: hal_init() failed after fork: %d\n",
	    comp_id );
	exit(-1);
    }
    hal_ready(comp_id);
    if ( wait_comp_flag ) {
        int ready = 0, count=0, exited=0;
        hal_comp_t *comp = NULL;
	retval = 0;
        while(!ready && !exited) {
	    /* sleep for 10mS */
            struct timespec ts = {0, 10 * 1000 * 1000};
            nanosleep(&ts, NULL);
	    /* check for program ending */
	    retval = waitpid( pid, &status, WNOHANG );
	    if ( retval != 0 ) {
		exited = 1;
	    }
	    /* check for program becoming ready */
            rtapi_mutex_get(&(hal_data->mutex));
            comp = halpr_find_comp_by_name(new_comp_name);
            if(comp && comp->ready) {
                ready = 1;
            }
            rtapi_mutex_give(&(hal_data->mutex));
	    /* pacify the user */
            count++;
            if(count == 200) {
                fprintf(stderr, "Waiting for component '%s' to become ready.",
                        new_comp_name);
                fflush(stderr);
            } else if(count > 200 && count % 10 == 0) {
                fprintf(stderr, ".");
                fflush(stderr);
            }
        }
        if (count >= 100) {
	    /* terminate pacifier */
	    fprintf(stderr, "\n");
	}
	/* did it work? */
	if (ready) {
	    halcmd_info("Component '%s' ready\n", new_comp_name);
	} else {
	    if ( retval < 0 ) {
		halcmd_error("\nwaitpid(%d) failed\n", pid);
	    } else {
		halcmd_error("%s exited without becoming ready\n", prog_name);
	    }
	    return -1;
	}
    }
    if ( wait_flag ) {
	/* wait for child process to complete */
	retval = waitpid ( pid, &status, 0 );
	/* check result of waitpid() */
	if ( retval < 0 ) {
	    halcmd_error("waitpid(%d) failed\n", pid);
	    return -1;
	}
	if ( WIFEXITED(status) == 0 ) {
	    halcmd_error("program '%s' did not exit normally\n", prog_name );
	    return -1;
	}
	if ( ignore_flag == 0 ) {
	    retval = WEXITSTATUS(status);
	    if ( retval != 0 ) {
		halcmd_error("program '%s' failed, returned %d\n", prog_name, retval );
		return -1;
	    }
	}
	/* print success message */
	halcmd_info("Program '%s' finished\n", prog_name);
    } else {
	/* print success message */
	halcmd_info("Program '%s' started\n", prog_name);
    }
    return 0;
}


int do_waitusr_cmd(char *comp_name)
{
    hal_comp_t *comp;
    int exited;

    if (*comp_name == '\0') {
	halcmd_error("component name missing\n");
	return HAL_INVAL;
    }
    rtapi_mutex_get(&(hal_data->mutex));
    comp = halpr_find_comp_by_name(comp_name);
    if (comp == NULL) {
	rtapi_mutex_give(&(hal_data->mutex));
	halcmd_error("component '%s' not found\n", comp_name);
	return HAL_INVAL;
    }
    if (comp->type != 0) {
	rtapi_mutex_give(&(hal_data->mutex));
	halcmd_error("'%s' is not a userspace component\n", comp_name);
	return HAL_INVAL;
    }
    rtapi_mutex_give(&(hal_data->mutex));
    /* let the user know what is going on */
    halcmd_info("Waiting for component '%s'\n", comp_name);
    exited = 0;
    while(!exited) {
	/* sleep for 200mS */
	struct timespec ts = {0, 200 * 1000 * 1000};
	nanosleep(&ts, NULL);
	/* check for component still around */
	rtapi_mutex_get(&(hal_data->mutex));
	comp = halpr_find_comp_by_name(comp_name);
	if(comp == NULL) {
		exited = 1;
	}
	rtapi_mutex_give(&(hal_data->mutex));
    }
    halcmd_info("Component '%s' finished\n", comp_name);
    return 0;
}


static void print_comp_info(char **patterns)
{
    int next;
    hal_comp_t *comp;

    if (scriptmode == 0) {
	halcmd_output("Loaded HAL Components:\n");