halrmt.c

CNC 的开放码,EMC2 V2.2.8版

/** This file, 'halrmt.c', is a HAL component that provides a simple
    telnet interface to the EMC2 HAL.  It is a user space component.
*/

/** Copyright (C) 2006 Eric H. Johnson 
    Derived from work by John Kasunich
                       <jmkasunich AT users DOT sourceforge DOT net>

    Other contributers:
                       Alex Joni
                       <alex_joni AT users DOT sourceforge DOT net>
 */

/** This program is free software; you can redistribute it and/or
    modify it under the terms of version 2.1 of the GNU General
    Public License as published by the Free Software Foundation.
    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111 USA

    THE AUTHORS OF THIS LIBRARY ACCEPT ABSOLUTELY NO LIABILITY FOR
    ANY HARM OR LOSS RESULTING FROM ITS USE.  IT IS _EXTREMELY_ UNWISE
    TO RELY ON SOFTWARE ALONE FOR SAFETY.  Any machinery capable of
    harming persons must have provisions for completely removing power
    from all motors, etc, before persons enter any danger area.  All
    machinery must be designed to comply with local and national safety
    codes, and the authors of this software can not, and do not, take
    any responsibility for such compliance.

    This code was written as part of the EMC HAL project.  For more
    information, go to www.linuxcnc.org.
*/

#ifndef ULAPI
#error This is a user mode component only!
#endif

#include "config.h"

#define _REENTRANT

#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <string.h>
#include <sys/stat.h>
#include <sys/wait.h>
#include <unistd.h>
#include <fcntl.h>
#include <signal.h>
#include <errno.h>
#include <time.h>
#include <string.h>
#include <math.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <sys/uio.h>
#include <pthread.h>

#include "rtapi.h"		/* RTAPI realtime OS API */
#include "hal.h"		/* HAL public API decls */
#include "../hal_priv.h"	/* private HAL decls */
/* non-EMC related uses of halrmt may want to avoid libnml dependency */
#ifndef NO_INI
#include "inifile.hh"		/* iniFind() from libnml */
#endif

/***********************************************************************
*                  LOCAL FUNCTION DECLARATIONS                         *
************************************************************************/

/* These functions are used internally by this file.  The code is at
   the end of the file.  */

#define MAX_TOK 20
#define MAX_CMD_LEN 1024
#define MAX_EXPECTED_SIGS 999

static int release_HAL_mutex(void);
static int do_help_cmd(char *command);
static int unloadrt_comp(char *mod_name);
static char *data_type(int type);
static char *pin_data_dir(int dir);
static char *param_data_dir(int dir);
static char *data_arrow1(int dir);
static char *data_arrow2(int dir);
static char *data_value(int type, void *valptr);
static char *data_value2(int type, void *valptr);
static void save_comps(FILE *dst);
static void save_signals(FILE *dst);
static void save_links(FILE *dst, int arrows);
static void save_nets(FILE *dst, int arrows);
static void save_params(FILE *dst);
static void save_threads(FILE *dst);
static void print_help_general(int showR);

/***********************************************************************
*                         GLOBAL VARIABLES                             *
************************************************************************/

int comp_id = -1;	/* -1 means hal_init() not called yet */
int hal_flag = 0;	/* used to indicate that halrmt might have the
			   hal mutex, so the sig handler can't just
			   exit, instead it must set 'done' */
int done = 0;		/* used to break out of processing loop */
int linenumber=0;	/* used to print linenumber on errors */
int scriptmode = 0;	/* used to make output "script friendly" (suppress headers) */
int prompt_mode = 0;	/* when getting input from stdin, print a prompt */
char comp_name[HAL_NAME_LEN];	/* name for this instance of halrmt */

typedef struct {  
  int cliSock;
  char hostName[80];
  char version[8];
  int linked;
  int echo;
  int verbose;
  int enabled;
  int commMode;
  int commProt;
  char inBuf[256];
  char outBuf[4096];
  char progName[256];} connectionRecType;


int halSocket = 5006;
char errorStr[256];

int server_sockfd, client_sockfd;
socklen_t server_len, client_len;
struct sockaddr_in server_address;
struct sockaddr_in client_address;
int useSockets = 1;
int tokenIdx;
char *delims = " \n\r\0";
int connCount = -1;
int enabledConn = -1;

typedef enum {
  cmdHello, cmdSet, cmdGet, cmdQuit, cmdShutdown, cmdHelp, cmdUnknown} commandTokenType;
  
typedef enum {
  hcEcho, hcVerbose, hcEnable, hcConfig, hcCommMode, hcCommProt,
  hcComps, hcPins, hcPinVals, hcSigs, hcSigVals, hcParams, hcParamVals, hcFuncts, hcThreads,
  hcComp, hcPin, hcPinVal, hcSig, hcSigVal, hcParam, hcParamVal, hcFunct, hcThread,
  hcLoadRt, hcUnloadRt, hcLoadUsr, hcLinkps, hcLinksp, hcLinkpp, hcUnlinkp,
  hcLock, hcUnlock, hcNewSig, hcDelSig, hcSetP, hcSetS, hcAddF, hcDelF,
  hcSave, hcStart, hcStop, hcUnknown
  } halCommandType;
  
typedef enum {
  rtNoError, rtHandledNoError, rtStandardError, rtCustomError, rtCustomHandledError
  } cmdResponseType;  

char *commands[] = {"HELLO", "SET", "GET", "QUIT", "SHUTDOWN", "HELP", ""};
char *halCommands[] = {
  "ECHO", "VERBOSE", "ENABLE", "CONFIG", "COMM_MODE", "COMM_PROT",
  "COMPS", "PINS", "PINVALS", "SIGNALS", "SIGVALS", "PARAMS", "PARAMVALS", "FUNCTS", "THREADS",
  "COMP", "PIN", "PINVAL", "SIGNAL", "SIGVAL", "PARAM", "PARAMVAL", "FUNCT", "THREAD",
  "LOADRT", "UNLOADRT", "LOADUSR", "LINKPS", "LINKSP", "LINKPP", "UNLINKP",
  "LOCK", "UNLOCK", "NEWSIG", "DELSIG", "SETP", "SETS", "ADDF", "DELF",
  "SAVE", "START", "STOP", ""};

#ifndef NO_INI
    FILE *inifile = NULL;
#endif


/* signal handler */
static void quit(int sig)
{
    if ( hal_flag ) {
	/* this process might have the hal mutex, so just set the
	   'done' flag and return, exit after mutex work finishes */
	done = 1;
    } else {
	/* don't have to worry about the mutex, but if we just
	   return, we might return into the fgets() and wait 
	   all day instead of exiting.  So we exit from here. */
	if ( comp_id > 0 ) {
	    hal_exit(comp_id);
	}
	_exit(1);
    }
}

static void strupr(char *s)
{  
  int i;
  
  for (i = 0; i < (int)strlen(s); i++)
    if (s[i] > 96 && s[i] <= 'z')
      s[i] -= 32;
}

static int initSockets()
{
  server_sockfd = socket(AF_INET, SOCK_STREAM, 0);
  server_address.sin_family = AF_INET;
  server_address.sin_addr.s_addr = htonl(INADDR_ANY);
  server_address.sin_port = htons(halSocket);
  server_len = sizeof(server_address);
  bind(server_sockfd, (struct sockaddr *)&server_address, server_len);
  listen(server_sockfd, 5);
  signal(SIGCHLD, SIG_IGN);
  return 0;
}

static int sockWrite(connectionRecType *context)
{
   strcat(context->outBuf, "\r\n");
   write(context->cliSock, context->outBuf, strlen(context->outBuf));
   return 0;
}

static void sockWriteError(char *nakStr, connectionRecType *context)
{
  if (context->verbose == 1)
    sprintf(context->outBuf, "%s %s", nakStr, errorStr);
  else
    sprintf(context->outBuf, "%s", nakStr);
  sockWrite(context);
}

/***********************************************************************
*                   LOCAL FUNCTION DEFINITIONS                         *
************************************************************************/

/* release_HAL_mutex() unconditionally releases the hal_mutex
   very useful after a program segfaults while holding the mutex
*/
static int release_HAL_mutex(void)
{
    int comp_id, mem_id, retval;
    void *mem;
    hal_data_t *hal_data;

    /* do RTAPI init */
    comp_id = rtapi_init("hal_unlocker");
    if (comp_id < 0) {
        rtapi_print_msg(RTAPI_MSG_ERR, "ERROR: rtapi init failed\n");
        return HAL_FAIL;
    }
    /* get HAL shared memory block from RTAPI */
    mem_id = rtapi_shmem_new(HAL_KEY, comp_id, HAL_SIZE);
    if (mem_id < 0) {
        rtapi_print_msg(RTAPI_MSG_ERR,
            "ERROR: could not open shared memory\n");
        rtapi_exit(comp_id);
        return HAL_FAIL;
    }
    /* get address of shared memory area */
    retval = rtapi_shmem_getptr(mem_id, &mem);
    if (retval != RTAPI_SUCCESS) {
        rtapi_print_msg(RTAPI_MSG_ERR,
            "ERROR: could not access shared memory\n");
        rtapi_exit(comp_id);
        return HAL_FAIL;
    }
    /* set up internal pointers to shared mem and data structure */
    hal_data = (hal_data_t *) mem;
    /* release mutex  */
    rtapi_mutex_give(&(hal_data->mutex));
    /* release RTAPI resources */
    rtapi_shmem_delete(mem_id, comp_id);
    rtapi_exit(comp_id);
    /* done */
    return HAL_SUCCESS;

}

static int doLock(char *command, connectionRecType *context)
{
    int retval=0;
    char *nakStr = "SET LOCK NAK";

    /* if command is blank, want to lock everything */
    if (*command == '\0')
      retval = hal_set_lock(HAL_LOCK_ALL);
    else 
      if (strcmp(command, "none") == 0)
        retval = hal_set_lock(HAL_LOCK_NONE);
      else 
        if (strcmp(command, "tune") == 0)
          retval = hal_set_lock(HAL_LOCK_LOAD & HAL_LOCK_CONFIG);
        else 
	  if (strcmp(command, "all") == 0)
            retval = hal_set_lock(HAL_LOCK_ALL);

    if (retval != 0) {
      sprintf(errorStr, "HAL:%d: Locking failed", linenumber);
      sockWriteError(nakStr, context);
      }
    return retval;
}

static int doUnlock(char *command, connectionRecType *context)
{
    int retval=0;
    char *nakStr = "SET UNLOCK NAK";

    /* if command is blank, want to lock everything */
    if (*command == '\0')
      retval = hal_set_lock(HAL_LOCK_NONE);
    else 
      if (strcmp(command, "all") == 0)
        retval = hal_set_lock(HAL_LOCK_NONE);
      else 
        if (strcmp(command, "tune") == 0)
          retval = hal_set_lock(HAL_LOCK_LOAD & HAL_LOCK_CONFIG);

    if (retval != 0) {
      sprintf(errorStr, "HAL:%d: Unlocking failed", linenumber);
      sockWriteError(nakStr, context);
      }
    return retval;
}

static int doLinkpp(char *first_pin_name, char *second_pin_name, connectionRecType *context)
{
    int retval;
    hal_pin_t *first_pin, *second_pin;
    char *nakStr = "SET LINKPP NAK";

    rtapi_mutex_get(&(hal_data->mutex));
    /* check if the pins are there */
    first_pin = halpr_find_pin_by_name(first_pin_name);
    second_pin = halpr_find_pin_by_name(second_pin_name);
    if (first_pin == 0) {
	/* first pin not found*/
      rtapi_mutex_give(&(hal_data->mutex));
      sprintf(errorStr, "HAL:%d: ERROR: pin '%s' not found\n", linenumber, first_pin_name);
      sockWriteError(nakStr, context);
      return HAL_INVAL; 
      } 
    else 
      if (second_pin == 0) {
	rtapi_mutex_give(&(hal_data->mutex));
        sprintf(errorStr, "HAL:%d: ERROR: pin '%s' not found", linenumber, second_pin_name);
        sockWriteError(nakStr, context);
	return HAL_INVAL; 
        }
    
    /* give the mutex, as the other functions use their own mutex */
    rtapi_mutex_give(&(hal_data->mutex));
    
    /* check that both pins have the same type, 
       don't want ot create a sig, which after that won't be usefull */
    if (first_pin->type != second_pin->type) {
      sprintf(errorStr, "HAL:%d: ERROR: pins '%s' and '%s' not of the same type", 
	  linenumber, first_pin_name, second_pin_name);
      sockWriteError(nakStr, context);
      return HAL_INVAL; 
    }
	
    /* now create the signal */
    retval = hal_signal_new(first_pin_name, first_pin->type);

    if (retval == HAL_SUCCESS) {
      /* if it worked, link the pins to it */
      retval = hal_link(first_pin_name, first_pin_name);

      if ( retval == HAL_SUCCESS )
      /* if that worked, link the second pin to the new signal */
        retval = hal_link(second_pin_name, first_pin_name);
      }
    if (retval != HAL_SUCCESS) {
      sprintf(errorStr, "HAL:%d: linkpp failed", linenumber);
      sockWriteError(nakStr, context);
      }
    return retval;
}

static int doLink(char *pin, char *sig, connectionRecType *context)
{
    int retval;
    char *nakStr = "SET LINKPS NAK";

    /* if sig is blank, want to unlink pin */
    if (*sig == '\0') {
	/* unlink the pin */
	retval = hal_unlink(pin);
    } else {
	/* make the link */
	retval = hal_link(pin, sig);
    }
    if (retval != 0) {
      sprintf(errorStr, "HAL:%d: link failed", linenumber);
      sockWriteError(nakStr, context);
      }
    return retval;
}

static int doNewsig(char *name, char *type, connectionRecType *context)
{
    int retval;
    char *nakStr = "SET NEWSIG NAK";

    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, "u16") == 0)
          retval = hal_signal_new(name, HAL_U32);
        else 
          if (strcasecmp(type, "s32") == 0)
            retval = hal_signal_new(name, HAL_S32);
          else {
            sprintf(errorStr, "HAL:%d: Unknown signal type '%s'", 
              linenumber, type);
            sockWriteError(nakStr, context);
            retval = HAL_INVAL;
            }
    if (retval != HAL_SUCCESS) {
      sprintf(errorStr, "HAL:%d: newsig failed", linenumber);
      sockWriteError(nakStr, context);
      }
    return retval;
}

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