eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

plocHandler wip

Browse Source
This commit is contained in:
Jakob Meier
2021-04-15 13:17:15 +02:00
parent 5d4c2bd521
commit 0916ca87d9
9 changed files with 214 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

156
mission/devices/PlocHandler.cpp
View File

@ -41,24 +41,10 @@ ReturnValue_t PlocHandler::buildCommandFromCommand(
size_t commandDataLen) {
switch(deviceCommand) {
case(PLOC::TC_MEM_WRITE): {
const uint32_t memoryAddress = *(commandData) << 24 | *(commandData + 1) << 16
| *(commandData + 2) << 8 | *(commandData + 3);
const uint32_t memoryData = *(commandData + 4) << 24 | *(commandData + 5) << 16
| *(commandData + 6) << 8 | *(commandData + 7);
PLOC::TcMemWrite tcMemWrite(memoryAddress, memoryData);
rawPacket = tcMemWrite.getWholeData();
rawPacketLen = tcMemWrite.getFullSize();
rememberCommandId = PLOC::TC_MEM_WRITE;
return RETURN_OK;
return prepareTcMemWriteCommand(commandData, commandDataLen);
}
case(PLOC::TC_MEM_READ): {
const uint32_t memoryAddress = *(commandData) << 24 | *(commandData + 1) << 16
| *(commandData + 2) << 8 | *(commandData + 3);
PLOC::TcMemRead tcMemRead(memoryAddress);
rawPacket = tcMemRead.getWholeData();
rawPacketLen = tcMemRead.getFullSize();
rememberCommandId = PLOC::TC_MEM_READ;
return RETURN_OK;
return prepareTcMemReadCommand(commandData, commandDataLen);
}
default:
sif::debug << "PlocHandler::buildCommandFromCommand: Command not implemented" << std::endl;
@ -67,11 +53,43 @@ ReturnValue_t PlocHandler::buildCommandFromCommand(
return HasReturnvaluesIF::RETURN_FAILED;
}
ReturnValue_t PlocHandler::prepareTcMemWriteCommand(const uint8_t * commandData,
size_t commandDataLen) {
const uint32_t memoryAddress = *(commandData) << 24 | *(commandData + 1) << 16
| *(commandData + 2) << 8 | *(commandData + 3);
const uint32_t memoryData = *(commandData + 4) << 24 | *(commandData + 5) << 16
| *(commandData + 6) << 8 | *(commandData + 7);
PLOC::TcMemWrite tcMemWrite(memoryAddress, memoryData);
if (tcMemWrite.getFullSize() > PLOC::MAX_COMMAND_SIZE) {
sif::debug << "PlocHandler::prepareTcMemWriteCommand: Command too big" << std::endl;
return RETURN_FAILED;
}
memcpy(commandBuffer, tcMemWrite.getWholeData(), tcMemWrite.getFullSize());
rawPacket = commandBuffer;
rawPacketLen = tcMemWrite.getFullSize();
rememberCommandId = PLOC::TC_MEM_WRITE;
return RETURN_OK;
}
ReturnValue_t PlocHandler::prepareTcMemReadCommand(const uint8_t * commandData,
size_t commandDataLen) {
const uint32_t memoryAddress = *(commandData) << 24 | *(commandData + 1) << 16
| *(commandData + 2) << 8 | *(commandData + 3);
PLOC::TcMemRead tcMemRead(memoryAddress);
if (tcMemRead.getFullSize() > PLOC::MAX_COMMAND_SIZE) {
sif::debug << "PlocHandler::prepareTcMemReadCommand: Command too big" << std::endl;
return RETURN_FAILED;
}
memcpy(commandBuffer, tcMemRead.getWholeData(), tcMemRead.getFullSize());
rawPacket = commandBuffer;
rawPacketLen = tcMemRead.getFullSize();
rememberCommandId = PLOC::TC_MEM_READ;
return RETURN_OK;
}
void PlocHandler::fillCommandAndReplyMap() {
this->insertInCommandAndReplyMap(PLOC::TC_MEM_WRITE, 1, nullptr, PLOC::SIZE_ACK_REPORT);
this->insertInCommandAndReplyMap(PLOC::TC_MEM_READ, 1, nullptr, PLOC::SIZE_ACK_REPORT);
this->insertInReplyMap(PLOC::ACK_SUCCESS, 1);
this->insertInReplyMap(PLOC::ACK_FAILURE, 1);
}
ReturnValue_t PlocHandler::scanForReply(const uint8_t *start,
@ -79,31 +97,34 @@ ReturnValue_t PlocHandler::scanForReply(const uint8_t *start,
ReturnValue_t result = RETURN_OK;
*foundId = (*(start) << 8 | *(start + 1)) & APID_MASK;
uint16_t apid = (*(start) << 8 | *(start + 1)) & APID_MASK;
switch(*foundId) {
case(PLOC::ACK_SUCCESS):
switch(apid) {
case(PLOC::APID_ACK_SUCCESS):
*foundLen = PLOC::SIZE_ACK_REPORT;
*foundId = rememberCommandId;
break;
case(PLOC::ACK_FAILURE):
case(PLOC::APID_ACK_FAILURE):
*foundLen = PLOC::SIZE_ACK_REPORT;
break;
*foundId = rememberCommandId;
break;
default:
sif::debug << "PlocHandler::scanForReply: Reply has invalid apid" << std::endl;
result = IGNORE_REPLY_DATA;
return IGNORE_REPLY_DATA;
break;
}
result = verifyPacket(start, foundLen);
return result;
}
ReturnValue_t PlocHandler::verifyPacket(const uint8_t* start, size_t* foundLen) {
ReturnValue_t PlocHandler::verifyPacket(const uint8_t* start, size_t foundLen) {
uint16_t receivedCrc = *(start + *foundLen - 2) << 8 | *(start + *foundLen - 1);
uint16_t receivedCrc = *(start + foundLen - 2) << 8 | *(start + foundLen - 1);
if (receivedCrc != CRC::crc16ccitt(start, *foundLen, 0)) {
uint16_t recalculatedCrc = CRC::crc16ccitt(start, foundLen - 2, 0);
if (receivedCrc != recalculatedCrc) {
sif::debug << "PlocHandler::verifyPacket: CRC failure of ACK reply" << std::endl;
return CRC_FAILURE;
}
@ -113,55 +134,68 @@ ReturnValue_t PlocHandler::verifyPacket(const uint8_t* start, size_t* foundLen)
ReturnValue_t PlocHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
uint16_t apid = (*(packet) << 8 | *(packet + 1)) & APID_MASK;
if (apid == PLOC::APID_ACK_FAILURE) {
sif::error << "PlocHandler::interpretDeviceReply: Received ACK failure reply" << std::endl;
triggerEvent(ACK_FAILURE, id);
replyRawData(packet, PLOC::SIZE_ACK_REPORT, defaultRawReceiver);
return RETURN_OK;
}
if (verifyPacket(packet, PLOC::SIZE_ACK_REPORT) == CRC_FAILURE) {
sif::error << "PlocHandler::interpretDeviceReply: CRC failure in Ack reply" << std::endl;
replyRawData(packet, PLOC::SIZE_ACK_REPORT, defaultRawReceiver);
triggerEvent(CRC_FAILURE_IN_ACK_REPLY, id);
return RETURN_OK;
}
switch (id) {
case (PLOC::ACK_SUCCESS):
receiveTm();
receiveExecutionReport();
break;
case (PLOC::ACK_FAILURE):
//TODO: Interpretation of status field in ack reply.
sif::error << "PlocHandler::interpretDeviceReply: Received ack failure reply" << std::endl;
triggerEvent(ACK_FAILURE, rememberCommandId);
break;
default: {
sif::debug << "PlocHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
}
}
return RETURN_OK;
}
void PlocHandler::receiveTm() {
switch (rememberCommandId) {
case (PLOC::TC_MEM_WRITE):
receiveExecutionReport();
break;
case (PLOC::TC_MEM_READ):
receiveTmMemoryReadReport();
receiveExecutionReport();
break;
default:
sif::debug << "PlocHandler::receiveTm: Rembered unknown command id" << std::endl;
break;
}
sif::debug << "PlocHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
}
rememberCommandId = PLOC::NONE;
return RETURN_OK;
}
void PlocHandler::receiveExecutionReport() {
ReturnValue_t PlocHandler::receiveExecutionReport() {
size_t receivedDataLen = 0;
uint8_t *receivedData = nullptr;
communicationInterface->requestReceiveMessage(comCookie, PLOC::SIZE_EXE_REPORT);
communicationInterface->readReceivedMessage(comCookie, &receivedData, &receivedDataLen);
ReturnValue_t result = RETURN_OK;
if(!verifyPacket(receivedData, &receivedDataLen)) {
result = communicationInterface->requestReceiveMessage(comCookie, PLOC::SIZE_EXE_REPORT);
if (result != RETURN_OK) {
sif::error << "PlocHandler::receiveExecutionReport: Failed to request execution report"
<< std::endl;
return result;
}
result = communicationInterface->readReceivedMessage(comCookie, &receivedData, &receivedDataLen);
if (result != RETURN_OK) {
sif::error << "PlocHandler::receiveExecutionReport: Failed to read execution report"
<< std::endl;
return result;
}
if(verifyPacket(receivedData, receivedDataLen) == CRC_FAILURE) {
replyRawData(receivedData, receivedDataLen, defaultRawReceiver);
triggerEvent(EXE_RPT_INVALID_CRC);
sif::error << "PlocHandler::receiveExecutionReport: Execution report has invalid crc"
<< std::endl;
return;
return result;
}
handleExecutionReport(receivedData, receivedDataLen);
result = handleExecutionReport(receivedData, receivedDataLen);
}
void PlocHandler::handleExecutionReport(const uint8_t* receivedData, size_t receivedDataLen) {
@ -177,10 +211,10 @@ void PlocHandler::handleExecutionReport(const uint8_t* receivedData, size_t rece
sif::error << "PlocHandler::handleExecutionReport: Received execution failure report"
<< std::endl;
triggerEvent(EXE_FAILURE, rememberCommandId);
return;
return EXE_REPLY_CRC_FAILURE;
}
}
return;
return RETURN_OK;
}
void PlocHandler::receiveTmMemoryReadReport() {
@ -211,7 +245,7 @@ void PlocHandler::receiveTmMemoryReadReport() {
return;
}
if(!verifyPacket(receivedData, &receivedDataLen)) {
if(verifyPacket(receivedData, receivedDataLen) == CRC_FAILURE) {
replyRawData(receivedData, receivedDataLen, defaultRawReceiver);
triggerEvent(TM_READ_RPT_INVALID_CRC);
sif::error << "PlocHandler::receiveTmReadReport: TM read report has invalid crc"