int32 CFE_ES_RebuildCDS(void) { int32 Status = CFE_SUCCESS; int32 PoolOffset; /* First, determine if the CDS registry stored in the CDS is smaller or equal */ /* in size to the CDS registry we are currently configured for */ /* Copy the number of registry entries to the CDS */ Status = CFE_PSP_ReadFromCDS(&CFE_ES_Global.CDSVars.MaxNumRegEntries, CDS_REG_SIZE_OFFSET, sizeof(CFE_ES_Global.CDSVars.MaxNumRegEntries)); if ((Status == OS_SUCCESS) && (CFE_ES_Global.CDSVars.MaxNumRegEntries <= CFE_ES_CDS_MAX_NUM_ENTRIES)) { Status = CFE_PSP_ReadFromCDS(&CFE_ES_Global.CDSVars.Registry, CDS_REG_OFFSET, (CFE_ES_Global.CDSVars.MaxNumRegEntries * sizeof(CFE_ES_CDS_RegRec_t))); if (Status == OS_SUCCESS) { /* Calculate the starting offset of the memory pool */ PoolOffset = (CDS_REG_OFFSET + (CFE_ES_Global.CDSVars.MaxNumRegEntries * sizeof(CFE_ES_CDS_RegRec_t)) + 3) & 0xfffffffc;; /* Calculate the size of the memory pool */ CFE_ES_Global.CDSVars.MemPoolSize = CFE_ES_Global.CDSVars.CDSSize - PoolOffset - sizeof(CFE_ES_Global.CDSVars.ValidityField); /* Scan the memory pool and identify the created but currently unused memory blocks */ Status = CFE_ES_RebuildCDSPool(CFE_ES_Global.CDSVars.MemPoolSize, PoolOffset); } else { /* Registry in CDS is unreadable */ Status = CFE_ES_CDS_INVALID; CFE_ES_WriteToSysLog("CFE_CDS:Rebuild-Registry in CDS is unreadable\n"); } } else { /* Registry in CDS is too large to recover */ Status = CFE_ES_CDS_INVALID; CFE_ES_WriteToSysLog("CFE_CDS:Rebuild-Registry in CDS too large to recover\n"); } return Status; }
int32 CFE_ES_ValidateCDS(void) { /* Assume the CDS is invalid */ int32 Status = CFE_ES_CDS_INVALID; /* Perform 2 checks to validate the CDS Memory Pool */ /* First, determine if the first validity check field is correct */ Status = CFE_PSP_ReadFromCDS(&CFE_ES_Global.CDSVars.ValidityField, 0, sizeof(CFE_ES_Global.CDSVars.ValidityField)); if (Status == OS_SUCCESS) { if (strncmp(CFE_ES_Global.CDSVars.ValidityField, "_CDSBeg_", sizeof(CFE_ES_Global.CDSVars.ValidityField)) == 0) { Status = CFE_PSP_ReadFromCDS(&CFE_ES_Global.CDSVars.ValidityField, (CFE_ES_Global.CDSVars.CDSSize-sizeof(CFE_ES_Global.CDSVars.ValidityField)), sizeof(CFE_ES_Global.CDSVars.ValidityField)); if (Status == OS_SUCCESS) { if (strncmp(CFE_ES_Global.CDSVars.ValidityField, "_CDSEnd_", sizeof(CFE_ES_Global.CDSVars.ValidityField)) == 0) { Status = CFE_SUCCESS; } else /* Validity Field failed */ { Status = CFE_ES_CDS_INVALID; } } else /* BSP reported an error reading from CDS */ { CFE_ES_WriteToSysLog("CFE_CDS:Validate-2nd ReadFromCDS Failed. Status=0x%X\n", Status); } } else /* Validity Field failed */ { Status = CFE_ES_CDS_INVALID; } } else /* BSP reported an error reading from CDS */ { CFE_ES_WriteToSysLog("CFE_CDS:Validate-1st ReadFromCDS Failed. Status=0x%X\n", Status); } return Status; } /* End of CFE_ES_ValidateCDS() */
/* ** 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; }