eive-obsw/bsp_q7s/callbacks/rwSpiCallback.cpp

284 lines
9.0 KiB
C++
Raw Normal View History

2021-09-07 16:11:02 +02:00
#include "rwSpiCallback.h"
2022-05-10 18:34:39 +02:00
#include <fsfw/timemanager/Stopwatch.h>
2022-01-17 15:58:27 +01:00
#include "devices/gpioIds.h"
2021-09-07 16:11:02 +02:00
#include "fsfw/serviceinterface/ServiceInterface.h"
2022-01-17 15:58:27 +01:00
#include "fsfw_hal/linux/UnixFileGuard.h"
#include "fsfw_hal/linux/spi/SpiCookie.h"
2023-03-24 20:50:33 +01:00
#include "mission/acs/RwHandler.h"
2021-06-21 09:50:26 +02:00
namespace rwSpiCallback {
namespace {
static bool MODE_SET = false;
ReturnValue_t openSpi(const std::string& devname, int flags, GpioIF* gpioIF, gpioId_t gpioId,
MutexIF* mutex, MutexIF::TimeoutType timeoutType, uint32_t timeoutMs,
int& fd);
/**
* @brief This function closes a spi session. Pulls the chip select to high an releases the
* mutex.
* @param gpioId Gpio ID of chip select
* @param gpioIF Pointer to gpio interface to drive the chip select
* @param mutex The spi mutex
*/
2022-05-11 15:45:38 +02:00
void closeSpi(int fd, gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex);
} // namespace
2022-01-17 15:58:27 +01:00
ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sendData,
size_t sendLen, void* args) {
2022-05-10 18:34:39 +02:00
// Stopwatch watch;
2022-08-24 17:27:47 +02:00
ReturnValue_t result = returnvalue::OK;
2022-01-17 15:58:27 +01:00
RwHandler* handler = reinterpret_cast<RwHandler*>(args);
if (handler == nullptr) {
sif::error << "rwSpiCallback::spiCallback: Pointer to handler is invalid" << std::endl;
2022-08-24 17:27:47 +02:00
return returnvalue::FAILED;
2022-01-17 15:58:27 +01:00
}
2022-05-05 16:32:09 +02:00
uint8_t writeBuffer[2] = {};
2022-01-17 15:58:27 +01:00
uint8_t writeSize = 0;
gpioId_t gpioId = cookie->getChipSelectPin();
2022-10-26 17:11:57 +02:00
GpioIF& gpioIF = comIf->getGpioInterface();
2022-01-17 15:58:27 +01:00
MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
uint32_t timeoutMs = 0;
2022-05-14 11:34:25 +02:00
MutexIF* mutex = comIf->getCsMutex();
cookie->getMutexParams(timeoutType, timeoutMs);
2022-10-26 17:11:57 +02:00
if (mutex == nullptr) {
2022-01-17 15:58:27 +01:00
sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl;
2022-08-24 17:27:47 +02:00
return returnvalue::FAILED;
2022-01-17 15:58:27 +01:00
}
int fileDescriptor = 0;
const std::string& dev = comIf->getSpiDev();
2022-10-26 17:11:57 +02:00
result = openSpi(dev, O_RDWR, &gpioIF, gpioId, mutex, timeoutType, timeoutMs, fileDescriptor);
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
return result;
2022-01-17 15:58:27 +01:00
}
2022-01-17 15:58:27 +01:00
spi::SpiModes spiMode = spi::SpiModes::MODE_0;
uint32_t spiSpeed = 0;
cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
// We are in protected section, so we can use the static variable here without issues.
// We don't need to set the speed because a SPI core is used, but the mode has to be set once
// correctly for all RWs
if (not MODE_SET) {
comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
MODE_SET = true;
2022-01-17 15:58:27 +01:00
}
2021-06-21 09:50:26 +02:00
2022-01-17 15:58:27 +01:00
/** Sending frame start sign */
writeBuffer[0] = FLAG_BYTE;
2022-01-17 15:58:27 +01:00
writeSize = 1;
2021-06-21 09:50:26 +02:00
2022-01-17 15:58:27 +01:00
if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
2022-10-26 17:11:57 +02:00
closeSpi(fileDescriptor, gpioId, &gpioIF, mutex);
2023-02-15 17:02:22 +01:00
return rws::SPI_WRITE_FAILURE;
2022-01-17 15:58:27 +01:00
}
/** Encoding and sending command */
size_t idx = 0;
while (idx < sendLen) {
switch (*(sendData + idx)) {
case 0x7E:
writeBuffer[0] = 0x7D;
writeBuffer[1] = 0x5E;
writeSize = 2;
break;
case 0x7D:
writeBuffer[0] = 0x7D;
writeBuffer[1] = 0x5D;
writeSize = 2;
break;
default:
writeBuffer[0] = *(sendData + idx);
writeSize = 1;
break;
2021-06-21 09:50:26 +02:00
}
2022-01-17 15:58:27 +01:00
if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
2022-10-26 17:11:57 +02:00
closeSpi(fileDescriptor, gpioId, &gpioIF, mutex);
2023-02-15 17:02:22 +01:00
return rws::SPI_WRITE_FAILURE;
}
2022-01-17 15:58:27 +01:00
idx++;
}
2022-01-17 15:58:27 +01:00
/** Sending frame end sign */
writeBuffer[0] = FLAG_BYTE;
2022-01-17 15:58:27 +01:00
writeSize = 1;
2021-06-21 09:50:26 +02:00
2022-01-17 15:58:27 +01:00
if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
2022-10-26 17:11:57 +02:00
closeSpi(fileDescriptor, gpioId, &gpioIF, mutex);
2023-02-15 17:02:22 +01:00
return rws::SPI_WRITE_FAILURE;
2022-01-17 15:58:27 +01:00
}
2021-06-21 09:50:26 +02:00
2022-01-17 15:58:27 +01:00
uint8_t* rxBuf = nullptr;
result = comIf->getReadBuffer(cookie->getSpiAddress(), &rxBuf);
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
2022-10-26 17:11:57 +02:00
closeSpi(fileDescriptor, gpioId, &gpioIF, mutex);
2022-01-17 15:58:27 +01:00
return result;
}
size_t replyBufferSize = cookie->getMaxBufferSize();
// There must be a delay of at least 20 ms after sending the command.
// Delay for 70 ms here and release the SPI bus for that duration.
2022-10-26 17:11:57 +02:00
closeSpi(fileDescriptor, gpioId, &gpioIF, mutex);
2023-02-15 17:02:22 +01:00
usleep(rws::SPI_REPLY_DELAY);
2022-10-26 17:11:57 +02:00
result = openSpi(dev, O_RDWR, &gpioIF, gpioId, mutex, timeoutType, timeoutMs, fileDescriptor);
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
return result;
}
2022-01-17 15:58:27 +01:00
/**
* 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;
2023-02-17 12:19:53 +01:00
for (idx = 0; idx < 10; idx++) {
2022-01-17 15:58:27 +01:00
if (read(fileDescriptor, &byteRead, 1) != 1) {
sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl;
2022-10-26 17:11:57 +02:00
closeSpi(fileDescriptor, gpioId, &gpioIF, mutex);
2023-02-15 17:02:22 +01:00
return rws::SPI_READ_FAILURE;
2022-01-17 15:58:27 +01:00
}
if (idx == 0) {
if (byteRead != FLAG_BYTE) {
sif::error << "Invalid data, expected start marker" << std::endl;
2022-10-26 17:11:57 +02:00
closeSpi(fileDescriptor, gpioId, &gpioIF, mutex);
2023-02-15 17:02:22 +01:00
return rws::NO_START_MARKER;
2022-01-17 15:58:27 +01:00
}
2021-06-21 09:50:26 +02:00
}
2022-01-17 15:58:27 +01:00
if (byteRead != FLAG_BYTE) {
break;
2021-06-21 09:50:26 +02:00
}
2022-01-17 15:58:27 +01:00
if (idx == 9) {
sif::error << "rwSpiCallback::spiCallback: Empty frame timeout" << std::endl;
2022-10-26 17:11:57 +02:00
closeSpi(fileDescriptor, gpioId, &gpioIF, mutex);
2023-02-15 17:02:22 +01:00
return rws::NO_REPLY;
2022-01-17 15:58:27 +01:00
}
}
2021-06-21 09:50:26 +02:00
2022-01-17 15:58:27 +01:00
#if FSFW_HAL_SPI_WIRETAPPING == 1
sif::info << "RW start marker detected" << std::endl;
#endif
size_t decodedFrameLen = 0;
while (decodedFrameLen < replyBufferSize) {
/** First byte already read in */
if (decodedFrameLen != 0) {
byteRead = 0;
if (read(fileDescriptor, &byteRead, 1) != 1) {
sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl;
2023-02-15 17:02:22 +01:00
result = rws::SPI_READ_FAILURE;
2022-01-17 15:58:27 +01:00
break;
}
2021-06-21 09:50:26 +02:00
}
2022-01-17 15:58:27 +01:00
if (byteRead == FLAG_BYTE) {
/** Reached end of frame */
break;
} else if (byteRead == 0x7D) {
if (read(fileDescriptor, &byteRead, 1) != 1) {
sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl;
2023-02-15 17:02:22 +01:00
result = rws::SPI_READ_FAILURE;
2022-01-17 15:58:27 +01:00
break;
}
if (byteRead == 0x5E) {
*(rxBuf + decodedFrameLen) = 0x7E;
decodedFrameLen++;
continue;
} else if (byteRead == 0x5D) {
*(rxBuf + decodedFrameLen) = 0x7D;
decodedFrameLen++;
continue;
} else {
sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl;
2022-10-26 17:11:57 +02:00
closeSpi(fileDescriptor, gpioId, &gpioIF, mutex);
2023-02-15 17:02:22 +01:00
result = rws::INVALID_SUBSTITUTE;
2022-01-17 15:58:27 +01:00
break;
}
} else {
*(rxBuf + decodedFrameLen) = byteRead;
decodedFrameLen++;
continue;
2021-06-21 09:50:26 +02:00
}
2021-06-24 12:04:36 +02:00
/**
2022-01-17 15:58:27 +01:00
* 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.
2021-06-24 12:04:36 +02:00
*/
2022-01-17 15:58:27 +01:00
if (decodedFrameLen == replyBufferSize) {
if (read(fileDescriptor, &byteRead, 1) != 1) {
sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl;
2023-02-15 17:02:22 +01:00
result = rws::SPI_READ_FAILURE;
2022-01-17 15:58:27 +01:00
break;
}
if (byteRead != FLAG_BYTE) {
sif::error << "rwSpiCallback::spiCallback: Missing end sign " << static_cast<int>(FLAG_BYTE)
<< std::endl;
2022-01-17 15:58:27 +01:00
decodedFrameLen--;
2023-02-15 17:02:22 +01:00
result = rws::MISSING_END_SIGN;
2022-01-17 15:58:27 +01:00
break;
}
}
2022-08-24 17:27:47 +02:00
result = returnvalue::OK;
2022-01-17 15:58:27 +01:00
}
2022-01-17 15:58:27 +01:00
cookie->setTransferSize(decodedFrameLen);
2021-06-21 09:50:26 +02:00
2022-10-26 17:11:57 +02:00
closeSpi(fileDescriptor, gpioId, &gpioIF, mutex);
2022-01-17 15:58:27 +01:00
return result;
}
namespace {
ReturnValue_t openSpi(const std::string& devname, int flags, GpioIF* gpioIF, gpioId_t gpioId,
MutexIF* mutex, MutexIF::TimeoutType timeoutType, uint32_t timeoutMs,
int& fd) {
ReturnValue_t result = mutex->lockMutex(timeoutType, timeoutMs);
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
sif::debug << "rwSpiCallback::spiCallback: Failed to lock mutex" << std::endl;
return result;
}
fd = open(devname.c_str(), flags);
if (fd < 0) {
sif::error << "rwSpiCallback::spiCallback: Failed to open device file" << std::endl;
2023-04-07 18:19:33 +02:00
return spi::OPENING_FILE_FAILED;
}
// Pull SPI CS low. For now, no support for active high given
if (gpioId != gpio::NO_GPIO) {
result = gpioIF->pullLow(gpioId);
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
sif::error << "rwSpiCallback::spiCallback: Failed to pull chip select low" << std::endl;
return result;
}
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
}
2022-05-11 15:45:38 +02:00
void closeSpi(int fd, gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex) {
close(fd);
2022-01-17 15:58:27 +01:00
if (gpioId != gpio::NO_GPIO) {
2022-08-24 17:27:47 +02:00
if (gpioIF->pullHigh(gpioId) != returnvalue::OK) {
2022-01-17 15:58:27 +01:00
sif::error << "closeSpi: Failed to pull chip select high" << std::endl;
2021-06-21 09:50:26 +02:00
}
2022-01-17 15:58:27 +01:00
}
2022-08-24 17:27:47 +02:00
if (mutex->unlockMutex() != returnvalue::OK) {
2022-01-17 15:58:27 +01:00
sif::error << "rwSpiCallback::closeSpi: Failed to unlock mutex" << std::endl;
;
}
}
} // namespace
2022-01-17 15:58:27 +01:00
} // namespace rwSpiCallback