From 655e01c2d13979111a11f226151a45d63ee42175 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 15 Feb 2023 19:27:19 +0100 Subject: [PATCH] some more bugfixes --- linux/devices/RwPollingTask.cpp | 136 ++++++++++++++++---------------- linux/devices/RwPollingTask.h | 2 +- 2 files changed, 70 insertions(+), 68 deletions(-) diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp index 657f8853..b300136a 100644 --- a/linux/devices/RwPollingTask.cpp +++ b/linux/devices/RwPollingTask.cpp @@ -172,6 +172,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf gpioId_t gpioId = rwCookie.getChipSelectPin(); GpioIF& gpioIF = spiIF->getGpioInterface(); pullCsLow(gpioId, spiLock, gpioIF); + uint8_t byteRead = 0; for (unsigned idx = 0; idx < MAX_RETRIES_REPLY; idx++) { result = openSpi(O_RDWR, fd); if (result != returnvalue::OK) { @@ -181,18 +182,17 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf * The reaction wheel responds with empty frames while preparing the reply data. * However, receiving more than 5 empty frames will be interpreted as an error. */ - uint8_t byteRead = 0; for (int idx = 0; idx < 5; idx++) { if (read(fd, &byteRead, 1) != 1) { sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; - pullCsHigh(gpioId, spiLock, gpioIF); + pullCsHigh(gpioId, gpioIF); closeSpi(fd); return rws::SPI_READ_FAILURE; } if (idx == 0) { if (byteRead != FLAG_BYTE) { sif::error << "Invalid data, expected start marker" << std::endl; - pullCsHigh(gpioId, spiLock, gpioIF); + pullCsHigh(gpioId, gpioIF); closeSpi(fd); return rws::NO_START_MARKER; } @@ -202,7 +202,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf break; } - pullCsHigh(gpioId, spiLock, gpioIF); + pullCsHigh(gpioId, gpioIF); closeSpi(fd); if (idx == MAX_RETRIES_REPLY - 1) { sif::error << "rwSpiCallback::spiCallback: Empty frame timeout" << std::endl; @@ -210,76 +210,78 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf } TaskFactory::delayTask(5); } - + } #if FSFW_HAL_SPI_WIRETAPPING == 1 - sif::info << "RW start marker detected" << std::endl; + sif::info << "RW start marker detected" << std::endl; #endif - size_t decodedFrameLen = 0; + size_t decodedFrameLen = 0; - while (decodedFrameLen < maxReplyLen) { - /** First byte already read in */ - if (decodedFrameLen != 0) { - byteRead = 0; - if (read(fd, &byteRead, 1) != 1) { - sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; - result = rws::SPI_READ_FAILURE; - break; - } - } - - if (byteRead == FLAG_BYTE) { - /** Reached end of frame */ + while (decodedFrameLen < maxReplyLen) { + // First byte already read in + if (decodedFrameLen != 0) { + byteRead = 0; + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; + result = rws::SPI_READ_FAILURE; break; - } else if (byteRead == 0x7D) { - if (read(fd, &byteRead, 1) != 1) { - sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; - result = rws::SPI_READ_FAILURE; - break; - } - if (byteRead == 0x5E) { - *(replyBuf + decodedFrameLen) = 0x7E; - decodedFrameLen++; - continue; - } else if (byteRead == 0x5D) { - *(replyBuf + decodedFrameLen) = 0x7D; - decodedFrameLen++; - continue; - } else { - sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl; - closeSpi(fd); - result = rws::INVALID_SUBSTITUTE; - break; - } - } else { - *(replyBuf + decodedFrameLen) = byteRead; + } + } + + if (byteRead == FLAG_BYTE) { + // Reached end of frame + break; + } else if (byteRead == 0x7D) { + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; + result = rws::SPI_READ_FAILURE; + break; + } + if (byteRead == 0x5E) { + *(replyBuf + decodedFrameLen) = 0x7E; decodedFrameLen++; continue; + } else if (byteRead == 0x5D) { + *(replyBuf + decodedFrameLen) = 0x7D; + decodedFrameLen++; + continue; + } else { + sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl; + result = rws::INVALID_SUBSTITUTE; + break; } - - /** - * There might be the unlikely case that each byte in a get-telemetry reply has been - * replaced by its substitute. Than the next byte must correspond to the end sign 0x7E. - * Otherwise there might be something wrong. - */ - if (decodedFrameLen == maxReplyLen) { - if (read(fd, &byteRead, 1) != 1) { - sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl; - result = rws::SPI_READ_FAILURE; - break; - } - if (byteRead != FLAG_BYTE) { - sif::error << "rwSpiCallback::spiCallback: Missing end sign " - << static_cast(FLAG_BYTE) << std::endl; - decodedFrameLen--; - result = rws::MISSING_END_SIGN; - break; - } - } - result = returnvalue::OK; + } else { + *(replyBuf + decodedFrameLen) = byteRead; + decodedFrameLen++; + continue; } + + // Check end marker. + /** + * There might be the unlikely case that each byte in a get-telemetry reply has been + * replaced by its substitute. Then the next byte must correspond to the end sign 0x7E. + * Otherwise there might be something wrong. + */ + if (decodedFrameLen == maxReplyLen) { + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl; + result = rws::SPI_READ_FAILURE; + break; + } + if (byteRead != FLAG_BYTE) { + sif::error << "rwSpiCallback::spiCallback: Missing end sign " << static_cast(FLAG_BYTE) + << std::endl; + decodedFrameLen--; + result = rws::MISSING_END_SIGN; + break; + } + } + result = returnvalue::OK; } - return returnvalue::OK; + + pullCsHigh(gpioId, gpioIF); + closeSpi(fd); + return result; } ReturnValue_t RwPollingTask::writeOneRwCmd(uint8_t rwIdx, int fd) { @@ -410,10 +412,10 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) { encodeHdlc(writeBuffer.data(), writeLen, lenToSend); if (write(fd, encodedBuffer.data(), lenToSend) != static_cast(lenToSend)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; - pullCsHigh(gpioId, spiLock, gpioIF); + pullCsHigh(gpioId, gpioIF); return rws::SPI_WRITE_FAILURE; } - pullCsHigh(gpioId, spiLock, gpioIF); + pullCsHigh(gpioId, gpioIF); return returnvalue::OK; } @@ -434,7 +436,7 @@ ReturnValue_t RwPollingTask::pullCsLow(gpioId_t gpioId, MutexIF* spiLock, GpioIF return returnvalue::OK; } -void RwPollingTask::pullCsHigh(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF) { +void RwPollingTask::pullCsHigh(gpioId_t gpioId, GpioIF& gpioIF) { if (gpioId != gpio::NO_GPIO) { if (gpioIF.pullHigh(gpioId) != returnvalue::OK) { sif::error << "closeSpi: Failed to pull chip select high" << std::endl; diff --git a/linux/devices/RwPollingTask.h b/linux/devices/RwPollingTask.h index 2b7d899f..701771f1 100644 --- a/linux/devices/RwPollingTask.h +++ b/linux/devices/RwPollingTask.h @@ -77,7 +77,7 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev size_t idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, uint8_t** readPtr); void encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, size_t& encodedLen); - void pullCsHigh(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF); + void pullCsHigh(gpioId_t gpioId, GpioIF& gpioIF); void closeSpi(int); };