diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 6ea85c6f..8f77d619 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -1,6 +1,7 @@ #include "PlocSupervisorHandler.h" #include +#include #include #include @@ -586,6 +587,8 @@ ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t r // } tmReader.setData(start, remainingSize); + sif::debug << "PlocSupervisorHandler::scanForReply: Received Packet" << std::endl; + arrayprinter::print(start, remainingSize); uint16_t apid = tmReader.getApid(); //(*(start) << 8 | *(start + 1)) & APID_MASK; switch (apid) { @@ -1915,13 +1918,13 @@ void PlocSupervisorHandler::handleBadApidServiceCombination(Event event, unsigne unsigned int serviceId) { const char* printString = ""; if (event == SUPV_UNKNOWN_TM) { - printString = "Unknown"; + printString = "PlocSupervisorHandler: Unknown"; } else if (event == SUPV_UNINIMPLEMENTED_TM) { - printString = "Unimplemented"; + printString = "PlocSupervisorHandler: Unimplemented"; } triggerEvent(event, apid, tmReader.getServiceId()); - sif::error << printString << " APID service combination 0x" << std::setw(2) << std::setfill('0') - << std::hex << apid << ", " << std::setw(2) << serviceId << std::endl; + sif::warning << printString << " APID service combination 0x" << std::setw(2) << std::setfill('0') + << std::hex << apid << ", 0x" << std::setw(2) << serviceId << std::endl; } void PlocSupervisorHandler::printAckFailureInfo(uint16_t statusCode, DeviceCommandId_t commandId) { diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index a80e23a8..4d4bee06 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -1,10 +1,9 @@ #include #include // Contains file controls like O_RDWR #include +#include #include #include - -#include #include #include @@ -293,8 +292,8 @@ ReturnValue_t PlocSupvUartManager::initiateUpdateContinuation() { void PlocSupvUartManager::stop() { MutexGuard mg(lock); - if(state == InternalState::SLEEPING or state == InternalState::GO_TO_SLEEP) { - return; + if (state == InternalState::SLEEPING or state == InternalState::GO_TO_SLEEP) { + return; } state = InternalState::GO_TO_SLEEP; } @@ -1001,16 +1000,17 @@ ReturnValue_t PlocSupvUartManager::readReceivedMessage(CookieIF* cookie, uint8_t ReturnValue_t PlocSupvUartManager::tryHdlcParsing() { size_t bytesRead = 0; - ReturnValue_t result = parseRecRingBufForHdlc(bytesRead); + size_t decodedLen = 0; + ReturnValue_t result = parseRecRingBufForHdlc(bytesRead, decodedLen); if (result == returnvalue::OK) { // Packet found, advance read pointer. if (state == InternalState::DEDICATED_REQUEST) { - decodedRingBuf.writeData(decodedBuf.data(), bytesRead); - decodedQueue.insert(bytesRead); + decodedRingBuf.writeData(decodedBuf.data(), decodedLen); + decodedQueue.insert(decodedLen); } else { MutexGuard mg(ipcLock); - ipcRingBuf.writeData(decodedBuf.data(), bytesRead); - ipcQueue.insert(bytesRead); + ipcRingBuf.writeData(decodedBuf.data(), decodedLen); + ipcQueue.insert(decodedLen); } recRingBuf.deleteData(bytesRead); } else if (result != NO_PACKET_FOUND) { @@ -1023,7 +1023,7 @@ ReturnValue_t PlocSupvUartManager::tryHdlcParsing() { return result; } -ReturnValue_t PlocSupvUartManager::parseRecRingBufForHdlc(size_t& readSize) { +ReturnValue_t PlocSupvUartManager::parseRecRingBufForHdlc(size_t& readSize, size_t& decodedLen) { size_t availableData = recRingBuf.getAvailableReadData(); if (availableData == 0) { return NO_PACKET_FOUND; @@ -1037,7 +1037,6 @@ ReturnValue_t PlocSupvUartManager::parseRecRingBufForHdlc(size_t& readSize) { } bool startMarkerFound = false; size_t startIdx = 0; - return returnvalue::OK; for (size_t idx = 0; idx < availableData; idx++) { // handle start marker if (encodedBuf[idx] == HDLC_START_MARKER) { @@ -1052,9 +1051,9 @@ ReturnValue_t PlocSupvUartManager::parseRecRingBufForHdlc(size_t& readSize) { if (encodedBuf[idx] == HDLC_END_MARKER) { if (startMarkerFound) { // Probably a packet, so decode it - size_t decodedLen = 0; - hdlc_remove_framing(encodedBuf.data() + startIdx, idx + 1, decodedBuf.data(), &decodedLen); - readSize = decodedLen; + hdlc_remove_framing(encodedBuf.data() + startIdx, idx + 1 - startIdx, decodedBuf.data(), + &decodedLen); + readSize = idx + 1; return returnvalue::OK; } else { readSize = ++idx; diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index d872ff03..ada96bf5 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -177,8 +177,8 @@ class PlocSupvUartManager : public DeviceCommunicationIF, static const uint32_t PREPARE_UPDATE_EXECUTION_REPORT = 2000; static constexpr uint8_t MAX_STORED_DECODED_PACKETS = 4; - static constexpr uint8_t HDLC_START_MARKER = 0x7C; - static constexpr uint8_t HDLC_END_MARKER = 0x7E; + static constexpr uint8_t HDLC_START_MARKER = 0x7E; + static constexpr uint8_t HDLC_END_MARKER = 0x7C; struct Update { uint8_t memoryId; @@ -261,7 +261,7 @@ class PlocSupvUartManager : public DeviceCommunicationIF, void executeFullCheckMemoryCommand(); ReturnValue_t tryHdlcParsing(); - ReturnValue_t parseRecRingBufForHdlc(size_t& readSize); + ReturnValue_t parseRecRingBufForHdlc(size_t& readSize, size_t& decodedLen); ReturnValue_t executeUpdate(); ReturnValue_t continueUpdate(); ReturnValue_t updateOperation(); diff --git a/thirdparty/tas/hdlc.c b/thirdparty/tas/hdlc.c index 4734c0b9..49808c1f 100644 --- a/thirdparty/tas/hdlc.c +++ b/thirdparty/tas/hdlc.c @@ -55,15 +55,15 @@ void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dle *dlen = tlen; } -void hdlc_remove_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen) +int hdlc_remove_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen) { uint16_t tlen = 0; uint16_t ii; uint8_t bt; *dlen = 0; - if (slen == 0) return; - if ((src[tlen] != 0x7E) && (src[slen-1] != 0x7C)) return; + if (slen == 0) return -1; + if ((src[tlen] != 0x7E) && (src[slen-1] != 0x7C)) return -1; for (ii = 1; ii < slen-1; ii++) { bt = *src++; @@ -75,6 +75,7 @@ void hdlc_remove_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t * dst[tlen++] = bt; } *dlen = tlen; + return 0; } diff --git a/thirdparty/tas/tas/hdlc.h b/thirdparty/tas/tas/hdlc.h index 97e69500..5599070b 100644 --- a/thirdparty/tas/tas/hdlc.h +++ b/thirdparty/tas/tas/hdlc.h @@ -28,7 +28,7 @@ extern "C" { void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen); -void hdlc_remove_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen); +int hdlc_remove_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen); #ifdef __cplusplus }