updating code from Flying Laptop
This is the framework of Flying Laptop OBSW version A.13.0.
This commit is contained in:
@ -1,67 +1,126 @@
|
||||
/*
|
||||
* DataPoolAdmin.cpp
|
||||
*
|
||||
* Created on: 05.12.2013
|
||||
* Author: baetz
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include <framework/datapool/DataPool.h>
|
||||
#include <framework/datapool/DataPoolAdmin.h>
|
||||
#include <framework/datapool/DataSet.h>
|
||||
#include <framework/datapool/PoolRawAccess.h>
|
||||
#include <framework/ipc/CommandMessage.h>
|
||||
#include <framework/ipc/QueueFactory.h>
|
||||
#include <framework/parameters/ParameterMessage.h>
|
||||
|
||||
|
||||
DataPoolAdmin::DataPoolAdmin(object_id_t objectId ) : SystemObject(objectId), commandQueue(), memoryHelper(this, &commandQueue){
|
||||
DataPoolAdmin::DataPoolAdmin(object_id_t objectId) :
|
||||
SystemObject(objectId), storage(NULL), commandQueue(NULL), memoryHelper(
|
||||
this, commandQueue), actionHelper(this, commandQueue) {
|
||||
commandQueue = QueueFactory::instance()->createMessageQueue();
|
||||
}
|
||||
|
||||
ReturnValue_t DataPoolAdmin::performOperation() {
|
||||
DataPoolAdmin::~DataPoolAdmin() {
|
||||
QueueFactory::instance()->deleteMessageQueue(commandQueue);
|
||||
}
|
||||
|
||||
ReturnValue_t DataPoolAdmin::performOperation(uint8_t opCode) {
|
||||
handleCommand();
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
MessageQueueId_t DataPoolAdmin::getCommandQueue() const {
|
||||
return commandQueue.getId();
|
||||
return commandQueue->getId();
|
||||
}
|
||||
|
||||
ReturnValue_t DataPoolAdmin::executeAction(ActionId_t actionId,
|
||||
MessageQueueId_t commandedBy, const uint8_t* data, uint32_t size) {
|
||||
if (actionId != SET_VALIDITY) {
|
||||
return INVALID_ACTION_ID;
|
||||
}
|
||||
|
||||
if (size != 5) {
|
||||
return INVALID_PARAMETERS;
|
||||
}
|
||||
|
||||
uint32_t address = (data[0] << 24) | (data[1] << 16) | (data[2] << 8)
|
||||
| data[3];
|
||||
|
||||
uint8_t valid = data[4];
|
||||
|
||||
uint32_t poolId = ::dataPool.PIDToDataPoolId(address);
|
||||
|
||||
DataSet mySet;
|
||||
PoolRawAccess variable(poolId, 0, &mySet, PoolVariableIF::VAR_READ_WRITE);
|
||||
ReturnValue_t status = mySet.read();
|
||||
if (status != RETURN_OK) {
|
||||
return INVALID_ADDRESS;
|
||||
}
|
||||
if (valid != 0) {
|
||||
variable.setValid(PoolVariableIF::VALID);
|
||||
} else {
|
||||
variable.setValid(PoolVariableIF::INVALID);
|
||||
}
|
||||
|
||||
mySet.commit();
|
||||
|
||||
return EXECUTION_FINISHED;
|
||||
}
|
||||
|
||||
ReturnValue_t DataPoolAdmin::getParameter(uint8_t domainId,
|
||||
uint16_t parameterId, ParameterWrapper* parameterWrapper,
|
||||
const ParameterWrapper* newValues, uint16_t startAtIndex) {
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
|
||||
void DataPoolAdmin::handleCommand() {
|
||||
CommandMessage command;
|
||||
ReturnValue_t result = commandQueue.receiveMessage(&command);
|
||||
ReturnValue_t result = commandQueue->receiveMessage(&command);
|
||||
if (result != RETURN_OK) {
|
||||
return;
|
||||
}
|
||||
|
||||
result = actionHelper.handleActionMessage(&command);
|
||||
|
||||
if (result == HasReturnvaluesIF::RETURN_OK) {
|
||||
return;
|
||||
}
|
||||
|
||||
result = handleParameterCommand(&command);
|
||||
if (result == HasReturnvaluesIF::RETURN_OK) {
|
||||
return;
|
||||
}
|
||||
|
||||
result = memoryHelper.handleMemoryCommand(&command);
|
||||
if (result != RETURN_OK) {
|
||||
command.setToUnknownCommand(command.getCommand());
|
||||
commandQueue.reply( &command );
|
||||
command.setToUnknownCommand();
|
||||
commandQueue->reply(&command);
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t DataPoolAdmin::handleMemoryLoad(uint32_t address,
|
||||
const uint8_t* data, uint32_t size, uint8_t** dataPointer) {
|
||||
uint32_t poolId = ::dataPool.PIDToDataPoolId( address );
|
||||
uint8_t arrayIndex = ::dataPool.PIDToArrayIndex( address );
|
||||
uint32_t poolId = ::dataPool.PIDToDataPoolId(address);
|
||||
uint8_t arrayIndex = ::dataPool.PIDToArrayIndex(address);
|
||||
DataSet testSet;
|
||||
PoolRawAccess varToGetSize( poolId, arrayIndex, &testSet, PoolVariableIF::VAR_READ );
|
||||
PoolRawAccess varToGetSize(poolId, arrayIndex, &testSet,
|
||||
PoolVariableIF::VAR_READ);
|
||||
ReturnValue_t status = testSet.read();
|
||||
if (status != RETURN_OK) {
|
||||
return INVALID_ADDRESS;
|
||||
}
|
||||
uint8_t typeSize = varToGetSize.getSizeOfType();
|
||||
if ( size > varToGetSize.getSizeTillEnd() ) {
|
||||
|
||||
if (size % typeSize != 0) {
|
||||
return INVALID_SIZE;
|
||||
}
|
||||
|
||||
if (size > varToGetSize.getSizeTillEnd()) {
|
||||
return INVALID_SIZE;
|
||||
}
|
||||
const uint8_t* readPosition = data;
|
||||
for ( ; size > 0; size -= typeSize ) {
|
||||
|
||||
for (; size > 0; size -= typeSize) {
|
||||
DataSet rawSet;
|
||||
PoolRawAccess variable( poolId, arrayIndex, &rawSet, PoolVariableIF::VAR_READ_WRITE );
|
||||
PoolRawAccess variable(poolId, arrayIndex, &rawSet,
|
||||
PoolVariableIF::VAR_READ_WRITE);
|
||||
status = rawSet.read();
|
||||
if (status == RETURN_OK) {
|
||||
status = variable.setEntryFromBigEndian( readPosition, typeSize );
|
||||
status = variable.setEntryFromBigEndian(readPosition, typeSize);
|
||||
if (status == RETURN_OK) {
|
||||
status = rawSet.commit(PoolVariableIF::VALID);
|
||||
status = rawSet.commit();
|
||||
}
|
||||
}
|
||||
arrayIndex += 1;
|
||||
@ -72,27 +131,29 @@ ReturnValue_t DataPoolAdmin::handleMemoryLoad(uint32_t address,
|
||||
|
||||
ReturnValue_t DataPoolAdmin::handleMemoryDump(uint32_t address, uint32_t size,
|
||||
uint8_t** dataPointer, uint8_t* copyHere) {
|
||||
uint32_t poolId = ::dataPool.PIDToDataPoolId( address );
|
||||
uint8_t arrayIndex = ::dataPool.PIDToArrayIndex( address );
|
||||
uint32_t poolId = ::dataPool.PIDToDataPoolId(address);
|
||||
uint8_t arrayIndex = ::dataPool.PIDToArrayIndex(address);
|
||||
DataSet testSet;
|
||||
PoolRawAccess varToGetSize( poolId, arrayIndex, &testSet, PoolVariableIF::VAR_READ );
|
||||
PoolRawAccess varToGetSize(poolId, arrayIndex, &testSet,
|
||||
PoolVariableIF::VAR_READ);
|
||||
ReturnValue_t status = testSet.read();
|
||||
if (status != RETURN_OK) {
|
||||
return INVALID_ADDRESS;
|
||||
}
|
||||
uint8_t typeSize = varToGetSize.getSizeOfType();
|
||||
if ( size > varToGetSize.getSizeTillEnd() ) {
|
||||
if (size > varToGetSize.getSizeTillEnd()) {
|
||||
return INVALID_SIZE;
|
||||
}
|
||||
uint8_t* ptrToCopy = copyHere;
|
||||
for ( ; size > 0; size -= typeSize ) {
|
||||
for (; size > 0; size -= typeSize) {
|
||||
DataSet rawSet;
|
||||
PoolRawAccess variable( poolId, arrayIndex, &rawSet, PoolVariableIF::VAR_READ );
|
||||
PoolRawAccess variable(poolId, arrayIndex, &rawSet,
|
||||
PoolVariableIF::VAR_READ);
|
||||
status = rawSet.read();
|
||||
if (status == RETURN_OK) {
|
||||
uint32_t temp = 0;
|
||||
status = variable.getEntryEndianSafe( ptrToCopy, &temp, size);
|
||||
if ( status != RETURN_OK ) {
|
||||
status = variable.getEntryEndianSafe(ptrToCopy, &temp, size);
|
||||
if (status != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
} else {
|
||||
@ -105,8 +166,135 @@ ReturnValue_t DataPoolAdmin::handleMemoryDump(uint32_t address, uint32_t size,
|
||||
}
|
||||
|
||||
ReturnValue_t DataPoolAdmin::initialize() {
|
||||
if (memoryHelper.initialize() == RETURN_OK) {
|
||||
return SystemObject::initialize();
|
||||
ReturnValue_t result = SystemObject::initialize();
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
return RETURN_FAILED;
|
||||
|
||||
result = memoryHelper.initialize();
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
|
||||
storage = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
|
||||
if (storage == NULL) {
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
|
||||
result = actionHelper.initialize();
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
//mostly identical to ParameterHelper::handleParameterMessage()
|
||||
ReturnValue_t DataPoolAdmin::handleParameterCommand(CommandMessage* command) {
|
||||
ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED;
|
||||
switch (command->getCommand()) {
|
||||
case ParameterMessage::CMD_PARAMETER_DUMP: {
|
||||
uint8_t domain = HasParametersIF::getDomain(
|
||||
ParameterMessage::getParameterId(command));
|
||||
uint16_t parameterId = HasParametersIF::getMatrixId(
|
||||
ParameterMessage::getParameterId(command));
|
||||
|
||||
DataPoolParameterWrapper wrapper;
|
||||
result = wrapper.set(domain, parameterId);
|
||||
|
||||
if (result == HasReturnvaluesIF::RETURN_OK) {
|
||||
result = sendParameter(command->getSender(),
|
||||
ParameterMessage::getParameterId(command), &wrapper);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case ParameterMessage::CMD_PARAMETER_LOAD: {
|
||||
|
||||
uint8_t domain = HasParametersIF::getDomain(
|
||||
ParameterMessage::getParameterId(command));
|
||||
uint16_t parameterId = HasParametersIF::getMatrixId(
|
||||
ParameterMessage::getParameterId(command));
|
||||
uint8_t index = HasParametersIF::getIndex(
|
||||
ParameterMessage::getParameterId(command));
|
||||
|
||||
const uint8_t *storedStream;
|
||||
uint32_t storedStreamSize;
|
||||
result = storage->getData(ParameterMessage::getStoreId(command),
|
||||
&storedStream, &storedStreamSize);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
break;
|
||||
}
|
||||
|
||||
ParameterWrapper streamWrapper;
|
||||
result = streamWrapper.set(storedStream, storedStreamSize);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
storage->deleteData(ParameterMessage::getStoreId(command));
|
||||
break;
|
||||
}
|
||||
|
||||
DataPoolParameterWrapper poolWrapper;
|
||||
result = poolWrapper.set(domain, parameterId);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
storage->deleteData(ParameterMessage::getStoreId(command));
|
||||
break;
|
||||
}
|
||||
|
||||
result = poolWrapper.copyFrom(&streamWrapper, index);
|
||||
|
||||
storage->deleteData(ParameterMessage::getStoreId(command));
|
||||
|
||||
if (result == HasReturnvaluesIF::RETURN_OK) {
|
||||
result = sendParameter(command->getSender(),
|
||||
ParameterMessage::getParameterId(command), &poolWrapper);
|
||||
}
|
||||
}
|
||||
break;
|
||||
default:
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
rejectCommand(command->getSender(), result, command->getCommand());
|
||||
}
|
||||
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
|
||||
}
|
||||
|
||||
//identical to ParameterHelper::sendParameter()
|
||||
ReturnValue_t DataPoolAdmin::sendParameter(MessageQueueId_t to, uint32_t id,
|
||||
const DataPoolParameterWrapper* wrapper) {
|
||||
uint32_t serializedSize = wrapper->getSerializedSize();
|
||||
|
||||
uint8_t *storeElement;
|
||||
store_address_t address;
|
||||
|
||||
ReturnValue_t result = storage->getFreeElement(&address, serializedSize,
|
||||
&storeElement);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
|
||||
uint32_t storeElementSize = 0;
|
||||
|
||||
result = wrapper->serialize(&storeElement, &storeElementSize,
|
||||
serializedSize, true);
|
||||
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
storage->deleteData(address);
|
||||
return result;
|
||||
}
|
||||
|
||||
CommandMessage reply;
|
||||
|
||||
ParameterMessage::setParameterDumpReply(&reply, id, address);
|
||||
|
||||
commandQueue->sendMessage(to, &reply);
|
||||
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
//identical to ParameterHelper::rejectCommand()
|
||||
void DataPoolAdmin::rejectCommand(MessageQueueId_t to, ReturnValue_t reason,
|
||||
Command_t initialCommand) {
|
||||
CommandMessage reply;
|
||||
reply.setReplyRejected(reason, initialCommand);
|
||||
commandQueue->sendMessage(to, &reply);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user