/******************************************************************************
** Function: UTF_put_packet_header()
**
** Purpose:
** Display the CCSDS header of a packet.
*/
void UTF_put_packet_header (uint8 source, CCSDS_PriHdr_t *packet)
{
uint16 sid;
CFE_TIME_SysTime_t TimeFromMsg;
sid = CCSDS_RD_SID(*packet);
if (CCSDS_SID_TYPE(sid) == CCSDS_CMD)
{
if (CCSDS_SID_SHDR(sid) == CCSDS_HAS_SEC_HDR)
{
UTF_put_text("%s CMD: APID:%04x SEQ:%d LEN:%d FC:%d ",
(source == UTF_SOURCE_SEND) ? ">": "<",
CCSDS_RD_APID(*packet),
CCSDS_RD_SEQ(*packet),
CCSDS_RD_LEN(*packet),
CCSDS_RD_FC(((CCSDS_CmdPkt_t *)packet)->SecHdr));
}
else
{
UTF_put_text("%s CMD: APID:%04x SEQ:%d LEN:%d ",
(source == UTF_SOURCE_SEND) ? ">": "<",
CCSDS_RD_APID(*packet),
CCSDS_RD_SEQ(*packet),
CCSDS_RD_LEN(*packet));
}
}
else
{
if (CCSDS_SID_SHDR(sid) == CCSDS_HAS_SEC_HDR)
{
TimeFromMsg = CFE_SB_GetMsgTime((CFE_SB_MsgPtr_t)packet);
UTF_put_text("%s TLM: APID:%04x SEQ:%d LEN:%d TIME:%08x %08x ",
(source == UTF_SOURCE_SEND) ? ">": "<",
CCSDS_RD_APID(*packet),
CCSDS_RD_SEQ(*packet),
CCSDS_RD_LEN(*packet),
TimeFromMsg.Seconds,
TimeFromMsg.Subseconds);
}
else
{
UTF_put_text("%s TLM: APID:%04x SEQ:%d LEN:%d ",
(source == UTF_SOURCE_SEND) ? ">": "<",
CCSDS_RD_APID(*packet),
CCSDS_RD_SEQ(*packet),
CCSDS_RD_LEN(*packet));
}
}
}
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() */
