int32 CFE_ES_InitCDSRegistry(void)
{
int32 Status = CFE_SUCCESS;
uint32 i = 0;
/* Initialize the local CDS Registry */
CFE_ES_Global.CDSVars.MaxNumRegEntries = CFE_ES_CDS_MAX_NUM_ENTRIES;
for (i=0; i<CFE_ES_Global.CDSVars.MaxNumRegEntries; i++)
{
CFE_ES_Global.CDSVars.Registry[i].Name[0] = '\0';
CFE_ES_Global.CDSVars.Registry[i].Size = 0;
CFE_ES_Global.CDSVars.Registry[i].MemHandle = 0;
CFE_ES_Global.CDSVars.Registry[i].Taken = FALSE;
CFE_ES_Global.CDSVars.Registry[i].Table = FALSE;
}
/* Copy the number of registry entries to the CDS */
Status = CFE_PSP_WriteToCDS(&CFE_ES_Global.CDSVars.MaxNumRegEntries,
CDS_REG_SIZE_OFFSET,
sizeof(CFE_ES_Global.CDSVars.MaxNumRegEntries));
if (Status == OS_SUCCESS)
{
Status = CFE_ES_UpdateCDSRegistry();
}
else
{
CFE_ES_WriteToSysLog("CFE_CDS:InitReg-Failed to write Reg Size. Status=0x%08X\n", Status);
}
return Status;
} /* End of CFE_ES_InitCDSRegistry() */
int32 CFE_ES_UpdateCDSRegistry(void)
{
int32 Status = CFE_SUCCESS;
/* Copy the contents of the local registry to the CDS */
Status = CFE_PSP_WriteToCDS(CFE_ES_Global.CDSVars.Registry,
CDS_REG_OFFSET,
sizeof(CFE_ES_Global.CDSVars.Registry));
if (Status != OS_SUCCESS)
{
CFE_ES_WriteToSysLog("CFE_CDS:UpdateReg-Failed to write CDS Registry. Status=0x%08X\n", Status);
}
return Status;
}
int32 CFE_ES_InitializeCDS(uint32 CDSSize)
{
int32 Status = CFE_SUCCESS;
uint32 MemBlock[32];
uint32 NumWritten=0;
uint32 Uint32Zero=0;
/* Clear the CDS to ensure everything is gone */
/* Create a block of zeros to write to the CDS */
CFE_PSP_MemSet(MemBlock, 0, sizeof(MemBlock));
/* While there is space to write another block of zeros, then do so */
while (((NumWritten + sizeof(MemBlock)) < CDSSize) && (Status == OS_SUCCESS))
{
Status = CFE_PSP_WriteToCDS(MemBlock, NumWritten, sizeof(MemBlock));
if (Status == OS_SUCCESS)
{
NumWritten += sizeof(MemBlock);
}
}
/* While there is space to write a uint32 of zeros, then do so */
if ((Status == OS_SUCCESS) && (NumWritten < CDSSize))
{
while (((NumWritten + sizeof(uint32)) < CDSSize) && (Status == OS_SUCCESS))
{
Status = CFE_PSP_WriteToCDS(&Uint32Zero, NumWritten, sizeof(uint32));
if (Status == OS_SUCCESS)
{
NumWritten += sizeof(uint32);
}
}
}
if (Status != OS_SUCCESS)
{
CFE_ES_WriteToSysLog("CFE_CDS:Init-Clear CDS failed @ Offset=%d Status=0x%08X\n", NumWritten, Status);
}
else
{
/* Initialize the Validity Check strings */
Status = CFE_PSP_WriteToCDS("_CDSBeg_", 0, sizeof(CFE_ES_Global.CDSVars.ValidityField));
if (Status == OS_SUCCESS)
{
Status = CFE_PSP_WriteToCDS("_CDSEnd_",
(CDSSize-sizeof(CFE_ES_Global.CDSVars.ValidityField)),
sizeof(CFE_ES_Global.CDSVars.ValidityField));
if (Status != OS_SUCCESS)
{
CFE_ES_WriteToSysLog("CFE_CDS:Init-'_CDSEnd_' write failed. Status=0x%08X\n", Status);
}
else
{
/* Determine the amount of CDS memory is available for the CDS Memory Pool */
/* Total CDS Size minus the validity fields and the space for the registry and the registry size */
CFE_ES_Global.CDSVars.MemPoolSize = CDSSize - CDS_POOL_OFFSET - sizeof(CFE_ES_Global.CDSVars.ValidityField);
/* Initialize the variables for managing the CDS Memory Pool */
Status = CFE_ES_CreateCDSPool(CFE_ES_Global.CDSVars.MemPoolSize, CDS_POOL_OFFSET);
if (Status == CFE_SUCCESS)
{
Status = CFE_ES_InitCDSRegistry();
}
}
}
else /* BSP reported an error writing to CDS */
{
CFE_ES_WriteToSysLog("CFE_CDS:Init-'_CDSBeg_' write failed. Status=0x%08X\n", Status);
}
}
return Status;
} /* End of CFE_ES_InitializeCDS() */
/*
** 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;
}
/*
** 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;
}
