/* 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;
pktServerArgT *pktServerArg;
/*
* 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) {
fprintf(stderr, "\nError occurred getting packet header.\n");
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 QNX_OS
//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:
{
error = ProcessArmTriggerPkt(pktHdr.size,numSampTimes);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
}
case EXT_SELECT_SIGNALS:
{
error = ProcessSelectSignalsPkt(ei,numSampTimes,pktHdr.size);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
}
case EXT_SELECT_TRIGGER:
{
error = ProcessSelectTriggerPkt(ei,pktHdr.size);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
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:
{
PRINT_VERBOSE(("got EXT_SETPARAM packet.\n"));
error = ProcessSetParamPkt(ei, pktHdr.size);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
}
case EXT_GETPARAMS:
{
PRINT_VERBOSE(("got EXT_GETPARAMS packet.\n"));
error = ProcessGetParamsPkt(ei);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
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 QNX_OS
{
extern sem_t* startStopSem;
sem_post(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;
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;
break;
case EXT_CANCEL_LOGGING:
{
error = ProcessCancelLoggingPkt(pktHdr.size);
if (error != EXT_NO_ERROR) goto EXIT_POINT;
break;
}
default:
fprintf(stderr,"received invalid packet.\n");
break;
} /* end switch */
EXIT_POINT:
if (error != EXT_NO_ERROR) {
if (disconnectOnError) {
fprintf(stderr,
"Error occurred in rt_PktServerWork.\n"
"Disconnecting from host!\n");
/* 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 */
/* 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 QNX_OS
/*
* 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();
//printf("Waiting on uploadSem\n");
sem_wait(uploadSem);
//printf("Unblocked in uploadSem in UploadServerWork\n");
//taskSafe();
#endif
if (!connected) {
//printf("Exiting UploadServerwork because host not connected\n");
}
else {
//printf("In UploadServerWork, host connected\n");
}
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) {
fprintf(stderr,"SendPktDataToHost() failed on data upload.\n");
goto EXIT_POINT;
}
if (bufMem->nBytes2 > 0) {
error = SendPktDataToHost(
bufMem->section2,
bufMem->nBytes2);
if (error != EXT_NO_ERROR) {
fprintf(stderr,"SendPktDataToHost() failed on data upload.\n");
goto EXIT_POINT;
}
}
/* comfirm 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: 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 EXT_SVR_MUTEX_ENABLE
pthread_mutex_lock(&_sendPktLock);
#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) {
fprintf(stderr,"SendPktDataToHost() failed on data upload.\n");
goto EXIT_POINT;
}
if (bufMem->nBytes2 > 0) {
error = SendPktDataToHost(
bufMem->section2,
bufMem->nBytes2);
if (error != EXT_NO_ERROR) {
fprintf(stderr,"SendPktDataToHost() failed on data upload.\n");
goto EXIT_POINT;
}
}
/* comfirm that the data was sent */
UploadBufDataSent(upList.tids[i], upInfoIdx);
}
UploadBufGetData(&upList, upInfoIdx, numSampTimes);
}
EXIT_POINT:
#ifdef EXT_SVR_MUTEX_ENABLE
pthread_mutex_unlock(&_sendPktLock);
#endif
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);
}
}
