int32 Ut_CFE_SB_SendMsgHook(CFE_SB_Msg_t *MsgPtr)
{
UtList_Add(&MsgQueue, MsgPtr, CFE_SB_GetTotalMsgLength(MsgPtr), 0);
UtPrintf("PKT: ");
UtPrintx(MsgPtr, (uint16)(CFE_SB_GetTotalMsgLength(MsgPtr)));
return CFE_SUCCESS;
}
void Ut_CFE_SB_AddMsgToPipe(void *MsgPtr, CFE_SB_PipeId_t PipeId)
{
if (PipeTable[PipeId].InUse == TRUE) {
UtList_Add(&PipeTable[PipeId].MsgQueue, MsgPtr, CFE_SB_GetTotalMsgLength((CFE_SB_MsgPtr_t)MsgPtr), 0);
}
else {
printf("Error - Invalid PipeId\n");
}
}
void TO_ForwardTelemetry(uint32 PrioMask)
{
int32 cfeStatus;
uint32 msgSize;
uint32 Surplus = 0;
uint32 i=0;
for(i=0; i < TO_MAX_TLM_CLASS_QUEUES; i++)
{
TO_TlmClassQueue_t *clsQueue = &TO_AppData.Config->ClassQueue[i];
uint32 budget = clsQueue->Quantum + clsQueue->Deficit + Surplus;
while(1)
{
if(TO_AppData.Config->ClassQueue[i].PktPtr == 0)
{
cfeStatus = CFE_SB_RcvMsg(&TO_AppData.Config->ClassQueue[i].PktPtr, clsQueue->PipeId, CFE_SB_POLL);
if((cfeStatus != CFE_SUCCESS))
{
clsQueue->Deficit = 0;
Surplus = budget;
break;
}
}
if(i == 3)
{
OS_printf("Video telemetry out.");
}
msgSize = CFE_SB_GetTotalMsgLength(TO_AppData.Config->ClassQueue[i].PktPtr);
if(msgSize <= budget)
{
TO_SendToChannel(TO_AppData.Config->ClassQueue[i].ChannelIdx,
TO_AppData.Config->ClassQueue[i].PktPtr,
msgSize);
TO_AppData.Config->ClassQueue[i].PktPtr = 0;
budget -= msgSize;
}
else
{
clsQueue->DeferredCount++;
clsQueue->Deficit = budget - Surplus;
Surplus = budget;
break;
}
}
}
}
/******************************************************************************
** Function: CFE_SB_GetUserDataLength()
**
** Purpose:
** Get the length of the user data of a message (total size - hdrs).
**
** Arguments:
** MsgPtr - Pointer to a CFE_SB_Msg_t
**
** Return:
** Size of the message minus the headers
*/
uint16 CFE_SB_GetUserDataLength(CFE_SB_MsgPtr_t MsgPtr)
{
#ifdef MESSAGE_FORMAT_IS_CCSDS
uint16 TotalMsgSize;
uint16 HdrSize;
CFE_SB_MsgId_t MsgId;
MsgId = CCSDS_RD_SID(MsgPtr->Hdr);
TotalMsgSize = CFE_SB_GetTotalMsgLength(MsgPtr);
HdrSize = CFE_SB_MsgHdrSize(MsgId);
return (TotalMsgSize - HdrSize);
#endif
}/* end CFE_SB_GetUserDataLength */
boolean THRSIM_VerifyCmdLength(CFE_SB_MsgPtr_t Msg, uint16 ExpectedLength) {
boolean result = TRUE;
uint16 ActualLength = CFE_SB_GetTotalMsgLength(Msg);
if (ExpectedLength != ActualLength) {
CFE_SB_MsgId_t MessageId = CFE_SB_GetMsgId(Msg);
uint16 CommandCode = CFE_SB_GetCmdCode(Msg);
CFE_EVS_SendEvent(THRSIM_MSGLEN_ERR_EID, CFE_EVS_ERROR, "Invalid cmd length: MID = 0x%04X, CC = %02u, Exp Len = %u, Len = %u", MessageId, CommandCode, ExpectedLength, ActualLength);
result = FALSE;
THRSIM_AppData.HkPacket.error_count++;
}
return result;
}
uint16 Ut_CFE_SB_GetUserDataLengthHook(CFE_SB_MsgPtr_t MsgPtr)
{
#ifdef MESSAGE_FORMAT_IS_CCSDS
uint16 TotalMsgSize;
uint16 HdrSize;
CFE_SB_MsgId_t MsgId;
MsgId = (CFE_SB_MsgId_t)MsgPtr->Hdr.StreamId;
TotalMsgSize = CFE_SB_GetTotalMsgLength(MsgPtr);
HdrSize = CFE_SB_MsgHdrSize(MsgId);
return (TotalMsgSize - HdrSize);
#endif
}/* end Ut_CFE_SB_GetUserDataLengthHook */
void Ut_CFE_SB_SetUserDataLengthHook(CFE_SB_MsgPtr_t MsgPtr,uint16 DataLength)
{
#ifdef MESSAGE_FORMAT_IS_CCSDS
uint32 TotalMsgSize, HdrSize;
CFE_SB_MsgId_t MsgId;
MsgId = (CFE_SB_MsgId_t)MsgPtr->Hdr.StreamId;
TotalMsgSize = CFE_SB_GetTotalMsgLength(MsgPtr);
HdrSize = CFE_SB_MsgHdrSize(MsgId);
TotalMsgSize = HdrSize + DataLength;
CCSDS_WR_LEN(MsgPtr->Hdr,TotalMsgSize);
#endif
}/* end Ut_CFE_SB_SetUserDataLengthHook */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void TO_forward_telemetry(void)
{
static struct sockaddr_in s_addr;
int status;
int32 CFE_SB_status;
uint16 size;
CFE_SB_Msg_t *PktPtr;
bzero((char *) &s_addr, sizeof(s_addr));
s_addr.sin_family = AF_INET;
s_addr.sin_addr.s_addr = inet_addr(tlm_dest_IP);
s_addr.sin_port = htons(cfgTLM_PORT);
status = 0;
do
{
CFE_SB_status = CFE_SB_RcvMsg(&PktPtr, TO_Tlm_pipe, CFE_SB_POLL);
if ( (CFE_SB_status == CFE_SUCCESS) && (suppress_sendto == FALSE) )
{
size = CFE_SB_GetTotalMsgLength(PktPtr);
if(downlink_on == TRUE)
{
CFE_ES_PerfLogEntry(TO_SOCKET_SEND_PERF_ID);
status = sendto(TLMsockid, (char *)PktPtr, size, 0,
(struct sockaddr *) &s_addr,
sizeof(s_addr) );
CFE_ES_PerfLogExit(TO_SOCKET_SEND_PERF_ID);
}
if (status < 0)
{
CFE_EVS_SendEvent(TO_TLMOUTSTOP_ERR_EID,CFE_EVS_ERROR,
"L%d TO sendto errno %d. Tlm output supressed\n", __LINE__, errno);
suppress_sendto = TRUE;
}
}
/* If CFE_SB_status != CFE_SUCCESS, then no packet was received from CFE_SB_RcvMsg() */
}while(CFE_SB_status == CFE_SUCCESS);
} /* End of TO_forward_telemetry() */
boolean TT_VerifyCmdLength(CFE_SB_MsgPtr_t msg, uint16 ExpectedLength)
{
boolean result = TRUE;
uint16 ActualLength = CFE_SB_GetTotalMsgLength(msg);
/*
** Verify the command packet length...
*/
if (ExpectedLength != ActualLength)
{
CFE_SB_MsgId_t MessageID = CFE_SB_GetMsgId(msg);
uint16 CommandCode = CFE_SB_GetCmdCode(msg);
CFE_EVS_SendEvent(TT_LEN_ERR_EID, CFE_EVS_ERROR,
"TT: Invalid cmd pkt: ID = 0x%X, CC = %d, Len = %d",
MessageID, CommandCode, ActualLength);
result = FALSE;
TT4_AppData.ErrCount++;
}
return(result);
} /* End of TT_VerifyCmdLength() */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **/
boolean $APPLICATION_NAME$_VerifyCmdLength(CFE_SB_MsgPtr_t msg, uint16 ExpectedLength)
{
boolean result = TRUE;
uint16 ActualLength = CFE_SB_GetTotalMsgLength(msg);
/*
** Verify the command packet length.
*/
if (ExpectedLength != ActualLength)
{
CFE_SB_MsgId_t MessageID = CFE_SB_GetMsgId(msg);
uint16 CommandCode = CFE_SB_GetCmdCode(msg);
CFE_EVS_SendEvent($APPLICATION_NAME$_LEN_ERR_EID, CFE_EVS_ERROR,
"Invalid msg length: ID = 0x%X, CC = %d, Len = %d, Expected = %d",
MessageID, CommandCode, ActualLength, ExpectedLength);
result = FALSE;
$APPLICATION_NAME$_HkTelemetryPkt.$application_name$_command_error_count++;
}
return(result);
} /* End of $APPLICATION_NAME$_VerifyCmdLength() */
void DS_FileSetupWrite(int32 FileIndex, CFE_SB_MsgPtr_t MessagePtr)
{
DS_DestFileEntry_t *DestFile = &DS_AppData.DestFileTblPtr->File[FileIndex];
DS_AppFileStatus_t *FileStatus = &DS_AppData.FileStatus[FileIndex];
boolean OpenNewFile = FALSE;
uint16 PacketLength;
#if (DS_FILE_HEADER_TYPE == DS_FILE_HEADER_GPM)
/*
** Store packet time for GPM style file headers...
*/
CFE_TIME_SysTime_t PacketTime = CFE_SB_GetMsgTime(MessagePtr);
DS_AppData.CurrentPktTime = PacketTime.Seconds;
#endif
/*
** Create local pointers for array indexed data...
*/
PacketLength = CFE_SB_GetTotalMsgLength(MessagePtr);
if (FileStatus->FileHandle == DS_CLOSED_FILE_HANDLE)
{
/*
** 1st packet since destination enabled or file closed...
*/
OpenNewFile = TRUE;
}
else
{
/*
** Test size of existing destination file...
*/
if ((FileStatus->FileSize + PacketLength) > DestFile->MaxFileSize)
{
/*
** This packet would cause file to exceed max size limit...
*/
DS_FileUpdateHeader(FileIndex);
DS_FileCloseDest(FileIndex);
OpenNewFile = TRUE;
}
else
{
/*
** File size is OK - write packet data to file...
*/
DS_FileWriteData(FileIndex, MessagePtr, PacketLength);
}
}
if (OpenNewFile)
{
/*
** Either the file did not exist or we closed it because
** of the size limit test above...
*/
DS_FileCreateDest(FileIndex);
if (FileStatus->FileHandle != DS_CLOSED_FILE_HANDLE)
{
/*
** By writing the first packet without first performing a size
** limit test, we avoid issues resulting from having the max
** file size set less than the size of one packet...
*/
DS_FileWriteData(FileIndex, MessagePtr, PacketLength);
}
}
/*
** If the write did not occur due to I/O error (create or write)
** then current state = file closed and destination disabled...
*/
return;
} /* End of DS_FileSetupWrite() */
void CFE_TBL_TaskPipe(CFE_SB_Msg_t *MessagePtr)
{
CFE_SB_MsgId_t MessageID = CFE_SB_GetMsgId(MessagePtr);
uint16 CommandCode = CFE_SB_GetCmdCode(MessagePtr);
int16 CmdIndx;
uint32 ActualLength;
CFE_TBL_CmdProcRet_t CmdStatus = CFE_TBL_INC_ERR_CTR; /* Assume a failed command */
/* Search the Command Handler Table for a matching message */
CmdIndx = CFE_TBL_SearchCmdHndlrTbl(MessageID, CommandCode);
/* Check to see if a matching command was found */
if (CmdIndx >= 0)
{
/* Verify Message Length before processing */
ActualLength = CFE_SB_GetTotalMsgLength(MessagePtr);
if (ActualLength == CFE_TBL_CmdHandlerTbl[CmdIndx].ExpectedLength)
{
/* All checks have passed, call the appropriate message handler */
CmdStatus = (CFE_TBL_CmdHandlerTbl[CmdIndx].MsgProcFuncPtr)(MessagePtr);
}
else /* Bad Message Length */
{
CFE_EVS_SendEvent( CFE_TBL_LEN_ERR_EID, CFE_EVS_ERROR,
"Invalid msg length -- ID = 0x%04X, CC = %d, Len = %d (!= %d)",
MessageID, CommandCode, ActualLength,
CFE_TBL_CmdHandlerTbl[CmdIndx].ExpectedLength );
}
/* Only update command counters when message has a command code */
if (CFE_TBL_CmdHandlerTbl[CmdIndx].MsgTypes == CFE_TBL_CMD_MSGTYPE)
{
if (CmdStatus == CFE_TBL_INC_CMD_CTR)
{
CFE_TBL_TaskData.CmdCounter++;
}
else if (CmdStatus == CFE_TBL_INC_ERR_CTR)
{
CFE_TBL_TaskData.ErrCounter++;
}
}
}
else
{
/* Determine whether event message should be */
/* "Bad Command Code" or "Bad Message ID" */
if (CmdIndx == CFE_TBL_BAD_CMD_CODE)
{
CFE_EVS_SendEvent(CFE_TBL_CC1_ERR_EID, CFE_EVS_ERROR,
"Invalid command code -- ID = 0x%04X, CC = %d",
MessageID, CommandCode);
/* Update the command error counter */
CFE_TBL_TaskData.ErrCounter++;
}
else /* CmdIndx == CFE_TBL_BAD_MSG_ID */
{
CFE_EVS_SendEvent(CFE_TBL_MID_ERR_EID, CFE_EVS_ERROR,
"Invalid message ID -- ID = 0x%04X",
MessageID);
/*
** Note: we only increment the command error counter when
** processing messages with command codes
*/
}
}
return;
} /* End of CFE_TBL_TaskPipe() */
void CI_ReadUpLink(void)
{
socklen_t addr_len;
int i;
int status;
addr_len = sizeof(CI_SocketAddress);
bzero((char *) &CI_SocketAddress, sizeof(CI_SocketAddress));
for (i = 0; i <= 10; i++)
{
status = recvfrom(CI_SocketID, (char *)&CI_IngestBuffer[0], sizeof(CI_IngestBuffer), MSG_DONTWAIT,
(struct sockaddr *) &CI_SocketAddress, &addr_len);
if ( (status < 0) && (errno == EWOULDBLOCK) )
break; /* no (more) messages */
else
{
if (status <= CI_MAX_INGEST)
{
CCSDS_PriHdr_t *PrimaryHeader = NULL;
CFE_ES_PerfLogEntry(CI_SOCKET_RCV_PERF_ID);
CI_HkTelemetryPkt.IngestPackets++;
#ifdef SOFTWARE_LITTLE_BIT_ORDER
PrimaryHeader = (CCSDS_PriHdr_t*)CI_IngestPointer;
PrimaryHeader->StreamId = ntohs(PrimaryHeader->StreamId);
PrimaryHeader->Sequence = ntohs(PrimaryHeader->Sequence);
PrimaryHeader->Length = ntohs(PrimaryHeader->Length);
if (CCSDS_SID_TYPE(PrimaryHeader->StreamId) == CCSDS_CMD) {
CCSDS_CmdPkt_t *CmdPkt = (CCSDS_CmdPkt_t*)PrimaryHeader;
CmdPkt->SecHdr.Command = ntohs(CmdPkt->SecHdr.Command);
} else {
CCSDS_TlmPkt_t *TlmPkt = (CCSDS_TlmPkt_t*)PrimaryHeader;
uint32 *SecondsPtr = ((uint32 *)&TlmPkt->SecHdr.Time[0]);
uint16 *SubsecondsPtr = ((uint16 *)&TlmPkt->SecHdr.Time[4]);
*SecondsPtr = ntohs(*SecondsPtr);
*SubsecondsPtr = ntohs(*SubsecondsPtr);
}
#endif
#ifdef DO_DEBUG
{
/*LOGAN - print out hex command message exactly as it was sent */
int i;
unsigned char *mpp = (unsigned char*) &CI_IngestPointer[0];
for( i=0; i< CFE_SB_GetTotalMsgLength(CI_IngestPointer); i++ ) { printf("%02x ", mpp[i] ); }
printf("\n");
}
#endif
CFE_SB_SendMsg(CI_IngestPointer);
CFE_ES_PerfLogExit(CI_SOCKET_RCV_PERF_ID);
}
else
{
CI_HkTelemetryPkt.IngestErrors++;
CFE_EVS_SendEvent(CI_INGEST_ERR_EID,CFE_EVS_ERROR, "CI: L%d, cmd %0x %0x dropped, too long\n", __LINE__, *(long *)CI_IngestBuffer, *(long *)(CI_IngestBuffer+4) );
}
}
}
return;
} /* End of CI_ReadUpLink() */
