halrmt.c

CNC 的开放码,EMC2 V2.2.8版

    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 {
	    retval = HAL_INVAL;
	}
	break;
    case HAL_FLOAT:
	fval = strtod ( value, &cp );
	if ((*cp != '\0') && (!isspace(*cp))) {
	    /* invalid character(s) in string */
	    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 */
	    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 */
	    retval = HAL_INVAL;
	} else {
	    *((hal_u32_t *) (d_ptr)) = ulval;
	}
	break;
    default:
	/* Shouldn't get here, but just in case... */
	retval = HAL_INVAL;
    }
    return retval;
}

static int doSetp(char *name, char *value, connectionRecType *context)
{
    char *nakStr = "SET SETP NAK";
    int retval;
    hal_param_t *param;
    hal_pin_t *pin;
    hal_type_t type;
    void *d_ptr;

    /* 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));
            sprintf(errorStr,
                "HAL:%d: ERROR: parameter or pin '%s' not found\n", linenumber, name);
            sockWriteError(nakStr, context);
            return HAL_INVAL;
        } else {
            /* found it */
            type = pin->type;
            if(pin->dir == HAL_OUT) {
                rtapi_mutex_give(&(hal_data->mutex));
                sprintf(errorStr,
                    "HAL:%d: ERROR: pin '%s' is not writable\n", linenumber, name);
                sockWriteError(nakStr, context);
                return HAL_INVAL;
            }
            if(pin->signal != 0) {
                rtapi_mutex_give(&(hal_data->mutex));
                sprintf(errorStr,
                    "HAL:%d: ERROR: pin '%s' is connected to a signal\n", linenumber, name);
                sockWriteError(nakStr, context);
                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));
            rtapi_print_msg(RTAPI_MSG_ERR,
                "HAL:%d: ERROR: param '%s' is not writable\n", linenumber, name);
            return HAL_INVAL;
        }
        d_ptr = SHMPTR(param->data_ptr);
    }

    retval = set_common(type, d_ptr, value, context);

    rtapi_mutex_give(&(hal_data->mutex));
    if (retval != 0) {
	sprintf(errorStr, "HAL:%d: setp failed\n", linenumber);
        sockWriteError(nakStr, context);
    }
    return retval;
}

static int doSets(char *name, char *value, connectionRecType *context)
{
    char *nakStr = "SET SETS NAK";
    int retval;
    hal_sig_t *sig;
    hal_type_t type;
    void *d_ptr;

    /* 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));
	sprintf(errorStr,
	    "HAL:%d: ERROR: signal '%s' not found\n", linenumber, name);
        sockWriteError(nakStr, context);
	return HAL_INVAL;
    }
    /* found it - does it have a writer? */
    if (sig->writers > 0) {
	rtapi_mutex_give(&(hal_data->mutex));
	sprintf(errorStr,
	    "HAL:%d: ERROR: signal '%s' already has writer(s)\n", linenumber, name);
        sockWriteError(nakStr, context);
	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, context);
    rtapi_mutex_give(&(hal_data->mutex));
    if (retval != 0) {
      sprintf(errorStr, "HAL:%d: sets failed\n", linenumber);
      sockWriteError(nakStr, context);
    }
    return retval;
}

static int doAddf(char *name, char *thread, char *parm, connectionRecType *context)
{
    int retval;
    char *nakStr = "SET ADDF NAK";
    
    if (parm[0] == '\0')
	    /* no - add function at end of thread */
      retval = hal_add_funct_to_thread(name, thread, -1);
    else
      retval = hal_add_funct_to_thread(name, thread, atoi(parm));
    if (retval != 0) {
      /* print fail message */
      sprintf(errorStr, "HAL:%d: Unable to add function '%s' to thread '%s'", linenumber, name, thread);
      sockWriteError(nakStr, context);
      }
    return retval;
}

static int doDelf(char *name, char *thread, connectionRecType *context)
{
    int retval;
    char *nakStr = "SET DELF NAK";
    
    retval = hal_del_funct_from_thread(name, thread);
    if (retval != 0) {
      /* print success message */
      sprintf(errorStr, "Failed to remove function '%s' from thread '%s'", name, thread);
      sockWriteError(nakStr, context);
    }
    return retval;
}

static int doStart(connectionRecType *context)
{
    int retval;
    char *nakStr = "SET START NAK";

    retval = hal_start_threads();
    if (retval != 0) {
	    /* print success message */
      sprintf(errorStr, "Failed to start realtime threads");
      sockWriteError(nakStr, context);
      }
    return retval;
}

static int doStop(connectionRecType *context)
{
    int retval;
    char *nakStr = "SET STOP NAK";

    retval = hal_stop_threads();
    if (retval != 0) {
	    /* print success message */
      sprintf(errorStr, "Unable to stop realtime threads");
      sockWriteError(nakStr, context);
      }
    return retval;
}

static int doLoadRt(char *mod_name, char *args[], connectionRecType *context)
{
    /* note: these are static so that the various searches can
       be skipped for subsequent commands */
    static char *rtmod_dir = EMC2_BIN_DIR "/rtapi_app";
    struct stat stat_buf;
    char mod_path[MAX_CMD_LEN+1];
    char *cp1;
    char *argv[MAX_TOK+1];
    char arg_string[MAX_CMD_LEN+1];
    int n, m, retval, status;
    hal_comp_t *comp;
    pid_t pid;
    char *nakStr = "SET LOADRT NAK";

    if (hal_get_lock()&HAL_LOCK_LOAD) {
      sprintf(errorStr,  "HAL:%d: ERROR: HAL is locked, loading of modules is not permitted", 
	linenumber);
      sockWriteError(nakStr, context);
      return HAL_PERM;
      }
    if ((strlen(rtmod_dir)+strlen(mod_name)+5) > MAX_CMD_LEN) {
      sprintf(errorStr, "HAL:%d: ERROR: Module path too long", linenumber);
      sockWriteError(nakStr, context);
      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 */
      sprintf(errorStr, "HAL:%d: ERROR: Can't find module '%s' in %s", 
	linenumber, mod_name, rtmod_dir);
      sockWriteError(nakStr, context);
      return -1;
      }
    /* now we need to fork, and then exec insmod.... */
    /* disconnect from the HAL shmem area before forking */
    hal_exit(comp_id);
    comp_id = 0;
    /* now the fork() */
    pid = fork();
    if (pid < 0) {
      sprintf(errorStr, "HAL:%d: ERROR: loadrt fork() failed", linenumber);
      sockWriteError(nakStr, context);
	/* reconnect to the HAL shmem area */
      comp_id = hal_init(comp_name);
      if (comp_id < 0) {
        sprintf(errorStr, "halrmt: hal_init() failed after fork: %d", comp_id);
        sockWriteError(nakStr, context);
        exit(-1);
	}
      hal_ready(comp_id);
      return -1;
      }
    if (pid == 0) {
	/* this is the child process - prepare to exec() insmod */
      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][0] != '\0') {
        argv[m++] = args[n++];
        }
      /* add a NULL to terminate the argv array */
      argv[m] = NULL;
      /* print debugging info if "very verbose" (-V) */
      rtapi_print_msg(RTAPI_MSG_DBG, "%s %s %s ", argv[0], argv[1], argv[2] );
      n = 3;
      while (argv[n] != NULL) {
        rtapi_print_msg(RTAPI_MSG_DBG, "%s ", argv[n++]);
        }
      rtapi_print_msg(RTAPI_MSG_DBG, "\n");
        /* call execv() to invoke insmod */
      execv(argv[0], argv);
      /* should never get here */
      sprintf(errorStr, "HAL:%d: ERROR: execv(%s) failed", linenumber, argv[0] );
      sockWriteError(nakStr, context);
      exit(1);
      }
    /* this is the parent process, wait for child to end */
    retval = waitpid (pid, &status, 0);
    /* reconnect to the HAL shmem area */
    comp_id = hal_init(comp_name);
    if (comp_id < 0) {
      sprintf(errorStr, "halcmd: hal_init() failed after loadrt: %d", comp_id);
      sockWriteError(nakStr, context);
      exit(-1);
      }
    hal_ready(comp_id);
    /* check result of waitpid() */
    if (retval < 0) {
      sprintf(errorStr, "HAL:%d: ERROR: waitpid(%d) failed", linenumber, pid);
      sockWriteError(nakStr, context);
      return -1;
      }
    if (WIFEXITED(status) == 0) {
      sprintf(errorStr, "HAL:%d: ERROR: child did not exit normally", linenumber);
      sockWriteError(nakStr, context);
      return -1;
      }
    retval = WEXITSTATUS(status);
    if (retval != 0) {
      sprintf(errorStr, "HAL:%d: ERROR: insmod failed, returned %d", linenumber, retval );
      sockWriteError(nakStr, context);
      return -1;
      }
    /* make the args that were passed to the module into a single string */
    n = 0;
    arg_string[0] = '\0';
    while ( 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) {
      sprintf(errorStr, "HAL:%d: ERROR: failed to allocate memory for module args", linenumber);
      sockWriteError(nakStr, context);
      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));
      sprintf(errorStr, "HAL:%d: ERROR: module '%s' not loaded", linenumber, mod_name);
      sockWriteError(nakStr, context);
      return HAL_INVAL;
      }
    /* link args to comp struct */
    comp->insmod_args = SHMOFF(cp1);
    rtapi_mutex_give(&(hal_data->mutex));
    /* print success message */
    return 0;
}

static int doDelsig(char *mod_name, connectionRecType *context)
{
    int next, retval, retval1, n;
    hal_sig_t *sig;
    char sigs[MAX_EXPECTED_SIGS][HAL_NAME_LEN+1];
    char *nakStr = "SET DELSIG NAK";

    /* check for "all" */
    if ( strcmp(mod_name, "all" ) != 0 ) {
      retval = hal_signal_delete(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 */
        sprintf(errorStr, "HAL:%d: ERROR: no signals found to be deleted", linenumber);
        sockWriteError(nakStr, context);
        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;
        n++;
	}
      }
    return retval1;
}

static int doUnloadRt(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' )) {