Example #1
0
File: wdLib.c Project: phoboz/vmx
STATUS wdDestroy(
    WDOG_ID wdId,
    BOOL dealloc
    )
{
    STATUS status;
    int level;

    /* Not callable from interrupts */
    if (INT_RESTRICT() != OK)
    {
        status = ERROR;
    }
    else
    {
        /* Lock interrupts */
        INT_LOCK(level);

        /* Verify object */
        if (OBJ_VERIFY(wdId, wdClassId) != OK )
        {
            INT_UNLOCK(level);
            status = ERROR;
        }
        else
        {
            /* Terminate object */
            objCoreTerminate(&wdId->objCore);

            /* Enter kernel */
            kernelState = TRUE;

            /* Unlock interrupts */
            INT_UNLOCK(level);

            /* Cancel watchdog timer */
            vmxWdCancel(wdId);
            wdId->status = WDOG_DEAD;

            taskSafe();

            /* Exit kernel */
            vmxExit();

            /* Deallocate if requested */
            if (dealloc == TRUE)
            {
                objFree(wdClassId, wdId);
            }

            taskUnsafe();
            status = OK;
        }
    }

    return status;
}
Example #2
0
File: semLib.c Project: phoboz/vmx
STATUS semDestroy(
    SEM_ID semId,
    BOOL   deallocate
    )
{
    STATUS status;
    int level;

    if (INT_RESTRICT() != OK)
    {
        errnoSet(S_intLib_NOT_ISR_CALLABLE);
        status = ERROR;
    }
    else
    {
        INT_LOCK(level);

        if (OBJ_VERIFY(semId, semClassId) != OK)
        {
            INT_UNLOCK(level);
            status = ERROR;
        }
        else
        {
            objCoreTerminate(&semId->objCore);

            /* Delete it */
            kernelState = TRUE;
            INT_UNLOCK(level);
            vmxSemDelete(semId);

            taskSafe();
            vmxExit();

            if (deallocate == TRUE)
            {
                objFree(semClassId, semId);
            }

            taskUnsafe();
            status = OK;
        }
    }

    return status;
}
Example #3
0
File: taskLib.c Project: phoboz/vmx
STATUS taskDestroy(
    int      taskId,
    BOOL     freeStack,
    unsigned timeout,
    BOOL     forceDestroy
    )
{
  STATUS status;
  int i, level;
  TCB_ID tcbId;

  if (INT_RESTRICT() != OK)
  {
    errnoSet (S_intLib_NOT_ISR_CALLABLE);
    return ERROR;
  }

  /* Get task context */
  tcbId = taskTcb(taskId);
  if (tcbId == NULL)
    return ERROR;

  /* If task self destruct and excption lib installed */
  if (tcbId == taskIdCurrent) {

    /* Wait for safe to destroy */
    while (tcbId->safeCount > 0)
      taskUnsafe();

    /* Kill it */
    status = excJobAdd(
        (VOIDFUNCPTR) taskDestroy,
        (ARG) tcbId,
        (ARG) freeStack,
        (ARG) WAIT_NONE,
        (ARG) FALSE,
        (ARG) 0,
        (ARG) 0
        );

    /* Block here and suspend */
    while(status == OK)
      taskSuspend(0);

  } /* End if task self destruct and exception lib installed */

taskDestroyLoop:

  /* Lock interrupts */
  INT_LOCK(level);

  /* Check id */
  if (TASK_ID_VERIFY(tcbId) != OK)
  {
    /* errno set by taskIdVerify() */

    /* Unlock interrupts */
    INT_UNLOCK(level);

    return ERROR;
  }

  /* Mask all signals */
  if (tcbId->pSignalInfo != NULL)
    tcbId->pSignalInfo->sigt_blocked = 0xffffffff;

  /* Block here for safe and running locked tasks */
  while ( (tcbId->safeCount > 0) ||
          ( (tcbId->status == TASK_READY) && (tcbId->lockCount > 0) )
        )
  {
    /* Enter kernel mode */
    kernelState = TRUE;

    /* Unlock interrupts */
    INT_UNLOCK(level);

    /* Check if force deletion, or suicide */
    if (forceDestroy || (tcbId == taskIdCurrent))
    {

      /* Remove protections */
      tcbId->safeCount = 0;
      tcbId->lockCount = 0;

      /* Check if flush of safety queue is needed */
      if (Q_FIRST(&tcbId->safetyQ) != NULL)
        vmxPendQFlush(&tcbId->safetyQ);

      /* Exit trough kernel */
      vmxExit();
    }
    else
    {
      /* Not forced deletion or suicide */

      /* Put task on safe queue */
      if (vmxPendQPut(&tcbId->safetyQ, timeout) != OK)
      {
        /* Exit trough kernel */
        vmxExit();

        errnoSet (S_taskLib_INVALID_TIMEOUT);

        return ERROR;
      }

      /* Exit trough kernel */
      status = vmxExit();

      /* Check for restart */
      if (status == SIG_RESTART)
      {
        timeout = (sigTimeoutRecalc)(timeout);
        goto taskDestroyLoop;
      }

      /* Check if unsuccessful */
      if (status == ERROR)
      {
          /* timer should have set errno to S_objLib_TIMEOUT */
          return ERROR;
      }

    } /* End else forced or suicide */

    /* Lock interrupts */
    INT_LOCK(level);

    /* Now verify class id again */
    if (TASK_ID_VERIFY(tcbId) != OK)
    {
      /* errno set by taskIdVerify() */

      /* Unlock interrupts */
      INT_UNLOCK(level);

      return ERROR;
    }

  } /* End while blocked by safety */

  /* Now only one cadidate is selected for deletion */

  /* Make myself safe */
  taskSafe();

  /* Protet deletion cadidate */
  tcbId->safeCount++;

   /* Check if not suicide */
   if (tcbId != taskIdCurrent)
   {
     /* Enter kernel mode */
     kernelState = TRUE;

     /* Unlock interrupts */
     INT_UNLOCK(level);

     /* Suspend victim */
     vmxSuspend(tcbId);

     /* Exit trough kernel */
     vmxExit();
  }
  else
  {
     /* Unlock interrupts */
     INT_UNLOCK(level);
  }

  /* Run deletion hooks */
  for (i = 0; i < MAX_TASK_DELETE_HOOKS; i++)
    if (taskDeleteHooks[i] != NULL)
      (*taskDeleteHooks[i])(tcbId);

  /* Lock task */
  taskLock();

  /* If dealloc and options dealloc stack */
  if ( freeStack && (tcbId->options & TASK_OPTIONS_DEALLOC_STACK) ) {

#if (_STACK_DIR == _STACK_GROWS_DOWN)

    objFree(taskClassId, tcbId->pStackEnd);

#else /* _STACK_GROWS_UP */

    objFree(taskClassId, tbcId - TASK_EXTRA_BYTES);

#endif /* _STACK_DIR */

  }

  /* Lock interrupts */
  INT_LOCK(level);

  /* Invalidate id */
  objCoreTerminate(&tcbId->objCore);

  /* Enter kernel mode */
  kernelState = TRUE;

  /* Unlock interrupts */
  INT_UNLOCK(level);

  /* Delete task */
  status = vmxDelete(tcbId);

  /* Check if safe quque needs to be flushed */
  if (Q_FIRST(&tcbId->safetyQ) != NULL)
    vmxPendQFlush(&tcbId->safetyQ);

  /* Exit trough kernel */
  vmxExit();

  /* Unprotect */
  taskUnlock();
  taskUnsafe();

  return OK;
}
Example #4
0
File: taskLib.c Project: phoboz/vmx
STATUS taskRestart(
    int taskId
    )
{
  TCB_ID tcbId;
  char *name, *rename;
  int len;
  unsigned priority;
  int options;
  char *pStackBase;
  unsigned stackSize;
  FUNCPTR entry;
  ARG args[MAX_TASK_ARGS];
  STATUS status;

  if (INT_RESTRICT() != OK)
  {
    errnoSet(S_intLib_NOT_ISR_CALLABLE);
    return ERROR;
  }

  /* If self restart */
  if ( (taskId == 0) || (taskId == (int) taskIdCurrent) )
  {

    /* Task must be unsafe */
    while (taskIdCurrent->safeCount > 0)
      taskSafe();

    /* Spawn a task that will restart this task */
    taskSpawn(restartTaskName, restartTaskPriority, restartTaskOptions,
              restartTaskStackSize, taskRestart, (ARG) taskIdCurrent,
              (ARG) 0,
              (ARG) 0,
              (ARG) 0,
              (ARG) 0,
              (ARG) 0,
              (ARG) 0,
              (ARG) 0,
              (ARG) 0,
              (ARG) 0);

    /* Wait for restart */
    while (1)
      taskSuspend(0);

  } /* End if self restart */

  /* Get task context */
  tcbId = taskTcb(taskId);
  if (tcbId == NULL)
    return ERROR;

  /* TASK_ID_VERIFY() already done by taskTcb() */

  /* Copy task data */
  priority = tcbId->priority;
  options = tcbId->options;
  entry = tcbId->entry;
  pStackBase = tcbId->pStackBase;
  stackSize = (tcbId->pStackEnd - tcbId->pStackBase) * _STACK_DIR;
  taskArgGet(tcbId, pStackBase, args);

  /* Copy name if needed */
  name = tcbId->name;
  rename = NULL;
  if (name != NULL) {
    len = strlen(name) + 1;
    rename = malloc(len);
    if (rename != NULL)
      strcpy(rename, name);
    name = rename;
  }

  /* Prevent deletion */
  taskSafe();

  if (taskTerminate((int) tcbId) != OK)
  {
    taskUnsafe();
    /* errno set by taskTerminate() */
    return ERROR;
  }

  /* Initialize task with same data */
  status = taskInit(tcbId, name, priority, options, pStackBase,
                    stackSize, entry, args[0], args[1], args[2],
                    args[3], args[4], args[5], args[6], args[7],
                    args[8], args[9]);
  if (status != OK)
  {
    /* errno set by taskInit() */
    return ERROR;
  }

  /* And start it */
  status = taskActivate((int) tcbId);
  if (status != OK)
  {
    /* errno set by taskActivate() */
    return ERROR;
  }

  /* Make me mortal */
  taskUnsafe();

  /* Free rename buffer if needed */
  if (rename != NULL)
    free(rename);

  return OK;
}
Example #5
0
/**
 * @brief Initialize the socket and serve images to the PC.
 * This is the task that serves images to the PC in a loop. This runs
 * as a separate task.
 */
int PCVideoServer::ServerTask()
{
	/* Setup to PC sockets */
	struct sockaddr_in serverAddr;
	int sockAddrSize = sizeof(serverAddr);
	int pcSock = ERROR;
	bzero ((char *) &serverAddr, sockAddrSize);
	serverAddr.sin_len = (u_char) sockAddrSize;
	serverAddr.sin_family = AF_INET;
	serverAddr.sin_port = htons (VIDEO_TO_PC_PORT);
	serverAddr.sin_addr.s_addr = htonl (INADDR_ANY);

	int success;
	while (true)
	{
		taskSafe();
		// Create the socket.
		if ((pcSock = socket (AF_INET, SOCK_STREAM, 0)) == ERROR)
		{
			wpi_setErrnoErrorWithContext("Failed to create the PCVideoServer socket");
			continue;
		}
		// Set the TCP socket so that it can be reused if it is in the wait state.
		int reuseAddr = 1;
		setsockopt(pcSock, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast<char*>(&reuseAddr), sizeof(reuseAddr));
		// Bind socket to local address.
		if (bind (pcSock, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR)
		{
			wpi_setErrnoErrorWithContext("Failed to bind the PCVideoServer port");
			close (pcSock);
			continue;
		}
		// Create queue for client connection requests.
		if (listen (pcSock, 1) == ERROR)
		{
			wpi_setErrnoErrorWithContext("Failed to listen on the PCVideoServer port");
			close (pcSock);
			continue;
		}

		struct sockaddr_in clientAddr;
		int clientAddrSize;
		int newPCSock = accept (pcSock, reinterpret_cast<sockaddr*>(&clientAddr), &clientAddrSize);
		if (newPCSock  == ERROR)
		{
			close(pcSock);
			continue;
		}
		//TODO: check camera error

		int numBytes = 0;
		int imageDataSize = 0;
		char* imageData = NULL;

		while(!m_stopServer)
		{
			success = semTake(m_newImageSem, 1000);
			if (success == ERROR)
			{
				// If the semTake timed out, there are no new images from the camera.
				continue;
			}
			success = AxisCamera::GetInstance().CopyJPEG(&imageData, numBytes, imageDataSize);
			if (!success)
			{
				// No point in running too fast -
				Wait(1.0);
				// If camera is not initialzed you will get failure and
				// the timestamp is invalid. Reset this value and try again.
				continue;
			}

			// Write header to PC
			static const char header[4]={1,0,0,0};
			int headerSend = write(newPCSock, const_cast<char*>(header), 4);

			// Write image length to PC
			int lengthSend = write(newPCSock, reinterpret_cast<char*>(&numBytes), 4);

			// Write image to PC
			int sent = write (newPCSock, imageData, numBytes);

			// The PC probably closed connection. Get out of here
			// and try listening again.
			if (headerSend == ERROR || lengthSend == ERROR || sent == ERROR)
			{
				break;
			}
		}
		// Clean up
		delete [] imageData;
		close (newPCSock);
		newPCSock = ERROR;
		close (pcSock);
		pcSock = ERROR;
		taskUnsafe();
		Wait(0.1);
	}
	return (OK);
}
Example #6
0
static int __wind_task_safe(struct task_struct *curr, struct pt_regs *regs)
{
	taskSafe();
	return 0;
}
/* Function: UploadServerWork =================================================
 * Abstract:
 *  Upload model signals to host for a single upInfo.
 */
void UploadServerWork(int32_T upInfoIdx, int_T numSampTimes)
{
    int_T         i;
    ExtBufMemList upList;
    boolean_T     error = EXT_NO_ERROR;

#ifdef VXWORKS
    /*
     * Don't spin the CPU unless we've got data to upload.
     * The upload.c/UploadBufAddTimePoint function gives the sem
     * each time that data is added.
     */
taskUnsafe();
    semTake(uploadSem, WAIT_FOREVER);
taskSafe();
#endif

    if (!connected) goto EXIT_POINT;
    
    UploadBufGetData(&upList, upInfoIdx, numSampTimes);
    while(upList.nActiveBufs > 0) {
        for (i=0; i<upList.nActiveBufs; i++) {
            const BufMem *bufMem = &upList.bufs[i];

            /*
             * We call SendPktDataToHost() instead of SendPktToHost() because
             * the packet header is combined with packet payload.  We do this
             * to avoid the overhead of making two calls for each upload
             * packet - one for the head and one for the payload.
             */
            error = SendPktDataToHost(
                bufMem->section1,
                bufMem->nBytes1);
            if (error != EXT_NO_ERROR) {
#ifndef EXTMODE_DISABLEPRINTF                    
                fprintf(stderr,"SendPktDataToHost() failed on data upload.\n");
#endif
                goto EXIT_POINT;
            }
            
            if (bufMem->nBytes2 > 0) {

                error = SendPktDataToHost(
                    bufMem->section2,
                    bufMem->nBytes2);
                if (error != EXT_NO_ERROR) {
#ifndef EXTMODE_DISABLEPRINTF                        
                    fprintf(stderr,"SendPktDataToHost() failed on data upload.\n");
#endif
                    goto EXIT_POINT;
                }
            }
            /* confirm that the data was sent */
            UploadBufDataSent(upList.tids[i], upInfoIdx);
        }
        UploadBufGetData(&upList, upInfoIdx, numSampTimes);
    }
    
EXIT_POINT:
    if (error != EXT_NO_ERROR) {
        /* An error in this function is caused by a physical failure in the
         * external mode connection.  We assume this failure caused the host
         * to disconnect.  The target must be disconnected and returned to a
         * state where it is running and can be re-connected to by the host.
         */
        ForceDisconnectFromHost(numSampTimes);
    }
}
/* Function: rt_PktServerWork ==================================================
 * Abstract:
 *  If not connected, establish communication of the packet line and the
 *  data upload line.  If connected, send/receive packets and parameters
 *  on the packet line.
 */
PUBLIC void rt_PktServerWork(RTWExtModeInfo *ei,
                             int_T          numSampTimes,
                             boolean_T      *stopReq)
{
    PktHeader  pktHdr;
    boolean_T  hdrAvail;
    boolean_T  error             = EXT_NO_ERROR;
    boolean_T  disconnectOnError = false;
    
    /*
     * If not connected, attempt to make connection to host.
     */
    if (!connected) {
        rtExtModeTestingKillIfOrphaned(false);

        error = ExtOpenConnection(extUD,&connected);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
    }

    /*
     * If ExtOpenConnection is not blocking and there are no pending
     * requests to open a connection, we'll still be unconnected.
     */
    if (!connected) goto EXIT_POINT; /* nothing to do */
    
    /*
     * Process packets.
     */

    /* Wait for a packet. */
    error = GetPktHdr(&pktHdr, &hdrAvail);
    if (error != EXT_NO_ERROR) {
#ifndef EXTMODE_DISABLEPRINTF            
        fprintf(stderr, "\nError occurred getting packet header.\n");
#endif
        disconnectOnError = true;
        goto EXIT_POINT;
    }
    rtExtModeTestingKillIfOrphaned(hdrAvail);
    
    if (!hdrAvail) goto EXIT_POINT; /* nothing to do */

    /*
     * This is the first packet.  Should contain the string:
     * 'ext-mode'.  Its contents are not important to us.
     * It is used as a flag to start the handshaking process.
     */
    if (!commInitialized) {
        pktHdr.type = EXT_CONNECT;
    }

    /* 
     * At this point we know that we have a packet: process it.
     */
#ifdef VXWORKS
    taskSafe();
#endif
    switch(pktHdr.type) {

    case EXT_GET_TIME:
    {
        /* Skip verbosity print out - we get too many of these */
        /*PRINT_VERBOSE(("got EXT_GET_TIME packet.\n"));*/
        time_T t = rteiGetT(ei);
        
        error = SendPktToHost(
            EXT_GET_TIME_RESPONSE,sizeof(time_T),
            (char_T *)&t);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
        break;
    }

    case EXT_ARM_TRIGGER:
    {
#ifndef EXTMODE_DISABLESIGNALMONITORING
        error = ProcessCancelLoggingArmTriggerPkt(EXT_ARM_TRIGGER_RESPONSE, pktHdr.size, numSampTimes);
	if (error != EXT_NO_ERROR) goto EXIT_POINT;
#else
        error = AcknowledgeSignalActionPkt(pktHdr.size, EXT_ARM_TRIGGER_RESPONSE);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
#endif
        break;
    }

    case EXT_SELECT_SIGNALS:
    {
#ifndef EXTMODE_DISABLESIGNALMONITORING
        error = ProcessSelectTriggerSignalPkt(EXT_SELECT_SIGNALS_RESPONSE, ei, pktHdr.size, numSampTimes, ERRMSG_PROCESSSELECTSIGNAL);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
#else
        error = AcknowledgeSignalActionPkt(pktHdr.size, EXT_SELECT_SIGNALS_RESPONSE);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
#endif
        break;
    }

    case EXT_SELECT_TRIGGER: 
    {
#ifndef EXTMODE_DISABLESIGNALMONITORING
        error = ProcessSelectTriggerSignalPkt(EXT_SELECT_TRIGGER_RESPONSE, ei, pktHdr.size, -1, ERRMSG_PROCESSSELECTTRIGGER);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
#else
        error = AcknowledgeSignalActionPkt(pktHdr.size, EXT_SELECT_TRIGGER_RESPONSE);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
#endif
        break;
    }

    case EXT_CONNECT:
    {
        PRINT_VERBOSE(("got EXT_CONNECT packet.\n"));
        error = ProcessConnectPkt(ei);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
        break;
    }

    case EXT_SETPARAM:
    {
#ifndef EXTMODE_DISABLEPARAMETERTUNING
        PRINT_VERBOSE(("got EXT_SETPARAM packet.\n"));
        error = ProcessSetParamPkt(ei, pktHdr.size);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
#else
        PRINT_VERBOSE(("discard EXT_SETPARAM packet.\n"));
        error = AcknowledgeSetParamPkt(pktHdr.size);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
#endif
        break;
    }

    case EXT_GETPARAMS:
    {
#ifndef EXTMODE_DISABLEPARAMETERTUNING
        PRINT_VERBOSE(("got EXT_GETPARAMS packet.\n"));
        error = ProcessGetParamsPkt(ei);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
#endif
        break;
    }

    case EXT_DISCONNECT_REQUEST:
    {
        PRINT_VERBOSE(("got EXT_DISCONNECT_REQUEST packet.\n"));
        
        /*
         * Note that from the target's point of view this is
         * more a "notify" than a "request".  The host needs to
         * have this acknowledged before it can begin closing
         * the connection.
         */
        error = SendPktToHost(EXT_DISCONNECT_REQUEST_RESPONSE, 0, NULL);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;

        DisconnectFromHost(numSampTimes);

        break;
    }

    case EXT_DISCONNECT_REQUEST_NO_FINAL_UPLOAD:
    {
        PRINT_VERBOSE(("got EXT_DISCONNECT_REQUEST_NO_FINAL_UPLOAD packet.\n"));
        
        /*
         * The target receives this packet when the host is
         * immediately terminating the extmode communication due
         * to some error.  The target should not send back a
         * response or a final upload of data because the host is
         * expecting neither.  The target must be disconnected and
         * returned to a state where it is running and can be
         * re-connected to by the host.
         */
        ForceDisconnectFromHost(numSampTimes);

        break;
    }

    case EXT_MODEL_START:
        PRINT_VERBOSE(("got EXT_MODEL_START packet.\n"));
#ifdef VXWORKS
        {
            extern SEM_ID startStopSem;
            semGive(startStopSem);
        }
#endif
        startModel = true;
        error = SendPktToHost(EXT_MODEL_START_RESPONSE, 0, NULL);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
        break;

    case EXT_MODEL_STOP:
        PRINT_VERBOSE(("got EXT_MODEL_STOP packet.\n"));
        *stopReq = true;
        break;
#ifndef EXTMODE_DISABLETESTING
    case EXT_MODEL_PAUSE:
        PRINT_VERBOSE(("got EXT_MODEL_PAUSE packet.\n"));
        modelStatus = TARGET_STATUS_PAUSED;
        startModel  = false;

        error = SendPktToHost(EXT_MODEL_PAUSE_RESPONSE, 0, NULL);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
        break;
    case EXT_MODEL_STEP:
        PRINT_VERBOSE(("got EXT_MODEL_STEP packet.\n"));
        if ((modelStatus == TARGET_STATUS_PAUSED) && !startModel) {
            startModel = true;
        }
        
        error = SendPktToHost(EXT_MODEL_STEP_RESPONSE, 0, NULL);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
        break;
    case EXT_MODEL_CONTINUE:
        PRINT_VERBOSE(("got EXT_MODEL_CONTINUE packet.\n"));
        if (modelStatus == TARGET_STATUS_PAUSED) {
            modelStatus = TARGET_STATUS_RUNNING;
            startModel  = false;
        }
        
        error = SendPktToHost(EXT_MODEL_CONTINUE_RESPONSE, 0, NULL);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
#endif
        break;

    case EXT_CANCEL_LOGGING:
    {
#ifndef EXTMODE_DISABLESIGNALMONITORING
        error = ProcessCancelLoggingArmTriggerPkt(EXT_CANCEL_LOGGING_RESPONSE, pktHdr.size, numSampTimes);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
#else
        error = AcknowledgeSignalActionPkt(pktHdr.size, EXT_CANCEL_LOGGING_RESPONSE);
        if (error != EXT_NO_ERROR) goto EXIT_POINT;
#endif
        break;
    }

    default:
#ifndef EXTMODE_DISABLEPRINTF            
        fprintf(stderr,"received invalid packet.\n");
#endif
        break;
    } /* end switch */

EXIT_POINT:
    if (error != EXT_NO_ERROR) {
        if (disconnectOnError) {
#ifndef EXTMODE_DISABLEPRINTF                
            fprintf(stderr,
                "Error occurred in rt_PktServerWork.\n"
                "Disconnecting from host!\n");
#endif
            /* An error in this function which causes disconnectOnError to be
             * set to true is caused by a physical failure in the external mode
             * connection.  We assume this failure caused the host to disconnect.
             * The target must be disconnected and returned to a state
             * where it is running and can be re-connected to by the host.
             */
            ForceDisconnectFromHost(numSampTimes);
        }
    }
#ifdef VXWORKS
    taskUnsafe();
#endif
} /* end rt_PktServerWork */
Example #9
0
	// Shamelessly stolen from PCVideoServer
	int DashboardCommandServer()
	{
	    /* Setup to PC sockets */
	    struct sockaddr_in serverAddr;
	    int sockAddrSize = sizeof(serverAddr);
	    int pcSock = ERROR;
	    bzero ((char *) &serverAddr, sockAddrSize);
	    serverAddr.sin_len = (u_char) sockAddrSize;
	    serverAddr.sin_family = AF_INET;
	    serverAddr.sin_port = htons (kDashboardCommandPort);
	    serverAddr.sin_addr.s_addr = htonl (INADDR_ANY);

	    while (true)
	    {
	        taskSafe();
	    	  //  Create the socket.
	        if ((pcSock = socket (AF_INET, SOCK_STREAM, 0)) == ERROR)
	        {
	            perror ("socket");
	            continue;
	        }
	        //  Set the TCP socket so that it can be reused if it is in the wait state.
	        int reuseAddr = 1;
	        setsockopt(pcSock, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast<char*>(&reuseAddr), sizeof(reuseAddr));
	        //  Bind socket to local address.
	        if (bind (pcSock, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR)
	        {
	            perror ("bind");
	            close (pcSock);
	            continue;
	        }
	        //  Create queue for client connection requests.
	        if (listen (pcSock, 1) == ERROR)
	        {
	            perror ("listen");
	            close (pcSock);
	            continue;
	        }

	        struct sockaddr_in clientAddr;
	        int clientAddrSize;
	        int newPCSock = accept (pcSock, reinterpret_cast<sockaddr*>(&clientAddr), &clientAddrSize);
	        if (newPCSock  == ERROR)
	        {
	            close(pcSock);
	            continue;
	        }

	        char cmdBuffer[32];
	        char *pBuffer;
	        while(1)
	        {
	            int numBytes = 0;
	            pBuffer = cmdBuffer;
	            while (numBytes < 2 || (*(pBuffer-2) != '\r' && *(pBuffer-1) != '\n'))
	            {
		            numBytes += read(newPCSock, pBuffer++, 1);
	            }
	            char command = cmdBuffer[0];
	            switch (command)
	            {
	            case 'E':
	            	speedJag.EnableControl();
	            	//printf("Enable\n");
	            	break;
	            case 'D':
	            	speedJag.DisableControl();
	            	//printf("Disable\n");
	            	break;
	            case 'G':
	            	{
	            		double P, I, D;
	            		memcpy((char*)&P, cmdBuffer+1, sizeof(double));
	            		memcpy((char*)&I, cmdBuffer+9, sizeof(double));
	            		memcpy((char*)&D, cmdBuffer+17, sizeof(double));
	            		speedJag.SetPID(P, I, D);
	            		//printf("Set- P: %f I: %f D: %f\n", P, I, D);
	            		//P = speedJag.GetP();
	            		//I = speedJag.GetI();
	            		//D = speedJag.GetD();
	            		//printf("Get- P: %f I: %f D: %f\n", P, I, D);
	            	}
	            	break;
	            }

	            //no point in running too fast -
	            Wait(0.01);
	        }
	        //  Clean up
	        close (newPCSock);
	        newPCSock = ERROR;
	        close (pcSock);
	        pcSock = ERROR;
	        taskUnsafe();
	        Wait(0.1);
	    }
	    return (OK);
	}