int32 TO_AppInit(void)
{
int32 iStatus;
uint16 i;
TO_AppData.RunStatus = CFE_ES_APP_RUN;
/* Register the TO app */
CFE_ES_RegisterApp();
/* Setup CMD Pipes */
TO_AppData.CmdPipeDepth = TO_CMD_PIPE_DEPTH ;
strcpy(TO_AppData.CmdPipeName, "TO_CMD_PIPE");
TO_AppData.SchPipeDepth = TO_SCH_PIPE_DEPTH;
strcpy(TO_AppData.SchPipeName, "TO_SCH_PIPE");
/* Initialize event filter table */
TO_AppData.EventFilters[0].EventID = TO_INIT_INF_EID;
TO_AppData.EventFilters[0].Mask = CFE_EVS_NO_FILTER;
TO_AppData.EventFilters[1].EventID = TO_CTODPIPE_ERR_EID;
TO_AppData.EventFilters[1].Mask = CFE_EVS_NO_FILTER;
TO_AppData.EventFilters[2].EventID = TO_SUBSCRIBE_ERR_EID;
TO_AppData.EventFilters[2].Mask = CFE_EVS_NO_FILTER;
TO_AppData.EventFilters[3].EventID = TO_TLMOUTSOCKET_ERR_EID;
TO_AppData.EventFilters[3].Mask = CFE_EVS_NO_FILTER;
TO_AppData.EventFilters[4].EventID = TO_TLMOUTSTOP_ERR_EID;
TO_AppData.EventFilters[4].Mask = CFE_EVS_NO_FILTER;
TO_AppData.EventFilters[5].EventID = TO_MSGID_ERR_EID;
TO_AppData.EventFilters[5].Mask = CFE_EVS_NO_FILTER;
TO_AppData.EventFilters[6].EventID = TO_FNCODE_ERR_EID;
TO_AppData.EventFilters[6].Mask = CFE_EVS_EVERY_OTHER_TWO;
TO_AppData.EventFilters[7].EventID = TO_NOOP_INF_EID;
TO_AppData.EventFilters[7].Mask = CFE_EVS_NO_FILTER;
/* Register event filter table */
iStatus = CFE_EVS_Register(TO_AppData.EventFilters,
TO_EVT_COUNT,
CFE_EVS_BINARY_FILTER);
if ( iStatus != CFE_SUCCESS )
{
CFE_ES_WriteToSysLog("TO App: Error Registering Events");
}
/* Initialize housekeeping packet (clear user data area) */
CFE_SB_InitMsg(&TO_AppData.HkPacket,
TO_HK_TLM_MID,
sizeof(TO_AppData.HkPacket), TRUE);
/* Subscribe for Commands on the Command Pipe */
iStatus = CFE_SB_CreatePipe(&TO_AppData.CmdPipe,
TO_AppData.CmdPipeDepth,
TO_AppData.CmdPipeName);
if (iStatus == CFE_SUCCESS)
{
CFE_SB_Subscribe(TO_CMD_MID, TO_AppData.CmdPipe);
}
else
{
CFE_EVS_SendEvent(TO_CTODPIPE_ERR_EID,CFE_EVS_ERROR,
"TO: Can't create cmd pipe status %i",iStatus);
}
/* Subscribe for the Wakeup and Send_HK messages on the SCH Pipe*/
iStatus = CFE_SB_CreatePipe(&TO_AppData.SchPipe,
TO_AppData.SchPipeDepth,
TO_AppData.SchPipeName);
if (iStatus == CFE_SUCCESS)
{
CFE_SB_Subscribe(TO_SEND_HK_MID, TO_AppData.SchPipe);
CFE_SB_Subscribe(TO_SEND_TLM_MID, TO_AppData.SchPipe);
}
else
{
CFE_EVS_SendEvent(TO_CTODPIPE_ERR_EID,CFE_EVS_ERROR,
"TO: Can't create cmd pipe status %i",iStatus);
}
/* Install the delete handler */
OS_TaskInstallDeleteHandler((void *)(&TO_Cleanup));
/* Register and Load iLoad Table */
iStatus = TO_TableInit();
if(iStatus != CFE_SUCCESS)
{
goto end_of_function;
}
/* Setup TLM Pipes */
for (i=0; i < TO_MAX_TLM_CLASS_QUEUES; i++)
{
if(TO_AppData.Config->ClassQueue[i].Mode == TO_QUEUE_ENABLED)
{
char pipeName[OS_MAX_API_NAME];
sprintf(pipeName, "TO_CLSQUEUE_%u", i);
iStatus = CFE_SB_CreatePipe(&TO_AppData.Config->ClassQueue[i].PipeId,
TO_AppData.Config->ClassQueue[i].Size, pipeName);
if (iStatus != CFE_SUCCESS)
{
CFE_EVS_SendEvent(TO_TLMPIPE_ERR_EID,CFE_EVS_ERROR,
"TO: Can't create Tlm pipe status %i",iStatus);
}
}
}
/* Subscriptions for TLM pipe*/
for (i=0; i < TO_AppData.Config->MaxSubscriptions; i++)
{
if(TO_AppData.Config->Subscription[i].MsgId != TO_UNUSED )
{
CFE_SB_Qos_t qos = {0,0};
uint32 clsQueueIdx = TO_AppData.Config->Subscription[i].ClassQueueIdx;
iStatus = CFE_SB_SubscribeEx(TO_AppData.Config->Subscription[i].MsgId,
TO_AppData.Config->ClassQueue[clsQueueIdx].PipeId,
qos,
TO_AppData.Config->Subscription[i].MsgLimit);
if (iStatus != CFE_SUCCESS)
{
CFE_EVS_SendEvent(TO_SUBSCRIBE_ERR_EID,CFE_EVS_ERROR,
"TO: Can't subscribe to MID 0x%x status 0x%08X index %u",
TO_AppData.Config->Subscription[i].MsgId, iStatus, i);
}
else
{
TO_AppData.HkPacket.PktSubCount++;
}
}
}
for (i=0; i < TO_MAX_TLM_CHANNELS; i++)
{
if(TO_AppData.Config->Channel[i].Mode == TO_CHANNEL_ENABLED)
{
struct sockaddr_in servaddr;
int status;
int reuseaddr=1;
if((TO_AppData.Config->Channel[i].Socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0)
{
TO_AppData.Config->Channel[i].Mode = TO_CHANNEL_DISABLED;
CFE_EVS_SendEvent(TO_TLMOUTSOCKET_ERR_EID,CFE_EVS_ERROR,
"TO: TLM socket errno: %i on channel %u", errno, i);
continue;
}
setsockopt(TO_AppData.Config->Channel[i].Socket, SOL_SOCKET, SO_REUSEADDR, &reuseaddr,sizeof(reuseaddr));
bzero(&servaddr,sizeof(servaddr));
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
servaddr.sin_port=0;
status = bind(TO_AppData.Config->Channel[i].Socket, (struct sockaddr *)&servaddr,sizeof(servaddr));
if(status < 0)
{
CFE_EVS_SendEvent(TO_TLMOUTSOCKET_ERR_EID, CFE_EVS_ERROR,
"TO: TLM bind errno: %i on channel %u", errno, i);
TO_AppData.Config->Channel[i].Mode = TO_CHANNEL_DISABLED;
continue;
}
CFE_EVS_SendEvent(TO_TLMOUTENA_INF_EID, CFE_EVS_INFORMATION,
"TO UDP telemetry output enabled channel %u to %s:%u",
i, TO_AppData.Config->Channel[i].IP, TO_AppData.Config->Channel[i].DstPort);
}
}
TO_AppData.DropFileData = FALSE;
TO_AppData.DropEOF = FALSE;
TO_AppData.DropFIN = FALSE;
TO_AppData.DropACK = FALSE;
TO_AppData.DropMetaData = FALSE;
TO_AppData.DropNAK = FALSE;
TO_AppData.CorruptChecksum = FALSE;
TO_AppData.AdjustFileSize = FALSE;
CFE_EVS_SendEvent (TO_INIT_INF_EID, CFE_EVS_INFORMATION,
"TO Initialized. Version %d.%d.%d.%d Awaiting enable command.",
TO_MAJOR_VERSION,
TO_MINOR_VERSION,
TO_REVISION,
TO_MISSION_REV);
end_of_function:
return iStatus;
}
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
int32 CFE_ES_ShellOutputCommand(char * CmdString, char *Filename)
{
int32 Result;
int32 ReturnCode = CFE_SUCCESS;
int32 fd;
int32 FileSize;
int32 CurrFilePtr;
uint32 i;
/* the extra 1 added for the \0 char */
char CheckCmd [CFE_ES_CHECKSIZE + 1];
char Cmd [CFE_ES_MAX_SHELL_CMD];
char OutputFilename [OS_MAX_PATH_LEN];
/* Use default filename if not provided */
if (Filename[0] == '\0')
{
strncpy(OutputFilename, CFE_ES_DEFAULT_SHELL_FILENAME, OS_MAX_PATH_LEN);
}
else
{
strncpy(OutputFilename, Filename, OS_MAX_PATH_LEN);
}
/* Make sure string is null terminated */
OutputFilename[OS_MAX_PATH_LEN - 1] = '\0';
/* Remove previous version of output file */
OS_remove(OutputFilename);
fd = OS_creat(OutputFilename, OS_READ_WRITE);
if (fd < OS_FS_SUCCESS)
{
Result = OS_FS_ERROR;
}
else
{
strncpy(CheckCmd,CmdString,CFE_ES_CHECKSIZE);
CheckCmd[CFE_ES_CHECKSIZE] = '\0';
strncpy(Cmd,CmdString, CFE_ES_MAX_SHELL_CMD);
/* We need to check if this command is directed at ES, or at the
operating system */
if (strncmp(CheckCmd,"ES_",CFE_ES_CHECKSIZE) == 0)
{
/* This list can be expanded to include other ES functionality */
if ( strncmp(Cmd,CFE_ES_LIST_APPS_CMD,strlen(CFE_ES_LIST_APPS_CMD) )== 0)
{
Result = CFE_ES_ListApplications(fd);
}
else if ( strncmp(Cmd,CFE_ES_LIST_TASKS_CMD,strlen(CFE_ES_LIST_TASKS_CMD) )== 0)
{
Result = CFE_ES_ListTasks(fd);
}
else if ( strncmp(Cmd,CFE_ES_LIST_RESOURCES_CMD,strlen(CFE_ES_LIST_RESOURCES_CMD) )== 0)
{
Result = CFE_ES_ListResources(fd);
}
/* default if there is not an ES command that matches */
else
{
Result = CFE_ES_ERR_SHELL_CMD;
CFE_ES_WriteToSysLog("There is no ES Shell command that matches %s \n",Cmd);
}
}
/* if the command is not directed at ES, pass it through to the
* underlying OS */
else
{
Result = OS_ShellOutputToFile(Cmd,fd);
}
/* seek to the end of the file to get it's size */
FileSize = OS_lseek(fd,0,OS_SEEK_END);
if (FileSize == OS_FS_ERROR)
{
OS_close(fd);
CFE_ES_WriteToSysLog("OS_lseek call failed from CFE_ES_ShellOutputCmd 1\n");
Result = OS_FS_ERROR;
}
/* We want to add 3 characters at the end of the telemetry,'\n','$','\0'.
* To do this we need to make sure there are at least 3 empty characters at
* the end of the last CFE_ES_MAX_SHELL_PKT so we don't over write any data. If
* the current file has only 0,1, or 2 free spaces at the end, we want to
* make the file longer to start a new tlm packet of size CFE_ES_MAX_SHELL_PKT.
* This way we will get a 'blank' packet with the correct 3 characters at the end.
*/
else
{
/* if we are within 2 bytes of the end of the packet*/
if ( FileSize % CFE_ES_MAX_SHELL_PKT > (CFE_ES_MAX_SHELL_PKT - 3))
{
/* add enough bytes to start a new packet */
for (i = 0; i < CFE_ES_MAX_SHELL_PKT - (FileSize % CFE_ES_MAX_SHELL_PKT) + 1 ; i++)
{
OS_write(fd," ",1);
}
}
else
{
/* we are exactly at the end */
if( FileSize % CFE_ES_MAX_SHELL_PKT == 0)
{
OS_write(fd," ",1);
}
}
/* seek to the end of the file again to get it's new size */
FileSize = OS_lseek(fd,0,OS_SEEK_END);
if (FileSize == OS_FS_ERROR)
{
OS_close(fd);
CFE_ES_WriteToSysLog("OS_lseek call failed from CFE_ES_ShellOutputCmd 2\n");
Result = OS_FS_ERROR;
}
else
{
/* set the file back to the beginning */
OS_lseek(fd,0,OS_SEEK_SET);
/* start processing the chunks. We want to have one packet left so we are sure this for loop
* won't run over */
for (CurrFilePtr=0; CurrFilePtr < (FileSize - CFE_ES_MAX_SHELL_PKT); CurrFilePtr += CFE_ES_MAX_SHELL_PKT)
{
OS_read(fd, CFE_ES_TaskData.ShellPacket.ShellOutput, CFE_ES_MAX_SHELL_PKT);
/* Send the packet */
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &CFE_ES_TaskData.ShellPacket);
CFE_SB_SendMsg((CFE_SB_Msg_t *) &CFE_ES_TaskData.ShellPacket);
/* delay to not flood the pipe on large messages */
OS_TaskDelay(200);
}
/* finish off the last portion of the file */
/* over write the last packet with spaces, then it will get filled
* in with the correct info below. This assures that the last non full
* part of the packet will be spaces */
for (i =0; i < CFE_ES_MAX_SHELL_PKT; i++)
{
CFE_ES_TaskData.ShellPacket.ShellOutput[i] = ' ';
}
OS_read(fd, CFE_ES_TaskData.ShellPacket.ShellOutput, ( FileSize - CurrFilePtr));
/* From our check above, we are assured that there are at least 3 free
* characters to write our data into at the end of this last packet
*
* The \n assures we are on a new line, the $ gives us our prompt, and the
* \0 assures we are null terminalted.
*/
CFE_ES_TaskData.ShellPacket.ShellOutput[ CFE_ES_MAX_SHELL_PKT - 3] = '\n';
CFE_ES_TaskData.ShellPacket.ShellOutput[ CFE_ES_MAX_SHELL_PKT - 2] = '$';
CFE_ES_TaskData.ShellPacket.ShellOutput[ CFE_ES_MAX_SHELL_PKT - 1] = '\0';
/* Send the last packet */
CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &CFE_ES_TaskData.ShellPacket);
CFE_SB_SendMsg((CFE_SB_Msg_t *) &CFE_ES_TaskData.ShellPacket);
/* Close the file descriptor */
OS_close(fd);
} /* if FilseSize == OS_FS_ERROR */
} /* if FileSeize == OS_FS_ERROR */
}/* if fd < OS_FS_SUCCESS */
if (Result != OS_SUCCESS && Result != CFE_SUCCESS )
{
ReturnCode = CFE_ES_ERR_SHELL_CMD;
CFE_ES_WriteToSysLog("OS_ShellOutputToFile call failed from CFE_ES_ShellOutputCommand\n");
}
else
{
ReturnCode = CFE_SUCCESS;
}
return ReturnCode;
}
int32 THRSIM_Init(void) {
int32 Status = CFE_SUCCESS;
CFE_ES_RegisterApp();
/* Initialize app data */
THRSIM_AppData.CmdPipe = 0;
THRSIM_AppData.MsgPtr = NULL;
/* Set up and register events */
THRSIM_AppData.EventFilters[0].EventID = THRSIM_INIT_INF_EID;
THRSIM_AppData.EventFilters[0].Mask = CFE_EVS_NO_FILTER;
THRSIM_AppData.EventFilters[1].EventID = THRSIM_NOOP_INF_EID;
THRSIM_AppData.EventFilters[1].Mask = CFE_EVS_NO_FILTER;
THRSIM_AppData.EventFilters[2].EventID = THRSIM_CRCMDPIPE_ERR_EID;
THRSIM_AppData.EventFilters[2].Mask = CFE_EVS_NO_FILTER;
THRSIM_AppData.EventFilters[3].EventID = THRSIM_SUBSCRIBE_ERR_EID;
THRSIM_AppData.EventFilters[3].Mask = CFE_EVS_NO_FILTER;
THRSIM_AppData.EventFilters[4].EventID = THRSIM_MSGID_ERR_EID;
THRSIM_AppData.EventFilters[4].Mask = CFE_EVS_NO_FILTER;
THRSIM_AppData.EventFilters[5].EventID = THRSIM_CC1_ERR_EID;
THRSIM_AppData.EventFilters[5].Mask = CFE_EVS_NO_FILTER;
THRSIM_AppData.EventFilters[6].EventID = THRSIM_MSGLEN_ERR_EID;
THRSIM_AppData.EventFilters[6].Mask = CFE_EVS_NO_FILTER;
THRSIM_AppData.EventFilters[7].EventID = THRSIM_PIPE_ERR_EID;
THRSIM_AppData.EventFilters[7].Mask = CFE_EVS_NO_FILTER;
Status = CFE_EVS_Register(THRSIM_AppData.EventFilters, THRSIM_FILTERED_EVT_COUNT, CFE_EVS_BINARY_FILTER);
if (Status != CFE_SUCCESS) {
CFE_ES_WriteToSysLog("Error registering events, RC = 0x%08X\n", Status);
return Status;
}
/* Initialize housekeeping */
CFE_SB_InitMsg(&THRSIM_AppData.HkPacket, THRSIM_HK_TLM_MID, sizeof(THRSIM_HkPacket_t), TRUE);
THRSIM_ResetCounters();
/* Create a command pipe and subscribe to its messages */
Status = CFE_SB_CreatePipe(&THRSIM_AppData.CmdPipe, THRSIM_PIPE_DEPTH, "THRSIM_CMD_PIPE");
if (Status != CFE_SUCCESS) {
CFE_EVS_SendEvent(THRSIM_CRCMDPIPE_ERR_EID, CFE_EVS_ERROR, "Can't create command pipe, RC=0x%08X", Status);
return Status;
}
Status = CFE_SB_Subscribe(THRSIM_CMD_MID, THRSIM_AppData.CmdPipe);
if (Status != CFE_SUCCESS) {
CFE_EVS_SendEvent(THRSIM_SUBSCRIBE_ERR_EID, CFE_EVS_ERROR, "Error subscribing to THRSIM_CMD_MID(0x%04X), RC=0x%08X", THRSIM_CMD_MID, Status);
return Status;
}
Status = CFE_SB_Subscribe(THRSIM_SEND_HK_MID, THRSIM_AppData.CmdPipe);
if (Status != CFE_SUCCESS) {
CFE_EVS_SendEvent(THRSIM_SUBSCRIBE_ERR_EID, CFE_EVS_ERROR, "Error subscribing to THRSIM_SEND_HK_MID(0x%04X), RC=0x%08X", THRSIM_SEND_HK_MID, Status);
return Status;
}
CFE_SB_Subscribe(THRSIM_TICK_MID, THRSIM_AppData.CmdPipe);
if (Status != CFE_SUCCESS) {
CFE_EVS_SendEvent(THRSIM_SUBSCRIBE_ERR_EID, CFE_EVS_ERROR, "Error subscribing to tick message (0x%04X), RC=0x%08X", THRSIM_TICK_MID, Status);
return Status;
}
/* Initialize the output message */
CFE_SB_InitMsg(&THRSIM_AppData.thr_sim_msg, THR_SIM_MID, sizeof(thr_sim_msg_t), TRUE);
Status = THRSIM_HWIF_Init();
if (Status != CFE_SUCCESS) {
CFE_EVS_SendEvent(THRSIM_HWIF_ERR_EID, CFE_EVS_ERROR, "Error initializing HWIF, RC=0x%08X", Status);
return Status;
}
CFE_EVS_SendEvent(THRSIM_INIT_INF_EID, CFE_EVS_INFORMATION, "THRSIM app initialized.");
return CFE_SUCCESS;
}
int32 SC_AppInit(void)
{
int32 Result;
/* Clear global data structures */
CFE_PSP_MemSet(&SC_OperData, 0, sizeof(SC_OperData_t));
CFE_PSP_MemSet(&SC_AppData, 0, sizeof(SC_AppData_t));
/* Number of ATS and RTS commands already executed this second */
SC_OperData.NumCmdsSec = 0;
/* Continue ATS execution if ATS command checksum fails */
SC_AppData.ContinueAtsOnFailureFlag = SC_CONT_ON_FAILURE_START;
/* Make sure nothing is running */
SC_AppData.NextProcNumber = SC_NONE;
SC_AppData.NextCmdTime[SC_ATP] = SC_MAX_TIME;
SC_AppData.NextCmdTime[SC_RTP] = SC_MAX_TIME;
/* Initialize the SC housekeeping packet */
CFE_SB_InitMsg(&SC_OperData.HkPacket, SC_HK_TLM_MID, sizeof(SC_HkTlm_t), FALSE);
/* Select auto-exec RTS to start during first HK request */
if (CFE_ES_GetResetType(NULL) == CFE_ES_POWERON_RESET)
{
SC_AppData.AutoStartRTS = RTS_ID_AUTO_POWER_ON;
}
else
{
SC_AppData.AutoStartRTS = RTS_ID_AUTO_PROCESSOR;
}
/* Must be able to register for events */
Result = CFE_EVS_Register(NULL,0,CFE_EVS_NO_FILTER);
if (Result != CFE_SUCCESS)
{
CFE_ES_WriteToSysLog("Event Services Register returned: 0x%08X\n", Result);
return(Result);
}
/* Must be able to create Software Bus message pipe */
Result = CFE_SB_CreatePipe(&SC_OperData.CmdPipe, SC_PIPE_DEPTH, SC_CMD_PIPE_NAME);
if (Result != CFE_SUCCESS)
{
CFE_EVS_SendEvent(SC_INIT_SB_CREATE_ERR_EID, CFE_EVS_ERROR,
"Software Bus Create Pipe returned: 0x%08X", Result);
return(Result);
}
/* Must be able to subscribe to HK request command */
Result = CFE_SB_Subscribe(SC_SEND_HK_MID, SC_OperData.CmdPipe);
if (Result != CFE_SUCCESS)
{
CFE_EVS_SendEvent(SC_INIT_SB_SUBSCRIBE_HK_ERR_EID, CFE_EVS_ERROR,
"Software Bus subscribe to housekeeping returned: 0x%08X", Result);
return(Result);
}
/* Must be able to subscribe to 1Hz wakeup command */
Result = CFE_SB_Subscribe(SC_1HZ_WAKEUP_MID, SC_OperData.CmdPipe);
if (Result != CFE_SUCCESS)
{
CFE_EVS_SendEvent(SC_INIT_SB_SUBSCRIBE_1HZ_ERR_EID, CFE_EVS_ERROR,
"Software Bus subscribe to 1 Hz cycle returned: 0x%08X", Result);
return(Result);
}
/* Must be able to subscribe to SC commands */
Result = CFE_SB_Subscribe(SC_CMD_MID, SC_OperData.CmdPipe);
if (Result != CFE_SUCCESS)
{
CFE_EVS_SendEvent(SC_INIT_SB_SUBSCRIBE_CMD_ERR_EID, CFE_EVS_ERROR,
"Software Bus subscribe to command returned: 0x%08X", Result);
return(Result);
}
/* Must be able to create and initialize tables */
Result = SC_InitTables();
if (Result != CFE_SUCCESS)
{
return(Result);
}
/* Send application startup event */
CFE_EVS_SendEvent(SC_INIT_INF_EID, CFE_EVS_INFORMATION,
"SC Initialized. Version %d.%d.%d.%d",
SC_MAJOR_VERSION, SC_MINOR_VERSION, SC_REVISION, SC_MISSION_REV);
return(CFE_SUCCESS);
} /* end SC_AppInit() */
/*
** CFE_ES_putPoolBuf returns a block back to the memory pool.
*/
int32 CFE_ES_PutPoolBuf(CFE_ES_MemHandle_t Handle,
uint32 * BufPtr)
{
Pool_t *PoolPtr = (Pool_t *)Handle;
BD_t *BdPtr = (BD_t *) ((uint8 *)BufPtr - sizeof(BD_t));
uint32 Block;
/* OS_MutSemTake(PoolPtr->MutexId); */
/* Handle Preset Return Code */
if (cfe_es_api_return_value[CFE_ES_PUTPOOLBUF_PROC] != UTF_CFE_USE_DEFAULT_RETURN_CODE)
{
return cfe_es_api_return_value[CFE_ES_PUTPOOLBUF_PROC];
}
/* Handle Function Hook */
if (UTF_ES_HookTable.CFE_ES_PutPoolBuf)
return(UTF_ES_HookTable.CFE_ES_PutPoolBuf(Handle, *BufPtr));
if ( ((uint32)BdPtr < Handle) || ((uint32)BdPtr >= (PoolPtr->End - sizeof(BD_t))) )
{
/* sanity check */
CFE_ES_WriteToSysLog("CFE_ES:putPool-Invalid Memory Handle (0x%08X) or memory block (0x%08X).\n",
(uint32) Handle, (uint32)BdPtr);
/* OS_MutSemGive(PoolPtr->MutexId); */
return(CFE_ES_ERR_MEM_HANDLE);
}
/*
** Simple sanity checks for descriptor
*/
/* Don't allow a block that has already been deallocated to be deallocated again */
if (BdPtr->Allocated != CFE_ES_MEMORY_ALLOCATED)
{
PoolPtr->CheckErrCntr++;
CFE_ES_WriteToSysLog("CFE_ES:putPool-Deallocating unallocated memory block @ 0x%08X\n", (uint32)BdPtr);
/* OS_MutSemGive(PoolPtr->MutexId); */
return(CFE_ES_ERR_MEM_HANDLE);
}
if (BdPtr->CheckBits != CFE_ES_CHECK_PATTERN)
{
PoolPtr->CheckErrCntr++;
CFE_ES_WriteToSysLog("CFE_ES:putPool-Invalid Memory descriptor @ 0x%08X\n", (uint32)BdPtr);
/* OS_MutSemGive(PoolPtr->MutexId); */
return(CFE_ES_ERR_MEM_HANDLE);
}
Block = CFE_ES_GetBlockSize(PoolPtr, BdPtr->Size);
if (Block == 0xFFFFFFFF)
{
PoolPtr->CheckErrCntr++;
CFE_ES_WriteToSysLog("CFE_ES:putPool-Invalid/Corrupted Memory descriptor @ 0x%08X\n", (uint32)BdPtr);
/* OS_MutSemGive(PoolPtr->MutexId); */
return(CFE_ES_ERR_MEM_HANDLE);
}
BdPtr->Allocated = CFE_ES_MEMORY_DEALLOCATED;
BdPtr->Next = (uint32 *)PoolPtr->SizeDescPtr->Top; /* Set by GetBlockSize call */
PoolPtr->SizeDescPtr->Top = BdPtr;
PoolPtr->SizeDescPtr->NumFree++;
/* OS_MutSemGive(PoolPtr->MutexId); */
return (int32)Block;
}
/*
** CFE_ES_putPoolBuf returns a block back to the memory pool.
*/
int32 CFE_ES_PutPoolBuf(CFE_ES_MemHandle_t Handle,
uint32 * BufPtr)
{
Pool_t *PoolPtr = (Pool_t *)Handle;
BD_t *BdPtr = (BD_t *) ((uint8 *)BufPtr - sizeof(BD_t));
uint32 Block;
if (PoolPtr != NULL)
{
if ( ((uint32)BdPtr < Handle) || ((uint32)BdPtr >= (PoolPtr->End - sizeof(BD_t))) )
{
/* sanity check */
CFE_ES_WriteToSysLog("CFE_ES:putPoolBuf err:Invalid Memory Handle (0x%08X) or memory block (0x%08X).\n",
(uint32) Handle, (uint32)BdPtr);
return(CFE_ES_ERR_MEM_HANDLE);
}
}
else
{
/* sanity check */
CFE_ES_WriteToSysLog("CFE_ES:putPoolBuf err:Invalid Memory Handle (0x%08X).\n", (uint32) Handle);
return(CFE_ES_ERR_MEM_HANDLE);
}
if (PoolPtr->UseMutex == CFE_ES_USE_MUTEX)
{
OS_MutSemTake(PoolPtr->MutexId);
}
/*
** Simple sanity checks for descriptor
*/
/* Don't allow a block that has already been deallocated to be deallocated again */
if (BdPtr->Allocated != CFE_ES_MEMORY_ALLOCATED)
{
PoolPtr->CheckErrCntr++;
CFE_ES_WriteToSysLog("CFE_ES:putPoolBuf err:Deallocating unallocated memory block @ 0x%08X\n", (uint32)BdPtr);
if (PoolPtr->UseMutex == CFE_ES_USE_MUTEX)
{
OS_MutSemGive(PoolPtr->MutexId);
}
return(CFE_ES_ERR_MEM_HANDLE);
}
if (BdPtr->CheckBits != CFE_ES_CHECK_PATTERN)
{
PoolPtr->CheckErrCntr++;
CFE_ES_WriteToSysLog("CFE_ES:putPoolBuf err:Invalid/Corrupted Memory descriptor @ 0x%08X\n", (uint32)BdPtr);
if (PoolPtr->UseMutex == CFE_ES_USE_MUTEX)
{
OS_MutSemGive(PoolPtr->MutexId);
}
return(CFE_ES_ERR_MEM_HANDLE);
}
Block = CFE_ES_GetBlockSize(PoolPtr, BdPtr->Size);
if (Block == 0xFFFFFFFF)
{
PoolPtr->CheckErrCntr++;
CFE_ES_WriteToSysLog("CFE_ES:putPoolBuf err:size(%d) > max(%d).\n",BdPtr->Size,PoolPtr->SizeDesc[0].MaxSize);
if (PoolPtr->UseMutex == CFE_ES_USE_MUTEX)
{
OS_MutSemGive(PoolPtr->MutexId);
}
return(CFE_ES_ERR_MEM_HANDLE);
}
BdPtr->Allocated = CFE_ES_MEMORY_DEALLOCATED;
BdPtr->Next = (uint32 *)PoolPtr->SizeDescPtr->Top; /* Set by GetBlockSize call */
PoolPtr->SizeDescPtr->Top = BdPtr;
PoolPtr->SizeDescPtr->NumFree++;
if (PoolPtr->UseMutex == CFE_ES_USE_MUTEX)
{
OS_MutSemGive(PoolPtr->MutexId);
}
return (int32)Block;
}
int32 SIMCI_TaskInit(void) {
int32 Status = CFE_SUCCESS;
CFE_ES_RegisterApp();
SIMCI_AppData.CommandPipe = 0;
SIMCI_AppData.MsgPtr = NULL;
SIMCI_AppData.UplinkSocketID = 0;
SIMCI_AppData.IngestSocketID = 0;
SIMCI_AppData.UplinkSocketConnected = FALSE;
SIMCI_AppData.IngestSocketConnected = FALSE;
/* Set up and register events */
SIMCI_AppData.EventFilters[0].EventID = SIMCI_COMMANDRST_INF_EID;
SIMCI_AppData.EventFilters[0].Mask = CFE_EVS_NO_FILTER;
SIMCI_AppData.EventFilters[1].EventID = SIMCI_UPLINK_ERR_EID;
SIMCI_AppData.EventFilters[1].Mask = CFE_EVS_NO_FILTER;
SIMCI_AppData.EventFilters[2].EventID = SIMCI_INGEST_ERR_EID;
SIMCI_AppData.EventFilters[2].Mask = CFE_EVS_NO_FILTER;
SIMCI_AppData.EventFilters[3].EventID = SIMCI_CRCMDPIPE_ERR_EID;
SIMCI_AppData.EventFilters[3].Mask = CFE_EVS_NO_FILTER;
SIMCI_AppData.EventFilters[4].EventID = SIMCI_SOCKETCREATE_ERR_EID;
SIMCI_AppData.EventFilters[4].Mask = CFE_EVS_NO_FILTER;
SIMCI_AppData.EventFilters[5].EventID = SIMCI_SOCKETBIND_ERR_EID;
SIMCI_AppData.EventFilters[5].Mask = CFE_EVS_NO_FILTER;
SIMCI_AppData.EventFilters[6].EventID = SIMCI_MSGID_ERR_EID;
SIMCI_AppData.EventFilters[6].Mask = CFE_EVS_NO_FILTER;
SIMCI_AppData.EventFilters[7].EventID = SIMCI_CC1_ERR_EID;
SIMCI_AppData.EventFilters[7].Mask = CFE_EVS_NO_FILTER;
CFE_EVS_Register(SIMCI_AppData.EventFilters, SIMCI_FILTERED_EVT_COUNT, CFE_EVS_BINARY_FILTER);
if (Status != CFE_SUCCESS) {
CFE_ES_WriteToSysLog("SIMCI: Error registering events, RC = 0x%08X\n", Status);
return Status;
}
/* Initialize Housekeeping */
CFE_SB_InitMsg(&SIMCI_AppData.HkPacket, SIMCI_HK_TLM_MID, sizeof(SIMCI_HkPacket_t), TRUE);
SIMCI_ResetCounters();
SIMCI_AppData.HkPacket.ingest_socket_connected = FALSE;
SIMCI_AppData.HkPacket.uplink_socket_connected = FALSE;
/* Create a command pipe and subscribe to its messages */
Status = CFE_SB_CreatePipe(&SIMCI_AppData.CommandPipe, SIMCI_PIPE_DEPTH, "SIMCI_CMD_PIPE");
if (Status != CFE_SUCCESS) {
CFE_EVS_SendEvent(SIMCI_CRCMDPIPE_ERR_EID, CFE_EVS_ERROR, "SIMCI: Can't create command pipe, RC=%08X", Status);
return Status;
}
Status = CFE_SB_Subscribe(SIMCI_CMD_MID, SIMCI_AppData.CommandPipe);
if (Status != CFE_SUCCESS) {
CFE_EVS_SendEvent(SIMCI_SUBSCRIBE_ERR_EID, CFE_EVS_ERROR, "SIMCI: Error subscribing to SIMCI_CMD_MID(0x%04X), RC=%08X", SIMCI_CMD_MID, Status);
return Status;
}
Status = CFE_SB_Subscribe(SIMCI_SEND_HK_MID, SIMCI_AppData.CommandPipe);
if (Status != CFE_SUCCESS) {
CFE_EVS_SendEvent(SIMCI_SUBSCRIBE_ERR_EID, CFE_EVS_ERROR, "SIMCI: Error subscribing to SIMCI_SEND_HK_MID(0x%04X), RC=%08X", SIMCI_SEND_HK_MID, Status);
return Status;
}
/* Set up Uplink socket */
SIMCI_AppData.UplinkSocketID = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (SIMCI_AppData.UplinkSocketID < 0) {
CFE_EVS_SendEvent(SIMCI_SOCKETCREATE_ERR_EID, CFE_EVS_ERROR, "SIMCI: Error creating uplink socket, errno=%08X", errno);
return ERROR;
} else {
struct sockaddr_in socket_address;
CFE_PSP_MemSet((void*)&socket_address, 0, sizeof(socket_address));
socket_address.sin_family = AF_INET;
socket_address.sin_addr.s_addr = htonl(INADDR_ANY);
socket_address.sin_port = htons(SIMCI_UPLINK_PORT);
Status = bind(SIMCI_AppData.UplinkSocketID, (struct sockaddr *)&socket_address, sizeof(socket_address));
if (Status == ERROR) {
CFE_EVS_SendEvent(SIMCI_SOCKETBIND_ERR_EID, CFE_EVS_ERROR, "SIMCI: Error binding uplink socket, errno=%08X", errno);
return Status;
} else {
SIMCI_AppData.UplinkSocketConnected = TRUE;
}
}
/* Set up Ingest socket */
SIMCI_AppData.IngestSocketID = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (SIMCI_AppData.IngestSocketID < 0) {
CFE_EVS_SendEvent(SIMCI_SOCKETCREATE_ERR_EID, CFE_EVS_ERROR, "SIMCI: Error creating ingest socket, errno=%08X", errno);
return ERROR;
} else {
struct sockaddr_in socket_address;
CFE_PSP_MemSet((void*)&socket_address, 0, sizeof(socket_address));
socket_address.sin_family = AF_INET;
socket_address.sin_addr.s_addr = htonl(INADDR_ANY);
socket_address.sin_port = htons(SIMCI_INGEST_PORT);
Status = bind(SIMCI_AppData.IngestSocketID, (struct sockaddr *)&socket_address, sizeof(socket_address));
if (Status == ERROR) {
CFE_EVS_SendEvent(SIMCI_SOCKETBIND_ERR_EID, CFE_EVS_ERROR, "SIMCI: Error binding ingest socket, errno=%08X", errno);
return Status;
} else {
SIMCI_AppData.IngestSocketConnected = TRUE;
}
}
Status = SIMCI_HWIF_Init();
if (Status != CFE_SUCCESS) {
CFE_EVS_SendEvent(SIMCI_HWIF_ERR_EID, CFE_EVS_ERROR, "Error initializing HWIF, RC=0x%08X", Status);
return Status;
}
/* Install the delete handler */
OS_TaskInstallDeleteHandler((void*)&SIMCI_DeleteCallback);
CFE_EVS_SendEvent(SIMCI_STARTUP_INF_EID, CFE_EVS_INFORMATION, "SIMCI App Initialized");
return CFE_SUCCESS;
}
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void HS_AppMain(void)
{
int32 Status = CFE_SUCCESS;
uint32 RunStatus = CFE_ES_APP_RUN;
/*
** Performance Log, Start
*/
CFE_ES_PerfLogEntry(HS_APPMAIN_PERF_ID);
/*
** Register this application with Executive Services
*/
Status = CFE_ES_RegisterApp();
/*
** Perform application specific initialization
*/
if (Status == CFE_SUCCESS)
{
Status = HS_AppInit();
}
/*
** If no errors were detected during initialization, then wait for everyone to start
*/
if (Status == CFE_SUCCESS)
{
CFE_ES_WaitForStartupSync(HS_STARTUP_SYNC_TIMEOUT);
/*
** Enable and set the watchdog timer
*/
CFE_PSP_WatchdogSet(HS_WATCHDOG_TIMEOUT_VALUE);
CFE_PSP_WatchdogService();
CFE_PSP_WatchdogEnable();
CFE_PSP_WatchdogService();
/*
** Subscribe to Event Messages
*/
if (HS_AppData.CurrentEventMonState == HS_STATE_ENABLED)
{
Status = CFE_SB_SubscribeEx(CFE_EVS_EVENT_MSG_MID,
HS_AppData.EventPipe,
CFE_SB_Default_Qos,
HS_EVENT_PIPE_DEPTH);
if (Status != CFE_SUCCESS)
{
CFE_EVS_SendEvent(HS_SUB_EVS_ERR_EID, CFE_EVS_ERROR,
"Error Subscribing to Events,RC=0x%08X",Status);
}
}
}
if (Status != CFE_SUCCESS)
{
/*
** Set run status to terminate main loop
*/
RunStatus = CFE_ES_APP_ERROR;
}
/*
** Application main loop
*/
while(CFE_ES_RunLoop(&RunStatus) == TRUE)
{
/*
** Performance Log, Stop
*/
CFE_ES_PerfLogExit(HS_APPMAIN_PERF_ID);
/*
** Task Delay for a configured timeout
*/
#if HS_POST_PROCESSING_DELAY != 0
OS_TaskDelay(HS_POST_PROCESSING_DELAY);
#endif
/*
** Task Delay for a configured timeout
*/
Status = CFE_SB_RcvMsg(&HS_AppData.MsgPtr, HS_AppData.WakeupPipe, HS_WAKEUP_TIMEOUT);
/*
** Performance Log, Start
*/
CFE_ES_PerfLogEntry(HS_APPMAIN_PERF_ID);
/*
** Process the software bus message
*/
if ((Status == CFE_SUCCESS) ||
(Status == CFE_SB_NO_MESSAGE) ||
(Status == CFE_SB_TIME_OUT))
{
Status = HS_ProcessMain();
}
/*
** Note: If there were some reason to exit the task
** normally (without error) then we would set
** RunStatus = CFE_ES_APP_EXIT
*/
if (Status != CFE_SUCCESS)
{
/*
** Set request to terminate main loop
*/
RunStatus = CFE_ES_APP_ERROR;
}
} /* end CFS_ES_RunLoop while */
/*
** Check for "fatal" process error...
*/
if (Status != CFE_SUCCESS)
{
/*
** Send an event describing the reason for the termination
*/
CFE_EVS_SendEvent(HS_APP_EXIT_EID, CFE_EVS_CRITICAL,
"Application Terminating, err = 0x%08X", Status);
/*
** In case cFE Event Services is not working
*/
CFE_ES_WriteToSysLog("HS App: Application Terminating, ERR = 0x%08X\n", Status);
}
HS_CustomCleanup();
/*
** Performance Log, Stop
*/
CFE_ES_PerfLogExit(HS_APPMAIN_PERF_ID);
/*
** Exit the application
*/
CFE_ES_ExitApp(RunStatus);
} /* end HS_AppMain */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
int32 HS_AppInit(void)
{
int32 Status = CFE_SUCCESS;
/*
** Initialize operating data to default states...
*/
HS_AppData.ServiceWatchdogFlag = HS_STATE_ENABLED;
HS_AppData.AlivenessCounter = 0;
HS_AppData.RunStatus = CFE_ES_APP_RUN;
HS_AppData.EventsMonitoredCount = 0;
HS_AppData.MsgActExec = 0;
HS_AppData.CurrentAppMonState = HS_APPMON_DEFAULT_STATE;
HS_AppData.CurrentEventMonState = HS_EVENTMON_DEFAULT_STATE;
HS_AppData.CurrentAlivenessState = HS_ALIVENESS_DEFAULT_STATE;
HS_AppData.CurrentCPUHogState = HS_CPUHOG_DEFAULT_STATE;
#if HS_MAX_EXEC_CNT_SLOTS != 0
HS_AppData.ExeCountState = HS_STATE_ENABLED;
#else
HS_AppData.ExeCountState = HS_STATE_DISABLED;
#endif
HS_AppData.MsgActsState = HS_STATE_ENABLED;
HS_AppData.AppMonLoaded = HS_STATE_ENABLED;
HS_AppData.EventMonLoaded = HS_STATE_ENABLED;
HS_AppData.CDSState = HS_STATE_ENABLED;
HS_AppData.CurrentCPUUtilIndex = 0;
HS_AppData.CurrentCPUHoggingTime = 0;
HS_AppData.MaxCPUHoggingTime = HS_UTIL_HOGGING_TIMEOUT;
HS_AppData.UtilCpuAvg = 0;
HS_AppData.UtilCpuPeak = 0;
/*
** Register for event services...
*/
Status = CFE_EVS_Register (NULL, 0, CFE_EVS_BINARY_FILTER);
if (Status != CFE_SUCCESS)
{
CFE_ES_WriteToSysLog("HS App: Error Registering For Event Services, RC = 0x%08X\n", Status);
return (Status);
}
/*
** Create Critical Data Store
*/
Status = CFE_ES_RegisterCDS(&HS_AppData.MyCDSHandle,
sizeof(HS_CDSData_t),
HS_CDSNAME);
if (Status == CFE_ES_CDS_ALREADY_EXISTS)
{
/*
** Critical Data Store already existed, we need to get a
** copy of its current contents to see if we can use it
*/
Status = CFE_ES_RestoreFromCDS(&HS_AppData.CDSData, HS_AppData.MyCDSHandle);
if (Status == CFE_SUCCESS)
{
if((HS_AppData.CDSData.ResetsPerformed != (uint16) ~HS_AppData.CDSData.ResetsPerformedNot) ||
(HS_AppData.CDSData.MaxResets != (uint16) ~HS_AppData.CDSData.MaxResetsNot))
{
/*
** Report error restoring data
*/
CFE_EVS_SendEvent(HS_CDS_CORRUPT_ERR_EID, CFE_EVS_ERROR,
"Data in CDS was corrupt, initializing resets data", Status);
/*
** If data was corrupt, initialize data
*/
HS_SetCDSData(0, HS_MAX_RESTART_ACTIONS);
}
}
else
{
/*
** Report error restoring data
*/
CFE_EVS_SendEvent(HS_CDS_RESTORE_ERR_EID, CFE_EVS_ERROR,
"Failed to restore data from CDS (Err=0x%08x), initializing resets data", Status);
/*
** If data could not be retrieved, initialize data
*/
HS_SetCDSData(0, HS_MAX_RESTART_ACTIONS);
}
Status = CFE_SUCCESS;
}
else if (Status == CFE_SUCCESS)
{
/*
** If CDS did not previously exist, initialize data
*/
HS_SetCDSData(0, HS_MAX_RESTART_ACTIONS);
}
else
{
/*
** Disable saving to CDS
*/
HS_AppData.CDSState = HS_STATE_DISABLED;
/*
** Initialize values anyway (they will not be saved)
*/
HS_SetCDSData(0, HS_MAX_RESTART_ACTIONS);
}
/*
** Set up the HS Software Bus
*/
Status = HS_SbInit();
if(Status != CFE_SUCCESS)
{
return (Status);
}
/*
** Register The HS Tables
*/
Status = HS_TblInit();
if(Status != CFE_SUCCESS)
{
return (Status);
}
/*
** Perform custom initialization (for cpu utilization monitoring)
*/
Status = HS_CustomInit();
/*
** Application initialization event
*/
CFE_EVS_SendEvent (HS_INIT_EID, CFE_EVS_INFORMATION,
"HS Initialized. Version %d.%d.%d.%d",
HS_MAJOR_VERSION,
HS_MINOR_VERSION,
HS_REVISION,
HS_MISSION_REV);
return CFE_SUCCESS;
} /* end HS_AppInit */
/*
** Function:
** CFE_ES_CDSBlockWrite
**
** Purpose:
**
*/
int32 CFE_ES_CDSBlockWrite(CFE_ES_CDSBlockHandle_t BlockHandle, void *DataToWrite)
{
int32 Status = CFE_SUCCESS;
int32 BinIndex = 0;
/* Validate the handle before doing anything */
if ((BlockHandle < sizeof(CFE_ES_Global.CDSVars.ValidityField)) ||
(BlockHandle > (CFE_ES_CDSMemPool.End - sizeof(CFE_ES_CDSBlockDesc_t) -
CFE_ES_CDSMemPool.MinBlockSize - sizeof(CFE_ES_Global.CDSVars.ValidityField))))
{
CFE_ES_WriteToSysLog("CFE_ES:CDSBlkWrite-Invalid Memory Handle.\n");
return(CFE_ES_ERR_MEM_HANDLE);
}
OS_MutSemTake(CFE_ES_CDSMemPool.MutexId);
/* Get a copy of the block descriptor associated with the specified handle */
/* Read the block descriptor for the first block in the memory pool */
Status = CFE_PSP_ReadFromCDS(&CFE_ES_CDSBlockDesc, BlockHandle, sizeof(CFE_ES_CDSBlockDesc_t));
if (Status == OS_SUCCESS)
{
/* Validate the block to make sure it is still active and not corrupted */
if ((CFE_ES_CDSBlockDesc.CheckBits != CFE_ES_CDS_CHECK_PATTERN) ||
(CFE_ES_CDSBlockDesc.AllocatedFlag != CFE_ES_CDS_BLOCK_USED))
{
CFE_ES_WriteToSysLog("CFE_ES:CDSBlkWrite-Invalid Handle or Block Descriptor.\n");
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return(CFE_ES_ERR_MEM_HANDLE);
}
BinIndex = CFE_ES_CDSGetBinIndex(CFE_ES_CDSBlockDesc.ActualSize);
/* Final sanity check on block descriptor, is the Actual size reasonable */
if (BinIndex < 0)
{
CFE_ES_CDSMemPool.CheckErrCntr++;
CFE_ES_WriteToSysLog("CFE_ES:CDSBlkWrite-Invalid Block Descriptor\n");
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return(CFE_ES_ERR_MEM_HANDLE);
}
/* Use the size specified when the CDS was created to compute the CRC */
CFE_ES_CDSBlockDesc.CRC = CFE_ES_CalculateCRC(DataToWrite, CFE_ES_CDSBlockDesc.SizeUsed, 0, CFE_ES_DEFAULT_CRC);
/* Write the new block descriptor for the data coming from the Application */
Status = CFE_PSP_WriteToCDS(&CFE_ES_CDSBlockDesc, BlockHandle, sizeof(CFE_ES_CDSBlockDesc_t));
if (Status == OS_SUCCESS)
{
/* Write the new data coming from the Application to the CDS */
Status = CFE_PSP_WriteToCDS(DataToWrite, (BlockHandle + sizeof(CFE_ES_CDSBlockDesc_t)), CFE_ES_CDSBlockDesc.SizeUsed);
if (Status != OS_SUCCESS)
{
CFE_ES_WriteToSysLog("CFE_ES:CDSBlkWrite-Err writing data to CDS (Stat=0x%08x) @Offset=0x%08x\n",
Status, (BlockHandle + sizeof(CFE_ES_CDSBlockDesc_t)));
}
}
else
{
CFE_ES_WriteToSysLog("CFE_ES:CDSBlkWrite-Err writing BlockDesc to CDS (Stat=0x%08x) @Offset=0x%08x\n",
Status, BlockHandle);
}
}
else
{
CFE_ES_WriteToSysLog("CFE_ES:CDSBlkWrite-Err reading from CDS (Stat=0x%08x)\n", Status);
}
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return Status;
}
/*
** Function:
** CFE_ES_CDSBlockRead
**
** Purpose:
**
*/
int32 CFE_ES_CDSBlockRead(void *DataRead, CFE_ES_CDSBlockHandle_t BlockHandle)
{
int32 Status = CFE_SUCCESS;
uint32 CrcOfCDSData;
int32 BinIndex;
/* Validate the handle before doing anything */
if ((BlockHandle < sizeof(CFE_ES_Global.CDSVars.ValidityField)) ||
(BlockHandle > (CFE_ES_CDSMemPool.End - sizeof(CFE_ES_CDSBlockDesc_t) -
CFE_ES_CDSMemPool.MinBlockSize - sizeof(CFE_ES_Global.CDSVars.ValidityField))))
{
CFE_ES_WriteToSysLog("CFE_ES:CDSBlkRd-Invalid Memory Handle.\n");
return(CFE_ES_ERR_MEM_HANDLE);
}
OS_MutSemTake(CFE_ES_CDSMemPool.MutexId);
/* Get a copy of the block descriptor associated with the specified handle */
/* Read the block descriptor for the first block in the memory pool */
Status = CFE_PSP_ReadFromCDS(&CFE_ES_CDSBlockDesc, BlockHandle, sizeof(CFE_ES_CDSBlockDesc_t));
if (Status == OS_SUCCESS)
{
/* Validate the block to make sure it is still active and not corrupted */
if ((CFE_ES_CDSBlockDesc.CheckBits != CFE_ES_CDS_CHECK_PATTERN) ||
(CFE_ES_CDSBlockDesc.AllocatedFlag != CFE_ES_CDS_BLOCK_USED))
{
CFE_ES_WriteToSysLog("CFE_ES:CDSBlkRd-Invalid Handle or Block Descriptor.\n");
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return(CFE_ES_ERR_MEM_HANDLE);
}
BinIndex = CFE_ES_CDSGetBinIndex(CFE_ES_CDSBlockDesc.ActualSize);
/* Final sanity check on block descriptor, is the Actual size reasonable */
if (BinIndex < 0)
{
CFE_ES_CDSMemPool.CheckErrCntr++;
CFE_ES_WriteToSysLog("CFE_ES:CDSBlkRd-Invalid Block Descriptor\n");
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return(CFE_ES_ERR_MEM_HANDLE);
}
/* Read the old data block */
Status = CFE_PSP_ReadFromCDS(DataRead, (BlockHandle + sizeof(CFE_ES_CDSBlockDesc_t)), CFE_ES_CDSBlockDesc.SizeUsed);
if (Status == OS_SUCCESS)
{
/* Compute the CRC for the data read from the CDS and determine if the data is still valid */
CrcOfCDSData = CFE_ES_CalculateCRC(DataRead, CFE_ES_CDSBlockDesc.SizeUsed, 0, CFE_ES_DEFAULT_CRC);
/* If the CRCs do not match, report an error */
if (CrcOfCDSData != CFE_ES_CDSBlockDesc.CRC)
{
Status = CFE_ES_CDS_BLOCK_CRC_ERR;
}
else
{
Status = CFE_SUCCESS;
}
}
else
{
CFE_ES_WriteToSysLog("CFE_ES:CDSBlkRd-Err reading block from CDS (Stat=0x%08x) @Offset=0x%08x\n",
Status, BlockHandle);
}
}
else
{
CFE_ES_WriteToSysLog("CFE_ES:CDSBlkRd-Err reading from CDS (Stat=0x%08x)\n", Status);
}
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return Status;
}
/*
** CFE_ES_PutCDSBlock returns a block back to the CDS memory pool.
*/
int32 CFE_ES_PutCDSBlock(CFE_ES_CDSBlockHandle_t BlockHandle)
{
int32 BinIndex;
int32 Status;
/* Perform some sanity checks on the BlockHandle */
/* First check, is the handle within an acceptable range of CDS offsets */
if ((BlockHandle < sizeof(CFE_ES_Global.CDSVars.ValidityField)) ||
(BlockHandle > (CFE_ES_CDSMemPool.End - sizeof(CFE_ES_CDSBlockDesc_t) -
CFE_ES_CDSMemPool.MinBlockSize - sizeof(CFE_ES_Global.CDSVars.ValidityField))))
{
CFE_ES_WriteToSysLog("CFE_ES:PutCDSBlock-Invalid Memory Handle.\n");
return(CFE_ES_ERR_MEM_HANDLE);
}
OS_MutSemTake(CFE_ES_CDSMemPool.MutexId);
/* Read a copy of the contents of the block descriptor being freed */
Status = CFE_PSP_ReadFromCDS(&CFE_ES_CDSBlockDesc, BlockHandle, sizeof(CFE_ES_CDSBlockDesc_t));
if (Status != OS_SUCCESS)
{
CFE_ES_WriteToSysLog("CFE_ES:PutCDSBlock-Err reading from CDS (Stat=0x%08x)\n", Status);
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return(CFE_ES_CDS_ACCESS_ERROR);
}
/* Make sure the contents of the Block Descriptor look reasonable */
if ((CFE_ES_CDSBlockDesc.CheckBits != CFE_ES_CDS_CHECK_PATTERN) ||
(CFE_ES_CDSBlockDesc.AllocatedFlag != CFE_ES_CDS_BLOCK_USED))
{
CFE_ES_WriteToSysLog("CFE_ES:PutCDSBlock-Invalid Handle or Block Descriptor.\n");
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return(CFE_ES_ERR_MEM_HANDLE);
}
BinIndex = CFE_ES_CDSGetBinIndex(CFE_ES_CDSBlockDesc.ActualSize);
/* Final sanity check on block descriptor, is the Actual size reasonable */
if (BinIndex < 0)
{
CFE_ES_CDSMemPool.CheckErrCntr++;
CFE_ES_WriteToSysLog("CFE_ES:PutCDSBlock-Invalid Block Descriptor\n");
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return(CFE_ES_ERR_MEM_HANDLE);
}
CFE_ES_CDSBlockDesc.Next = CFE_ES_CDSMemPool.SizeDesc[BinIndex].Top;
CFE_ES_CDSBlockDesc.AllocatedFlag = CFE_ES_CDS_BLOCK_UNUSED;
CFE_ES_CDSMemPool.SizeDesc[BinIndex].Top = BlockHandle;
/* Store the new CDS Block Descriptor in the CDS */
Status = CFE_PSP_WriteToCDS(&CFE_ES_CDSBlockDesc, BlockHandle, sizeof(CFE_ES_CDSBlockDesc_t));
if (Status != OS_SUCCESS)
{
CFE_ES_WriteToSysLog("CFE_ES:PutCDSBlock-Err writing to CDS (Stat=0x%08x)\n", Status);
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return(CFE_ES_CDS_ACCESS_ERROR);
}
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return Status;
}
/*
** Function:
** CFE_ES_GetCDSBlock
**
** Purpose:
** CFE_ES_GetCDSBlock allocates a block from the CDS memory pool.
*/
int32 CFE_ES_GetCDSBlock(CFE_ES_CDSBlockHandle_t *BlockHandle,
uint32 BlockSize )
{
int32 BinIndex;
int32 Status;
OS_MutSemTake(CFE_ES_CDSMemPool.MutexId);
BinIndex = CFE_ES_CDSGetBinIndex(BlockSize);
if (BinIndex < 0)
{
CFE_ES_WriteToSysLog("CFE_ES:GetCDSBlock-err:size(%d) > max(%d).\n", BlockSize, CFE_ES_CDS_MAX_BLOCK_SIZE);
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return(CFE_ES_ERR_MEM_BLOCK_SIZE);
}
/*
** Check if any of the requested size are available
*/
if (CFE_ES_CDSMemPool.SizeDesc[BinIndex].Top != 0)
{
/*
** Get it off the top on the list
*/
Status = CFE_PSP_ReadFromCDS(&CFE_ES_CDSBlockDesc,
CFE_ES_CDSMemPool.SizeDesc[BinIndex].Top,
sizeof(CFE_ES_CDSBlockDesc_t));
if (Status != OS_SUCCESS)
{
CFE_ES_WriteToSysLog("CFE_ES:GetCDSBlock-Err reading from CDS (Stat=0x%08x)\n", Status);
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return(CFE_ES_CDS_ACCESS_ERROR);
}
/* The handle returned is the byte offset of the block in the CDS */
*BlockHandle = CFE_ES_CDSMemPool.SizeDesc[BinIndex].Top;
/* A local version of the block descriptor is initialized */
CFE_ES_CDSBlockDesc.CheckBits = CFE_ES_CDS_CHECK_PATTERN;
CFE_ES_CDSBlockDesc.AllocatedFlag = CFE_ES_CDS_BLOCK_USED;
CFE_ES_CDSBlockDesc.SizeUsed = BlockSize;
CFE_ES_CDSBlockDesc.ActualSize = CFE_ES_CDSMemPool.SizeDesc[BinIndex].MaxSize;
CFE_ES_CDSMemPool.SizeDesc[BinIndex].Top = CFE_ES_CDSBlockDesc.Next;
CFE_ES_CDSBlockDesc.CRC = 0;
CFE_ES_CDSBlockDesc.Next = 0;
}
else /* Create a new block */
{
if ( (CFE_ES_CDSMemPool.Current == 0) ||
(((uint32)CFE_ES_CDSMemPool.Current +
sizeof(CFE_ES_CDSBlockDesc_t) +
CFE_ES_CDSMemPool.SizeDesc[BinIndex].MaxSize ) >= CFE_ES_CDSMemPool.End) )
{
CFE_ES_WriteToSysLog("CFE_ES:GetCDSBlock-err:Request for %d bytes won't fit in remaining memory\n", BlockSize);
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return(CFE_ES_ERR_MEM_BLOCK_SIZE);
}
*BlockHandle = (CFE_ES_CDSBlockHandle_t)CFE_ES_CDSMemPool.Current;
CFE_ES_CDSMemPool.SizeDesc[BinIndex].NumCreated++;
CFE_ES_CDSMemPool.RequestCntr++;
/*
** Initialize the buffer descriptor that will be kept in front of the CDS Block
*/
CFE_ES_CDSBlockDesc.CheckBits = CFE_ES_CDS_CHECK_PATTERN;
CFE_ES_CDSBlockDesc.AllocatedFlag = CFE_ES_CDS_BLOCK_USED;
CFE_ES_CDSBlockDesc.SizeUsed = BlockSize;
CFE_ES_CDSBlockDesc.ActualSize = CFE_ES_CDSMemPool.SizeDesc[BinIndex].MaxSize;
CFE_ES_CDSBlockDesc.CRC = 0;
CFE_ES_CDSBlockDesc.Next = 0;
/*
** Adjust pool current pointer to first unallocated byte in CDS
*/
CFE_ES_CDSMemPool.Current = CFE_ES_CDSMemPool.Current
+ CFE_ES_CDSBlockDesc.ActualSize
+ sizeof(CFE_ES_CDSBlockDesc_t);
}
/* Store the new CDS Block Descriptor in the CDS */
Status = CFE_PSP_WriteToCDS(&CFE_ES_CDSBlockDesc, *BlockHandle, sizeof(CFE_ES_CDSBlockDesc_t));
if (Status != OS_SUCCESS)
{
CFE_ES_WriteToSysLog("CFE_ES:GetCDSBlock-Err writing to CDS (Stat=0x%08x)\n", Status);
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return(CFE_ES_CDS_ACCESS_ERROR);
}
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return Status;
}
/*
** Function:
** CFE_ES_RebuildCDSPool
**
** Purpose:
**
*/
int32 CFE_ES_RebuildCDSPool(uint32 CDSPoolSize, uint32 StartOffset)
{
char MutexName[10] = {"CDS_POOL"};
uint32 i = 0;
uint32 Size = (CDSPoolSize & 0xfffffffc);
int32 Status = OS_SUCCESS;
uint32 Offset = StartOffset;
int32 BinIndex = 0;
/* create a semphore to protect this memory pool */
OS_MutSemCreate(&(CFE_ES_CDSMemPool.MutexId), MutexName, 0);
/* Take the semaphore to ensure the mem pool is not being used during it's creation */
OS_MutSemTake(CFE_ES_CDSMemPool.MutexId);
CFE_ES_CDSMemPool.Start = StartOffset;
CFE_ES_CDSMemPool.End = StartOffset + Size;
CFE_ES_CDSMemPool.Size = Size;
CFE_ES_CDSMemPool.Current = 0;
CFE_ES_CDSMemPool.SizeIndex = -1;
CFE_ES_CDSMemPool.CheckErrCntr = 0;
CFE_ES_CDSMemPool.RequestCntr = 0;
for (i=0; i<CFE_ES_CDS_NUM_BLOCK_SIZES; i++)
{
CFE_ES_CDSMemPool.SizeDesc[i].Top = 0;
CFE_ES_CDSMemPool.SizeDesc[i].NumCreated = 0;
CFE_ES_CDSMemPool.SizeDesc[i].MaxSize = CFE_ES_CDSMemPoolDefSize[i];
}
if (CDSPoolSize < (CFE_ES_CDSMemPool.MinBlockSize + sizeof(CFE_ES_CDSBlockDesc_t)))
{
/* Must be able make Pool verification, block descriptor and at least one of the smallest blocks */
CFE_ES_WriteToSysLog("CFE_ES:RebuildCDSPool-Pool size(%u) too small for one CDS Block, need >=%u\n",
CDSPoolSize, (CFE_ES_CDSMemPool.MinBlockSize + sizeof(CFE_ES_CDSBlockDesc_t)));
/* Give and delete semaphore since CDS Pool rebuild failed */
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
OS_MutSemDelete(CFE_ES_CDSMemPool.MutexId);
return(CFE_ES_BAD_ARGUMENT);
}
/* Scan the CDS memory trying to find blocks that were created but are now free */
while ((Status == OS_SUCCESS) &&
(Offset < (CFE_ES_CDSMemPool.End - sizeof(CFE_ES_CDSBlockDesc_t))) &&
(CFE_ES_CDSMemPool.Current == 0))
{
/* Read the block descriptor for the first block in the memory pool */
Status = CFE_PSP_ReadFromCDS(&CFE_ES_CDSBlockDesc, Offset, sizeof(CFE_ES_CDSBlockDesc_t));
if (Status == OS_SUCCESS)
{
/* First, determine if the block is being or has been used */
if (CFE_ES_CDSBlockDesc.CheckBits == CFE_ES_CDS_CHECK_PATTERN)
{
/* See if the block is currently being used */
if (CFE_ES_CDSBlockDesc.AllocatedFlag != CFE_ES_CDS_BLOCK_USED)
{
/* If the block is not currently being used, */
/* then add it to the appropriate linked list in the memory pool */
BinIndex = CFE_ES_CDSGetBinIndex(CFE_ES_CDSBlockDesc.SizeUsed);
/* Sanity-check the block descriptor */
if (BinIndex >= 0)
{
CFE_ES_CDSBlockDesc.Next = CFE_ES_CDSMemPool.SizeDesc[BinIndex].Top;
CFE_ES_CDSBlockDesc.AllocatedFlag = CFE_ES_CDS_BLOCK_UNUSED;
CFE_ES_CDSMemPool.SizeDesc[BinIndex].Top = Offset;
/* Store the new CDS Block Descriptor in the CDS */
Status = CFE_PSP_WriteToCDS(&CFE_ES_CDSBlockDesc, Offset, sizeof(CFE_ES_CDSBlockDesc_t));
if (Status != OS_SUCCESS)
{
CFE_ES_WriteToSysLog("CFE_ES:RebuildCDS-Err writing to CDS (Stat=0x%08x)\n", Status);
Status = CFE_ES_CDS_ACCESS_ERROR;
}
}
else
{
CFE_ES_CDSMemPool.CheckErrCntr++;
CFE_ES_WriteToSysLog("CFE_ES:RebuildCDS-Invalid Block Descriptor \n");
Status = CFE_ES_CDS_ACCESS_ERROR;
}
}
/* Skip to the next block of memory */
Offset = Offset + CFE_ES_CDSBlockDesc.ActualSize + sizeof(CFE_ES_CDSBlockDesc_t);
}
else
{
/* If the block has never been used, then we should save the offset as the current offset */
/* which in turn will finish the scan of the CDS memory */
CFE_ES_CDSMemPool.Current = Offset;
}
}
else
{
CFE_ES_WriteToSysLog("CFE_ES:RebuildCDS-Err reading from CDS (Stat=0x%08x)\n", Status);
Status = CFE_ES_CDS_ACCESS_ERROR;
}
} /* end while */
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return Status;
}
int32 CFE_ES_PoolCreateEx(CFE_ES_MemHandle_t *HandlePtr,
uint8 *MemPtr,
uint32 Size,
uint32 NumBlockSizes,
uint32 *BlockSizes,
uint16 UseMutex )
{
char MutexName[10];
uint32 i;
uint32 j;
uint32 k;
uint32 *BlockSizeArrayPtr;
uint32 BlockSizeArraySize;
uint32 MinBlockSize;
/*
** Local Variables
*/
Pool_t *PoolPtr = (Pool_t *)MemPtr;
*HandlePtr = (uint32)MemPtr;
/* Force the size given to be 32 bit aligned */
Size &= 0xFFFFFFFC;
#ifdef CFE_ES_MEMPOOL_ALIGNED
/* Determine if the memory pool address is 32-bit aligned */
if ((((uint32)MemPtr) & 0x00000003) != 0)
{
CFE_ES_WriteToSysLog("CFE_ES:poolCreate Pool Address(0x%08X) is not 32-bit aligned.\n", (uint32)MemPtr);
return(CFE_ES_BAD_ARGUMENT);
}
#endif
/* If too many sizes are specified, return an error */
if (NumBlockSizes > CFE_ES_MAX_MEMPOOL_BLOCK_SIZES)
{
CFE_ES_WriteToSysLog("CFE_ES:poolCreate Num Block Sizes (%d) greater than max (%d)\n",
NumBlockSizes, CFE_ES_MAX_MEMPOOL_BLOCK_SIZES);
return(CFE_ES_BAD_ARGUMENT);
}
if ((UseMutex != CFE_ES_USE_MUTEX) && (UseMutex != CFE_ES_NO_MUTEX))
{
CFE_ES_WriteToSysLog("CFE_ES:poolCreate Invalid Mutex Usage Option (%d), must be %d or %d\n",
UseMutex, CFE_ES_NO_MUTEX, CFE_ES_USE_MUTEX);
return(CFE_ES_BAD_ARGUMENT);
}
if (UseMutex == CFE_ES_USE_MUTEX)
{
/*
** Construct a name for the Mutex from the address
** This is needed only because OS_MutSemCreate requires
** a unique name for each semaphore created.
*/
sprintf(MutexName, "%08X", (unsigned int)MemPtr);
/* create a semphore to protect this memory pool */
OS_MutSemCreate(&(PoolPtr->MutexId), MutexName, 0);
/* Take the semaphore to ensure the mem pool is not being used during its creation */
OS_MutSemTake(PoolPtr->MutexId);
}
PoolPtr->Start = (uint32 *)*HandlePtr;
PoolPtr->End = (uint32)((uint8 *)PoolPtr->Start + Size);
PoolPtr->Size = Size;
PoolPtr->Current = (uint32 *)(MemPtr + sizeof(Pool_t));
PoolPtr->SizeDescPtr = NULL;
PoolPtr->CheckErrCntr = 0;
PoolPtr->RequestCntr = 0;
PoolPtr->UseMutex = UseMutex;
for (i=0; i<CFE_ES_MAX_MEMPOOL_BLOCK_SIZES; i++)
{
PoolPtr->SizeDesc[i].NumCreated = 0;
PoolPtr->SizeDesc[i].NumFree = 0;
PoolPtr->SizeDesc[i].MaxSize = 0;
PoolPtr->SizeDesc[i].Top = NULL;
}
/* Use default block sizes if none or too many sizes are specified */
if ((NumBlockSizes == 0) || (BlockSizes == NULL))
{
BlockSizeArrayPtr = &CFE_ES_MemPoolDefSize[0];
BlockSizeArraySize = CFE_ES_MAX_MEMPOOL_BLOCK_SIZES;
}
else
{
BlockSizeArrayPtr = BlockSizes;
BlockSizeArraySize = NumBlockSizes;
}
/* Use specified block sizes but make sure they are ordered largest to smallest */
MinBlockSize = 0xffffffff;
for (i=0; i<BlockSizeArraySize; i++)
{
if ((BlockSizeArrayPtr[i] < MinBlockSize) && (BlockSizeArrayPtr[i] != 0))
{
MinBlockSize = BlockSizeArrayPtr[i];
}
j = 0;
while (j<CFE_ES_MAX_MEMPOOL_BLOCK_SIZES)
{
if (BlockSizeArrayPtr[i] > PoolPtr->SizeDesc[j].MaxSize)
{
/* Make space for new size */
for (k=i; k>j; k--)
{
PoolPtr->SizeDesc[k].MaxSize = PoolPtr->SizeDesc[k-1].MaxSize;
}
/* Insert the size in the correct location */
PoolPtr->SizeDesc[j].MaxSize = BlockSizeArrayPtr[i];
j = CFE_ES_MAX_MEMPOOL_BLOCK_SIZES;
}
else
{
j++;
}
}
}
if (Size <= (sizeof(Pool_t) + MinBlockSize + sizeof(BD_t) ) )
{ /* must be able make Pool management structure and at least one of the smallest blocks */
CFE_ES_WriteToSysLog("CFE_ES:poolCreate Pool size(%d) too small for management structure, need >=(%d)\n",
(int)Size,
(int)(sizeof(Pool_t) + MinBlockSize + sizeof(BD_t)) );
if (UseMutex == CFE_ES_USE_MUTEX)
{
/* Mutex is no longer needed with error in initialization. Give it and delete it */
OS_MutSemGive(PoolPtr->MutexId);
OS_MutSemDelete(PoolPtr->MutexId);
}
return(CFE_ES_BAD_ARGUMENT);
}
if (UseMutex == CFE_ES_USE_MUTEX)
{
OS_MutSemGive(PoolPtr->MutexId);
}
return(CFE_SUCCESS);
}
/*
** Name: CFE_ES_Main
** Purpose: This is the entry point to the cFE application code.
**
*/
void CFE_ES_Main(uint32 StartType, uint32 StartSubtype, uint32 ModeId, uint8 *StartFilePath )
{
int i;
int32 ReturnCode;
/*
** Initialize the Reset variables. This call is required
** Before most of the ES functions can be used including the
** ES System log.
*/
CFE_ES_SetupResetVariables(StartType, StartSubtype, ModeId);
/*
** Initialize the Logic Perf variables
** Because this is in the ES Reset area, it must be called after
** CFE_ES_SetupResetVariables.
*/
CFE_ES_SetupPerfVariables(StartType);
/*
** Announce the startup
*/
CFE_ES_WriteToSysLog("ES Startup: CFE_ES_Main started\n");
/*
** Create and Mount the filesystems needed
*/
CFE_ES_InitializeFileSystems(StartType);
/*
** Install exception Handlers ( Placeholder )
*/
CFE_PSP_AttachExceptions();
/*
** Initialize the ES Application Table
** to mark all entries as unused.
*/
for ( i = 0; i < CFE_ES_MAX_APPLICATIONS; i++ )
{
CFE_ES_Global.AppTable[i].RecordUsed = FALSE;
}
/*
** Initialize the ES Task Table
** to mark all entries as unused.
*/
for ( i = 0; i < OS_MAX_TASKS; i++ )
{
CFE_ES_Global.TaskTable[i].RecordUsed = FALSE;
}
/*
** Initialize the ES Generic Counter Table
** to mark all entries as unused.
*/
for ( i = 0; i < CFE_ES_MAX_GEN_COUNTERS; i++ )
{
CFE_ES_Global.CounterTable[i].RecordUsed = FALSE;
}
/*
** Create the ES Shared Data Mutex
*/
ReturnCode = OS_MutSemCreate(&(CFE_ES_Global.SharedDataMutex), "ES_DATA_MUTEX", 0 );
if(ReturnCode != OS_SUCCESS)
{
CFE_ES_WriteToSysLog("ES Startup: Error: ES Shared Data Mutex could not be created. RC=0x%08X\n",
ReturnCode);
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_STARTUP_SEM);
} /* end if */
/*
** Create the ES Startup Sync Semaphore
*/
ReturnCode = OS_BinSemCreate(&(CFE_ES_Global.StartupSyncSemaphore),"ES_SYNC_SEM", 0, 0 );
if(ReturnCode != OS_SUCCESS)
{
CFE_ES_WriteToSysLog("ES Startup: Error: ES Startup Sync Semaphore could not be created. RC=0x%08X\n",
ReturnCode);
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_STARTUP_SEM);
} /* end if */
/*
** Create the tasks, OS objects, and initialize hardware
*/
CFE_ES_CreateObjects();
/*
** Before the Applications are loaded, set up the state
** variables that allow the startup syncronization to work.
** This depends on a couple of things:
** 1. The fact that the cFE applications all use the CFE_ES_RunLoop Call
** 2. The StartupSyncSemaphore has been created ( see above )
** 3. The App(s) that wish to wait calls CFE_ES_WaitForStartupSync
*/
CFE_ES_Global.AppStartupCounter = 0;
CFE_ES_Global.StartupFileComplete = FALSE;
CFE_ES_Global.StartupSemaphoreReleased = FALSE;
/*
** Start the cFE Applications from the disk using the file
** specified in the CFE_ES_NONVOL_STARTUP_FILE or CFE_ES_VOLATILE_STARTUP_FILE
** ( defined in the cfe_platform_cfg.h file )
*/
CFE_ES_StartApplications(StartType, StartFilePath );
/*
** Indicate that the startup file is complete. This avoids
** certain race conditions.
*/
CFE_ES_Global.StartupFileComplete = TRUE;
/*
** Startup is complete
*/
CFE_ES_WriteToSysLog("ES Startup: CFE Core Startup Complete\n");
}
/*
** Function:
** CFE_ES_GetPoolBuf
**
** Purpose:
** CFE_ES_GetPoolBuf allocates a block from the memory pool.
*/
int32 CFE_ES_GetPoolBuf(uint32 **BufPtr,
CFE_ES_MemHandle_t Handle,
uint32 Size )
{
Pool_t * PoolPtr = (Pool_t *)Handle;
uint32 Block;
BD_t * BdPtr;
uint32 AppId= 0xFFFFFFFF;
if (PoolPtr != NULL)
{
if (Handle != (CFE_ES_MemHandle_t)PoolPtr->Start)
{
CFE_ES_GetAppID(&AppId);
CFE_ES_WriteToSysLog("CFE_ES:getPoolBuf err:Bad handle(0x%08X) AppId=%d\n",Handle,AppId);
return(CFE_ES_ERR_MEM_HANDLE);
}
}
else
{
CFE_ES_GetAppID(&AppId);
CFE_ES_WriteToSysLog("CFE_ES:getPoolBuf err:Bad handle(0x%08X) AppId=%d\n",Handle,AppId);
return(CFE_ES_ERR_MEM_HANDLE);
}
if (PoolPtr->UseMutex == CFE_ES_USE_MUTEX)
{
OS_MutSemTake(PoolPtr->MutexId);
}
*BufPtr = NULL;
Block = CFE_ES_GetBlockSize(PoolPtr, Size);
if (Block == 0xFFFFFFFF)
{
CFE_ES_WriteToSysLog("CFE_ES:getPoolBuf err:size(%d) > max(%d).\n",Size,PoolPtr->SizeDesc[0].MaxSize);
if (PoolPtr->UseMutex == CFE_ES_USE_MUTEX)
{
OS_MutSemGive(PoolPtr->MutexId);
}
return(CFE_ES_ERR_MEM_BLOCK_SIZE);
}
/*
** Check if any of the requested size are available
*/
if (PoolPtr->SizeDescPtr->Top != NULL) /* Set by GetBlockSize call */
{
/*
** Get it off the top on the list
*/
BdPtr = PoolPtr->SizeDescPtr->Top;
BdPtr->CheckBits = CFE_ES_CHECK_PATTERN;
BdPtr->Allocated = CFE_ES_MEMORY_ALLOCATED; /* Flag memory block as allocated */
BdPtr->Size = Block;
PoolPtr->SizeDescPtr->Top = (BD_t *)BdPtr->Next;
PoolPtr->SizeDescPtr->NumFree--;
BdPtr->Next = NULL;
*BufPtr = (uint32 *)(BdPtr + 1);
}
else /* go make one */
{
BdPtr = (BD_t *)PoolPtr->Current; /* point to new memory block */
if ( ((uint32)BdPtr + sizeof(BD_t) + Block ) >= PoolPtr->End ){ /* can't fit in remaing mem */
CFE_ES_WriteToSysLog("CFE_ES:getPoolBuf err:Request won't fit in remaining memory\n");
if (PoolPtr->UseMutex == CFE_ES_USE_MUTEX)
{
OS_MutSemGive(PoolPtr->MutexId);
}
return(CFE_ES_ERR_MEM_BLOCK_SIZE);
}
PoolPtr->SizeDescPtr->NumCreated++;
PoolPtr->RequestCntr++;
/*
** create the buffer descriptor at the front of it
*/
BdPtr->CheckBits = CFE_ES_CHECK_PATTERN;
BdPtr->Allocated = CFE_ES_MEMORY_ALLOCATED; /* Flag memory block as allocated */
BdPtr->Size = Block;
BdPtr->Next = NULL;
*BufPtr = (uint32 *)(BdPtr + 1);
/*
** adjust pool current pointer
*/
PoolPtr->Current = (uint32 *)( (uint8 *)PoolPtr->Current
+ Block
+ sizeof(BD_t) );
}
if (PoolPtr->UseMutex == CFE_ES_USE_MUTEX)
{
OS_MutSemGive(PoolPtr->MutexId);
}
return (int32)Block;
}
/*
** Name: CFE_ES_SetupResetVariables
**
** Purpose: This function initializes the ES reset variables depending on the reset type.
** It will also initiate a power on reset when too many processor resets
** have happened.
**
*/
void CFE_ES_SetupResetVariables(uint32 StartType, uint32 StartSubtype, uint32 BootSource )
{
int32 status;
uint32 resetAreaSize;
/*
** Get the pointer to the Reset area from the BSP
*/
status = CFE_PSP_GetResetArea (&(CFE_ES_ResetDataPtr), &(resetAreaSize));
/*
** Make sure the status is OK or size is big enough
*/
if ( status == OS_ERROR )
{
/*
** Cannot use the ES System log without the Reset Area
*/
OS_printf("ES Startup: CFE_PSP_GetResetArea call Failed!\n");
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_MEMORY_ALLOC);
}
else if ( resetAreaSize < sizeof(CFE_ES_ResetData_t))
{
/*
** Cannot use the ES system log without the Reset Area
*/
OS_printf("ES Startup: Error: ES Reset area not big enough. Needed: %d, Given: %d.\n",
sizeof(CFE_ES_ResetData_t),
resetAreaSize);
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_MEMORY_ALLOC);
}
/*
** Determine how the system was started. The choices are:
** CFE_ES_POWER_ON_RESET, or CFE_ES_PROCESSOR_RESET
** The subtypes include:
** CFE_ES_POWER_CYCLE, CFE_ES_PUSH_BUTTON, CFE_ES_HW_SPECIAL_COMMAND,
** CFE_ES_HW_WATCHDOG, CFE_ES_RESET_COMMAND, or CFE_ES_EXCEPTION.
*/
if ( StartType == CFE_ES_POWERON_RESET )
{
/*
** Record the reset type and subtype
*/
CFE_ES_ResetDataPtr->ResetVars.ResetSubtype = StartSubtype;
CFE_ES_ResetDataPtr->ResetVars.ResetType = CFE_ES_POWERON_RESET;
/*
** Always log the power-on reset.
*/
status = CFE_ES_WriteToERLog(CFE_ES_CORE_LOG_ENTRY, CFE_ES_POWERON_RESET, StartSubtype,
"ES Startup: POWER ON Reset", NULL,0 );
/*
** Initialize all reset counters.
*/
CFE_ES_ResetDataPtr->ResetVars.ProcessorResetCount = 0;
CFE_ES_ResetDataPtr->ResetVars.MaxProcessorResetCount = CFE_ES_MAX_PROCESSOR_RESETS;
CFE_ES_Global.DebugVars.DebugFlag = 0;
}
else if ( StartType == CFE_ES_PROCESSOR_RESET )
{
/*
** If a Processor reset was not commanded, it must be a watchdog reset.
** Log the reset before updating any reset variables.
*/
if ( CFE_ES_ResetDataPtr->ResetVars.ES_CausedReset != TRUE )
{
CFE_ES_ResetDataPtr->ResetVars.ResetType = CFE_ES_PROCESSOR_RESET;
CFE_ES_ResetDataPtr->ResetVars.ResetSubtype = CFE_ES_HW_WATCHDOG;
/*
** Log the watchdog reset
*/
status = CFE_ES_WriteToERLog(CFE_ES_CORE_LOG_ENTRY, CFE_ES_PROCESSOR_RESET, StartSubtype,
"ES Startup: PROCESSOR RESET due to Watchdog.", NULL,0 );
/*
** When coming up from a Processor reset that was not caused by ES, check to see
** if the maximum number has been exceeded
*/
if ( CFE_ES_ResetDataPtr->ResetVars.ProcessorResetCount >=
CFE_ES_ResetDataPtr->ResetVars.MaxProcessorResetCount )
{
CFE_ES_WriteToSysLog("ES Startup: CFE ES Power On Reset Due to Max Processor Resets.\n");
/*
** Log the reset in the ER Log. The log will be wiped out, but it's good to have
** the entry just in case something fails.
*/
status = CFE_ES_WriteToERLog(CFE_ES_CORE_LOG_ENTRY, CFE_ES_POWERON_RESET, StartSubtype,
"ES Startup: POWER ON RESET due to Maximum Processor Resets in ES Startup.", NULL,0 );
/*
** Call the BSP reset routine
*/
CFE_PSP_Restart(CFE_ES_POWERON_RESET);
/*
** Should not return here.
*/
CFE_ES_WriteToSysLog("ES Startup: Error: CFE_PSP_Restart returned.\n");
}
else
{
/*
** Increment the Processor Reset Count after the check to see
** if there are too many processor resets. This keeps the logic consistent with
** the resets that are caused by the cFE ( command or exception )
*/
CFE_ES_ResetDataPtr->ResetVars.ProcessorResetCount++;
} /* end if */
}
/*
** If a processor reset was commanded, the reset has already been logged.
** Update the reset variables only.
** The logic for detecting maximum resets is done on the command side
** on the "way down", or when the command is executed.
*/
else
{
CFE_ES_ResetDataPtr->ResetVars.ResetType = CFE_ES_PROCESSOR_RESET;
CFE_ES_ResetDataPtr->ResetVars.ResetSubtype = StartSubtype;
}
/*
** Initialize processor reset counters.
*/
CFE_ES_Global.DebugVars.DebugFlag = 0;
}
/*
** Clear the commanded reset flag, in case a watchdog happens.
*/
CFE_ES_ResetDataPtr->ResetVars.ES_CausedReset = FALSE;
CFE_ES_ResetDataPtr->ResetVars.BootSource = BootSource;
}
int32 CFE_TBL_TaskInit(void)
{
int32 Status = CFE_SUCCESS;
/*
** Register Table Services with ES
*/
Status = CFE_ES_RegisterApp();
if(Status != CFE_SUCCESS)
{
CFE_ES_WriteToSysLog("TBL:Call to CFE_ES_RegisterApp Failed:RC=0x%08X\n",Status);
return Status;
}/* end if */
/*
** Initialize global Table Services data
*/
CFE_TBL_InitData();
/*
** Register event filter table
*/
Status = CFE_EVS_Register(NULL, 0, 0);
if(Status != CFE_SUCCESS)
{
CFE_ES_WriteToSysLog("TBL:Call to CFE_EVS_Register Failed:RC=0x%08X\n",Status);
return Status;
}/* end if */
/*
** Create Software Bus message pipe
*/
Status = CFE_SB_CreatePipe(&CFE_TBL_TaskData.CmdPipe,
CFE_TBL_TaskData.PipeDepth,
CFE_TBL_TaskData.PipeName);
if(Status != CFE_SUCCESS)
{
CFE_ES_WriteToSysLog("TBL:Error creating cmd pipe:RC=0x%08X\n",Status);
return Status;
}/* end if */
/*
** Subscribe to Housekeeping request commands
*/
Status = CFE_SB_Subscribe(CFE_TBL_SEND_HK_MID, CFE_TBL_TaskData.CmdPipe);
if(Status != CFE_SUCCESS)
{
CFE_ES_WriteToSysLog("TBL:Error subscribing to HK Request:RC=0x%08X\n",Status);
return Status;
}/* end if */
/*
** Subscribe to Table task ground command packets
*/
Status = CFE_SB_Subscribe(CFE_TBL_CMD_MID, CFE_TBL_TaskData.CmdPipe);
if(Status != CFE_SUCCESS)
{
CFE_ES_WriteToSysLog("TBL:Error subscribing to gnd cmds:RC=0x%08X\n",Status);
return Status;
}/* end if */
/*
** Task startup event message
*/
Status = CFE_EVS_SendEvent(CFE_TBL_INIT_INF_EID, CFE_EVS_INFORMATION, "cFE TBL Initialized. cFE Version %d.%d.%d.%d",
CFE_MAJOR_VERSION,CFE_MINOR_VERSION,CFE_REVISION,CFE_MISSION_REV);
if(Status != CFE_SUCCESS)
{
CFE_ES_WriteToSysLog("TBL:Error sending init event:RC=0x%08X\n",Status);
return Status;
}/* end if */
return CFE_SUCCESS;
} /* End of CFE_TBL_TaskInit() */
/*
** Name: CFE_ES_InitializeFileSystems
**
** Purpose: This function initializes the file systems used in the cFE core.
**
*/
void CFE_ES_InitializeFileSystems(uint32 start_type)
{
int32 RetStatus;
uint32 *RamDiskMemoryAddress;
uint32 RamDiskMemorySize;
int32 BlocksFree;
int32 PercentFree;
/*
** Get the memory area for the RAM disk
*/
RetStatus = CFE_PSP_GetVolatileDiskMem(&(RamDiskMemoryAddress), &(RamDiskMemorySize));
if ( RetStatus != OS_FS_SUCCESS )
{
CFE_ES_WriteToSysLog("ES Startup: Cannot Get Memory for Volatile Disk. EC = 0x%08X\n",RetStatus);
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_VOLATILE_DISK);
}
/*
** Next, either format, or just initialize the RAM disk depending on
** the reset type
*/
if ( start_type == CFE_ES_POWERON_RESET )
{
RetStatus = OS_mkfs((void *)RamDiskMemoryAddress, "/ramdev0", "RAM", CFE_ES_RAM_DISK_SECTOR_SIZE, CFE_ES_RAM_DISK_NUM_SECTORS );
if ( RetStatus != OS_FS_SUCCESS )
{
CFE_ES_WriteToSysLog("ES Startup: Error Creating Volatile(RAM) Volume. EC = 0x%08X\n",RetStatus);
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_VOLATILE_DISK);
}
}
else
{
RetStatus = OS_initfs((void *)RamDiskMemoryAddress, "/ramdev0", "RAM", CFE_ES_RAM_DISK_SECTOR_SIZE, CFE_ES_RAM_DISK_NUM_SECTORS );
if ( RetStatus != OS_FS_SUCCESS )
{
CFE_ES_WriteToSysLog("ES Startup: Error Initializing Volatile(RAM) Volume. EC = 0x%08X\n",RetStatus);
CFE_ES_WriteToSysLog("ES Startup: Formatting Volatile(RAM) Volume.\n");
RetStatus = OS_mkfs((void *)RamDiskMemoryAddress, "/ramdev0", "RAM", CFE_ES_RAM_DISK_SECTOR_SIZE, CFE_ES_RAM_DISK_NUM_SECTORS );
if ( RetStatus != OS_SUCCESS )
{
CFE_ES_WriteToSysLog("ES Startup: Error Creating Volatile(RAM) Volume. EC = 0x%08X\n",RetStatus);
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_VOLATILE_DISK);
}
}
}
/*
** Now, mount the RAM disk
*/
RetStatus = OS_mount("/ramdev0", CFE_ES_RAM_DISK_MOUNT_STRING);
if ( RetStatus != OS_FS_SUCCESS )
{
CFE_ES_WriteToSysLog("ES Startup: Error Mounting Volatile(RAM) Volume. EC = 0x%08X\n",RetStatus);
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_VOLATILE_DISK);
}
/*
** During a Processor reset, if the RAM disk has less than a defined
** amount of free space, reformat and re-mount it.
** The parameter being checked is CFE_ES_RAM_DISK_PERCENT_RESERVED
** Note: When CFE_ES_RAM_DISK_PERCENT_RESERVED is set to 0, this feature is
** disabled.
*/
if ((start_type == CFE_ES_PROCESSOR_RESET) && (CFE_ES_RAM_DISK_PERCENT_RESERVED > 0))
{
/*
** See how many blocks are free in the RAM disk
*/
BlocksFree = OS_fsBlocksFree(CFE_ES_RAM_DISK_MOUNT_STRING);
if ( BlocksFree >= 0 )
{
/*
** Need a sanity check for the desktop systems.
** Because the desktop ports map the volatile disk to the host
** hard disk, it will report more free blocks than the defined number
** of sectors ( blocks ). Therefore it must be truncated.
*/
if ( BlocksFree > CFE_ES_RAM_DISK_NUM_SECTORS )
{
BlocksFree = CFE_ES_RAM_DISK_NUM_SECTORS - 1;
}
/*
** Determine if the disk is too full
*/
BlocksFree = BlocksFree * 100;
PercentFree = BlocksFree / CFE_ES_RAM_DISK_NUM_SECTORS;
CFE_ES_WriteToSysLog("Volatile Disk has %d Percent free space.\n",PercentFree);
if ( PercentFree < CFE_ES_RAM_DISK_PERCENT_RESERVED )
{
CFE_ES_WriteToSysLog("ES Startup: Insufficent Free Space on Volatile Disk, Reformatting.\n");
/*
** First, unmount the disk
*/
RetStatus = OS_unmount(CFE_ES_RAM_DISK_MOUNT_STRING);
if ( RetStatus == OS_FS_SUCCESS )
{
/*
** Remove the file system from the OSAL
*/
RetStatus = OS_rmfs("/ramdev0");
if ( RetStatus == OS_FS_SUCCESS )
{
/*
** Next, make a new file system on the disk
*/
RetStatus = OS_mkfs((void *)RamDiskMemoryAddress, "/ramdev0",
"RAM", CFE_ES_RAM_DISK_SECTOR_SIZE,
CFE_ES_RAM_DISK_NUM_SECTORS );
if ( RetStatus == OS_FS_SUCCESS )
{
/*
** Last, remount the disk
*/
RetStatus = OS_mount("/ramdev0", CFE_ES_RAM_DISK_MOUNT_STRING);
if ( RetStatus != OS_FS_SUCCESS )
{
CFE_ES_WriteToSysLog("ES Startup: Error Re-Mounting Volatile(RAM) Volume. EC = 0x%08X\n",RetStatus);
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_VOLATILE_DISK);
} /* end if mount */
}
else
{
CFE_ES_WriteToSysLog("ES Startup: Error Re-Formating Volatile(RAM) Volume. EC = 0x%08X\n",RetStatus);
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_VOLATILE_DISK);
} /* end if mkfs */
}
else /* could not Remove File system */
{
CFE_ES_WriteToSysLog("ES Startup: Error Removing Volatile(RAM) Volume. EC = 0x%08X\n",RetStatus);
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_VOLATILE_DISK);
} /* end if OS_rmfs */
}
else /* could not un-mount disk */
{
CFE_ES_WriteToSysLog("ES Startup: Error Un-Mounting Volatile(RAM) Volume. EC = 0x%08X\n",RetStatus);
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_VOLATILE_DISK);
}
} /* end if enough free space */
}
else /* could not determine free blocks */
{
/* Log error message -- note that BlocksFree returns the error code in this case */
CFE_ES_WriteToSysLog("ES Startup: Error Determining Blocks Free on Volume. EC = 0x%08X\n",BlocksFree);
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_VOLATILE_DISK);
} /* end if BlocksFree */
} /* end if processor reset */
} /* end function */
void SC_AppMain(void)
{
uint32 RunStatus = CFE_ES_APP_RUN;
int32 Result;
/* Register application with cFE */
Result = CFE_ES_RegisterApp();
/* Performance Log (start time counter) */
CFE_ES_PerfLogEntry(SC_APPMAIN_PERF_ID);
/* Startup initialization */
if (Result == CFE_SUCCESS)
{
Result = SC_AppInit();
}
/* Check for start-up error */
if (Result != CFE_SUCCESS)
{
/* Set request to terminate main loop */
RunStatus = CFE_ES_APP_ERROR;
}
/* Main process loop */
while (CFE_ES_RunLoop(&RunStatus))
{
/* Performance Log (stop time counter) */
CFE_ES_PerfLogExit(SC_APPMAIN_PERF_ID);
/* Pend on Software Bus for message */
Result = CFE_SB_RcvMsg(&SC_OperData.MsgPtr, SC_OperData.CmdPipe, CFE_SB_PEND_FOREVER);
/* Performance Log (start time counter) */
CFE_ES_PerfLogEntry(SC_APPMAIN_PERF_ID);
/* Check for Software Bus error */
if (Result == CFE_SUCCESS)
{
/* Invoke command handlers */
SC_ProcessRequest(SC_OperData.MsgPtr);
}
else
{
/* Exit main process loop */
RunStatus = CFE_ES_APP_ERROR;
}
}
/* Check for "fatal" process error */
if (Result != CFE_SUCCESS)
{
/* Send event describing reason for termination */
CFE_EVS_SendEvent(SC_APP_EXIT_ERR_EID, CFE_EVS_ERROR,
"App terminating, Result = 0x%08X", Result);
/* In case cFE Event Services is not working */
CFE_ES_WriteToSysLog("SC App terminating, Result = 0x%08X\n", Result);
}
/* Performance Log (stop time counter) */
CFE_ES_PerfLogExit(SC_APPMAIN_PERF_ID);
/* Let cFE kill the app */
CFE_ES_ExitApp(RunStatus);
} /* end SC_AppMain() */
/*-------------------------------------------------------------------------
**
** Functional Prolog
**
** Name: CFE_ES_CreateObjects
**
** Purpose: This function reads the es_object_table and performs all of the
** application layer initialization.
**----------------------------------------------------------------------------
*/
void CFE_ES_CreateObjects(void)
{
int32 ReturnCode;
boolean AppSlotFound;
int16 i;
int16 j;
CFE_ES_WriteToSysLog("ES Startup: Starting Object Creation calls.\n");
for ( i = 0; i < CFE_ES_OBJECT_TABLE_SIZE; i++ )
{
switch ( CFE_ES_ObjectTable[i].ObjectType )
{
case CFE_ES_DRIVER_TASK:
case CFE_ES_CORE_TASK:
/*
** Allocate an ES AppTable entry
*/
AppSlotFound = FALSE;
for ( j = 0; j < CFE_ES_MAX_APPLICATIONS; j++ )
{
if ( CFE_ES_Global.AppTable[j].RecordUsed == FALSE )
{
AppSlotFound = TRUE;
break;
}
}
/*
** If a slot was found, create the application
*/
if ( AppSlotFound == TRUE )
{
CFE_ES_LockSharedData(__func__,__LINE__);
/*
** Allocate and populate the ES_AppTable entry
*/
CFE_PSP_MemSet ( (void *)&(CFE_ES_Global.AppTable[j]), 0, sizeof(CFE_ES_AppRecord_t));
CFE_ES_Global.AppTable[j].RecordUsed = TRUE;
CFE_ES_Global.AppTable[j].Type = CFE_ES_APP_TYPE_CORE;
/*
** Fill out the parameters in the AppStartParams sub-structure
*/
strncpy((char *)CFE_ES_Global.AppTable[j].StartParams.Name, (char *)CFE_ES_ObjectTable[i].ObjectName, OS_MAX_API_NAME);
CFE_ES_Global.AppTable[j].StartParams.Name[OS_MAX_API_NAME - 1] = '\0';
/* EntryPoint field is not valid here for base apps */
/* FileName is not valid for base apps, either */
CFE_ES_Global.AppTable[j].StartParams.StackSize = CFE_ES_ObjectTable[i].ObjectSize;
CFE_ES_Global.AppTable[j].StartParams.StartAddress = (uint32)CFE_ES_ObjectTable[i].FuncPtrUnion.FunctionPtr;
CFE_ES_Global.AppTable[j].StartParams.ExceptionAction = CFE_ES_APP_EXCEPTION_PROC_RESTART;
CFE_ES_Global.AppTable[j].StartParams.Priority = CFE_ES_ObjectTable[i].ObjectPriority;
/*
** Fill out the Task Info
*/
strncpy((char *)CFE_ES_Global.AppTable[j].TaskInfo.MainTaskName, (char *)CFE_ES_ObjectTable[i].ObjectName, OS_MAX_API_NAME);
CFE_ES_Global.AppTable[j].TaskInfo.MainTaskName[OS_MAX_API_NAME - 1] = '\0';
CFE_ES_Global.AppTable[j].TaskInfo.NumOfChildTasks = 0;
/*
** Since this is a Core app, the AppStateRecord does not need to be filled out.
*/
/*
** Create the task
*/
ReturnCode = OS_TaskCreate(&CFE_ES_Global.AppTable[j].TaskInfo.MainTaskId, /* task id */
CFE_ES_ObjectTable[i].ObjectName, /* task name */
CFE_ES_ObjectTable[i].FuncPtrUnion.VoidPtr, /* task function pointer */
NULL, /* stack pointer */
CFE_ES_ObjectTable[i].ObjectSize, /* stack size */
CFE_ES_ObjectTable[i].ObjectPriority, /* task priority */
OS_FP_ENABLED); /* task options */
if(ReturnCode != OS_SUCCESS)
{
CFE_ES_Global.AppTable[j].RecordUsed = FALSE;
CFE_ES_WriteToSysLog("ES Startup: OS_TaskCreate error creating core App: %s: EC = 0x%08X\n",
CFE_ES_ObjectTable[i].ObjectName, ReturnCode);
CFE_ES_UnlockSharedData(__func__,__LINE__);
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_CORE_APP);
}
else
{
CFE_ES_ObjectTable[i].ObjectKey = CFE_ES_Global.AppTable[j].TaskInfo.MainTaskId;
/*
** Allocate and populate the CFE_ES_Global.TaskTable entry
*/
if ( CFE_ES_Global.TaskTable[CFE_ES_Global.AppTable[j].TaskInfo.MainTaskId].RecordUsed == TRUE )
{
CFE_ES_WriteToSysLog("ES Startup: CFE_ES_Global.TaskTable record used error for App: %s, continuing.\n",
CFE_ES_ObjectTable[i].ObjectName);
}
else
{
CFE_ES_Global.TaskTable[CFE_ES_Global.AppTable[j].TaskInfo.MainTaskId].RecordUsed = TRUE;
}
CFE_ES_Global.TaskTable[CFE_ES_Global.AppTable[j].TaskInfo.MainTaskId].AppId = j;
CFE_ES_Global.TaskTable[CFE_ES_Global.AppTable[j].TaskInfo.MainTaskId].TaskId = CFE_ES_Global.AppTable[j].TaskInfo.MainTaskId;
strncpy((char *)CFE_ES_Global.TaskTable[CFE_ES_Global.AppTable[j].TaskInfo.MainTaskId].TaskName, (char *)CFE_ES_Global.AppTable[j].TaskInfo.MainTaskName, OS_MAX_API_NAME);
CFE_ES_Global.TaskTable[CFE_ES_Global.AppTable[j].TaskInfo.MainTaskId].TaskName[OS_MAX_API_NAME - 1] = '\0';
CFE_ES_WriteToSysLog("ES Startup: Core App: %s created. App ID: %d\n",
CFE_ES_ObjectTable[i].ObjectName,j);
/*
** Increment the registered App and Registered External Task variables.
*/
CFE_ES_Global.RegisteredTasks++;
CFE_ES_Global.RegisteredCoreApps++;
CFE_ES_UnlockSharedData(__func__,__LINE__);
}
}
else /* appSlot not found -- This should never happen!*/
{
CFE_ES_WriteToSysLog("ES Startup: Error, No free application slots available for CORE App!\n");
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_CORE_APP);
}
break;
case CFE_ES_FUNCTION_CALL: /*----------------------------------------------------------*/
if ( CFE_ES_ObjectTable[i].FuncPtrUnion.FunctionPtr != NULL )
{
CFE_ES_WriteToSysLog("ES Startup: Calling %s\n",CFE_ES_ObjectTable[i].ObjectName);
/*
** Call the function
*/
ReturnCode = (*CFE_ES_ObjectTable[i].FuncPtrUnion.FunctionPtr)();
if(ReturnCode != CFE_SUCCESS)
{
CFE_ES_WriteToSysLog("ES Startup: Error returned when calling function: %s: EC = 0x%08X\n",
CFE_ES_ObjectTable[i].ObjectName, ReturnCode);
/*
** Delay to allow the message to be read
*/
OS_TaskDelay(CFE_ES_PANIC_DELAY);
/*
** cFE Cannot continue to start up.
*/
CFE_PSP_Panic(CFE_PSP_PANIC_CORE_APP);
} /* end if */
}
else
{
CFE_ES_WriteToSysLog("ES Startup: bad function pointer ( table entry = %d).\n",i);
}
break;
case CFE_ES_NULL_ENTRY: /*-------------------------------------------------------*/
break;
default:
break;
} /* end switch */
} /* end for */
CFE_ES_WriteToSysLog("ES Startup: Finished ES CreateObject table entries.\n");
}
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void CFE_ES_PerfLogAdd(uint32 Marker, uint32 EntryExit)
{
int32 IntFlags;
if (Perf->MetaData.State != CFE_ES_PERF_IDLE) {
/* if marker is out of range... */
if(Marker >= CFE_ES_PERF_MAX_IDS){
/* if marker has not been reported previously ... */
if(Perf->MetaData.InvalidMarkerReported == FALSE){
CFE_ES_WriteToSysLog("ES PERF:Invalid performance marker %d,max is %d\n",
Marker,(CFE_ES_PERF_MAX_IDS - 1));
Perf->MetaData.InvalidMarkerReported = TRUE;
}/* end if */
return;
}/* end if */
/* is this id filtered */
if (CFE_ES_TEST_LONG_MASK(Perf->MetaData.FilterMask, Marker)) {
/* disable interrupts to guarentee exclusive access to the data structures */
IntFlags = OS_IntLock();
Perf->DataBuffer[Perf->MetaData.DataEnd].Data = (Marker | (EntryExit << CFE_ES_PERF_EXIT_BIT));
CFE_PSP_Get_Timebase((uint32*)&Perf->DataBuffer[Perf->MetaData.DataEnd].TimerUpper32,(uint32*)&Perf->DataBuffer[Perf->MetaData.DataEnd].TimerLower32);
Perf->MetaData.DataEnd++;
if (Perf->MetaData.DataEnd >= CFE_ES_PERF_DATA_BUFFER_SIZE) {
Perf->MetaData.DataEnd = 0;
}
/* we have filled up the buffer */
Perf->MetaData.DataCount++;
if (Perf->MetaData.DataCount > CFE_ES_PERF_DATA_BUFFER_SIZE) {
Perf->MetaData.DataCount = CFE_ES_PERF_DATA_BUFFER_SIZE;
/* after the buffer fills up start and end point to the same entry since we
are now overwriting old data */
Perf->MetaData.DataStart = Perf->MetaData.DataEnd;
}
/* waiting for trigger */
if (Perf->MetaData.State == CFE_ES_PERF_WAITING_FOR_TRIGGER) {
if (CFE_ES_TEST_LONG_MASK(Perf->MetaData.TriggerMask, Marker)) {
Perf->MetaData.State = CFE_ES_PERF_TRIGGERED;
}
}
/* triggered */
if (Perf->MetaData.State == CFE_ES_PERF_TRIGGERED) {
Perf->MetaData.TriggerCount++;
if (Perf->MetaData.Mode == CFE_ES_PERF_TRIGGER_START) {
if (Perf->MetaData.TriggerCount >= CFE_ES_PERF_DATA_BUFFER_SIZE) {
Perf->MetaData.State = CFE_ES_PERF_IDLE;
}
}
else if (Perf->MetaData.Mode == CFE_ES_PERF_TRIGGER_CENTER) {
if (Perf->MetaData.TriggerCount >= CFE_ES_PERF_DATA_BUFFER_SIZE / 2) {
Perf->MetaData.State = CFE_ES_PERF_IDLE;
}
}
else if (Perf->MetaData.Mode == CFE_ES_PERF_TRIGGER_END) {
Perf->MetaData.State = CFE_ES_PERF_IDLE;
}
}
/* enable interrupts */
OS_IntUnlock(IntFlags);
}
}
}/* end CFE_ES_PerfLogAdd */
/*
** Function:
** CFE_ES_GetPoolBuf
**
** Purpose:
** CFE_ES_GetPoolBuf allocates a block from the memory pool.
*/
int32 CFE_ES_GetPoolBuf(uint32 **BufPtr,
CFE_ES_MemHandle_t HandlePtr,
uint32 Size )
{
Pool_t * PoolPtr = (Pool_t *)HandlePtr;
uint32 Block;
BD_t * BdPtr;
uint32 AppId = 0xFFFFFFFF;
/* OS_MutSemTake(PoolPtr->MutexId); */
/* Handle Preset Return Code */
if (cfe_es_api_return_value[CFE_ES_GETPOOLBUF_PROC] != UTF_CFE_USE_DEFAULT_RETURN_CODE)
{
return cfe_es_api_return_value[CFE_ES_GETPOOLBUF_PROC];
}
/* Handle Function Hook */
if (UTF_ES_HookTable.CFE_ES_GetPoolBuf)
return(UTF_ES_HookTable.CFE_ES_GetPoolBuf(BufPtr, HandlePtr, Size));
*BufPtr = NULL;
CFE_ES_GetAppID(&AppId);
if (HandlePtr != (CFE_ES_MemHandle_t)PoolPtr->Start)
{
CFE_ES_WriteToSysLog("CFE_ES:getPoolBuf err:Bad handle.AppId=%d\n",AppId);
/* OS_MutSemGive(PoolPtr->MutexId); */
return(CFE_ES_ERR_MEM_HANDLE);
}
Block = CFE_ES_GetBlockSize(PoolPtr, Size);
if (Block == 0xFFFFFFFF)
{
CFE_ES_WriteToSysLog("CFE_ES:getPoolBuf err:size(%d) > max(%d).\n",Size,CFE_ES_MAX_BLOCK_SIZE);
/* OS_MutSemGive(PoolPtr->MutexId); */
return(CFE_ES_ERR_MEM_BLOCK_SIZE);
}
/*
** Check if any of the requested size are available
*/
if (PoolPtr->SizeDescPtr->Top != NULL) /* Set by CFE_ES_GetBlockSize call */
{
/*
** Get it off the top on the list
*/
BdPtr = PoolPtr->SizeDescPtr->Top;
BdPtr->CheckBits = CFE_ES_CHECK_PATTERN;
BdPtr->Allocated = CFE_ES_MEMORY_ALLOCATED; /* Flag memory block as allocated */
BdPtr->Size = Block;
PoolPtr->SizeDescPtr->Top = (BD_t *)BdPtr->Next;
PoolPtr->SizeDescPtr->NumFree--;
BdPtr->Next = NULL;
*BufPtr = (uint32 *)(BdPtr + 1);
}
else /* go make one */
{
BdPtr = (BD_t *)PoolPtr->Current; /* point to new memory block */
if (((uint32)BdPtr + sizeof(BD_t) + Block) >= PoolPtr->End )
{ /* can't fit in remaing mem */
CFE_ES_WriteToSysLog("CFE_ES:getPoolBuf err:Request won't fit in remaining memory\n");
/* OS_MutSemGive(PoolPtr->MutexId); */
return(CFE_ES_ERR_MEM_BLOCK_SIZE);
}
PoolPtr->SizeDescPtr->NumCreated++;
PoolPtr->RequestCntr++;
/*
** create the buffer descriptor at the front of it
*/
BdPtr->CheckBits = CFE_ES_CHECK_PATTERN;
BdPtr->Allocated = CFE_ES_MEMORY_ALLOCATED; /* Flag memory block as allocated */
BdPtr->Size = Block;
BdPtr->Next = NULL;
*BufPtr = (uint32 *)(BdPtr + 1);
/*
** adjust pool current pointer
*/
PoolPtr->Current = (uint32 *)( (uint8 *)PoolPtr->Current
+ Block
+ sizeof(BD_t) );
}
/* OS_MutSemGive(PoolPtr->MutexId); */
return (int32)Block;
}
