/*
** CFE_ES_GetPoolBufInfo gets the size of the specified block (if it exists).
*/
int32 CFE_ES_GetPoolBufInfo(CFE_ES_MemHandle_t Handle,
uint32 * BufPtr)
{
Pool_t *PoolPtr = (Pool_t *)Handle;
BD_t *BdPtr = (BD_t *) ((uint8 *)BufPtr - sizeof(BD_t));
if (PoolPtr != NULL)
{
if ( ((uint32)BdPtr < Handle) || ((uint32)BdPtr >= (PoolPtr->End - sizeof(BD_t))) )
{
/* sanity check */
return(CFE_ES_BUFFER_NOT_IN_POOL);
}
}
else
{
/* bad handle */
return(CFE_ES_ERR_MEM_HANDLE);
}
if (PoolPtr->UseMutex == CFE_ES_USE_MUTEX)
{
OS_MutSemTake(PoolPtr->MutexId);
}
/*
** Simple sanity checks for descriptor
*/
/* If a block is no longer allocated, report an error */
if (BdPtr->Allocated != CFE_ES_MEMORY_ALLOCATED)
{
if (PoolPtr->UseMutex == CFE_ES_USE_MUTEX)
{
OS_MutSemGive(PoolPtr->MutexId);
}
return(CFE_ES_ERR_MEM_HANDLE);
}
if (BdPtr->CheckBits != CFE_ES_CHECK_PATTERN)
{
if (PoolPtr->UseMutex == CFE_ES_USE_MUTEX)
{
OS_MutSemGive(PoolPtr->MutexId);
}
return(CFE_ES_ERR_MEM_HANDLE);
}
if (PoolPtr->UseMutex == CFE_ES_USE_MUTEX)
{
OS_MutSemGive(PoolPtr->MutexId);
}
return ((int32) (BdPtr->Size));
}
static void test_006_003_execute(void) {
/* OS_MutSemTake() is invoked with sem_id set to -1, an error is
expected.*/
test_set_step(1);
{
int32 err;
err = OS_MutSemTake((uint32)-1);
test_assert(err == OS_ERR_INVALID_ID, "invalid sem_id not detected");
}
}
int32 CFE_ES_LockCDSRegistry(void)
{
int32 Status;
Status = OS_MutSemTake(CFE_ES_Global.CDSVars.RegistryMutex);
if (Status == OS_SUCCESS)
{
Status = CFE_SUCCESS;
}
return Status;
} /* End of CFE_ES_LockCDSRegistry() */
/******************************************************************************
** Function: CFE_FS_LockSharedData()
**
** Purpose:
** FS internal function to handle a semaphore take failure for the FS
** Data Mutex
**
** Arguments:
** FunctionName - the Function Name of the code that generated the error.
**
** Return:
** None
*/
void CFE_FS_LockSharedData(const char *FunctionName)
{
int32 Status;
uint32 AppId = 0;
Status = OS_MutSemTake(CFE_FS.SharedDataMutexId);
if (Status != OS_SUCCESS)
{
CFE_ES_GetAppID(&AppId);
CFE_ES_WriteToSysLog("FS SharedData Mutex Take Err Stat=0x%x,App=%d,Function=%s\n",
Status,AppId,FunctionName);
}/* end if */
return;
}/* end CFE_FS_LockSharedData */
/******************************************************************************
** Function: CFE_SB_LockSharedData()
**
** Purpose:
** SB internal function to handle a semaphore take failure for the Shared
** Data Mutex
**
** Arguments:
** FuncName - the function name containing the code that generated the error.
** LineNumber - the line number in the file of the code that generated the error.
**
** Return:
** None
*/
void CFE_SB_LockSharedData(const char *FuncName, int32 LineNumber){
int32 Status;
uint32 AppId = 0xFFFFFFFF;
Status = OS_MutSemTake(CFE_SB.SharedDataMutexId);
if (Status != OS_SUCCESS) {
CFE_ES_GetAppID(&AppId);
CFE_ES_WriteToSysLog("SB SharedData Mutex Take Err Stat=0x%x,App=%d,Func=%s,Line=%d\n",
Status,AppId,FuncName,LineNumber);
}/* end if */
return;
}/* end CFE_SB_LockSharedData */
/*
** CFE_ES_CreateCDSPool will initialize a pre-allocated memory pool.
*/
int32 CFE_ES_CreateCDSPool(uint32 CDSPoolSize, uint32 StartOffset)
{
char MutexName[10] = {"CDS_POOL"};
uint32 i = 0;
uint32 Size = (CDSPoolSize & 0xfffffffc);
/* 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 = StartOffset;
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:CreateCDSPool-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 creation failed */
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
OS_MutSemDelete(CFE_ES_CDSMemPool.MutexId);
return(CFE_ES_BAD_ARGUMENT);
}
OS_MutSemGive(CFE_ES_CDSMemPool.MutexId);
return(CFE_SUCCESS);
}
void task_2(void)
{
uint32 status;
printf("Starting task 2\n");
OS_TaskRegister();
while(1)
{
status = OS_MutSemTake(mutex_id);
if ( status != OS_SUCCESS )
{
printf("TASK 2:Error calling OS_MutSemTake\n");
}
shared_resource_x = task_2_id;
status = OS_QueuePut(msgq_id, (void*)&shared_resource_x, sizeof(uint32), 0);
if ( status != OS_SUCCESS )
{
printf("TASK 2:Error calling OS_QueuePut (1)\n");
}
OS_TaskDelay(150);
shared_resource_x = task_2_id;
status = OS_QueuePut(msgq_id, (void*)&shared_resource_x, sizeof(uint32), 0);
if ( status != OS_SUCCESS )
{
printf("TASK 2:Error calling OS_QueuePut (2)\n");
}
status = OS_MutSemGive(mutex_id);
if ( status != OS_SUCCESS )
{
printf("TASK 2:Error calling OS_MutSemGive\n");
}
OS_TaskDelay(500);
}
}
void task_1(void)
{
uint32 status;
OS_printf("Starting task 1\n");
OS_TaskRegister();
while(1)
{
status = OS_MutSemTake(mutex_id);
if ( status != OS_SUCCESS )
{
OS_printf("TASK 1:Error calling OS_MutSemTake with mutex_id = %d\n",mutex_id);
}
shared_resource_x = task_1_id;
status = OS_QueuePut(msgq_id, (void*)&shared_resource_x, sizeof(uint32), 0);
if ( status != OS_SUCCESS )
{
OS_printf("TASK 1:Error calling OS_QueuePut ( 1 )\n");
}
shared_resource_x = task_1_id;
status = OS_QueuePut(msgq_id, (void*)&shared_resource_x, sizeof(uint32), 0);
if ( status != OS_SUCCESS )
{
OS_printf("TASK 1:Error calling OS_QueuePut ( 2 )\n");
}
status = OS_MutSemGive(mutex_id);
if ( status != OS_SUCCESS )
{
OS_printf("TASK 1:Error calling OS_MutSemGive\n");
}
OS_TaskDelay(100);
}
}
/*
** 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;
}
/*
** 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;
}
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);
}
/*
** 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;
}
/*
** 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;
}
