237 lines
8.3 KiB
C++
237 lines
8.3 KiB
C++
#include <bsp_q7s/spiCallbacks/rwSpiCallback.h>
|
|
#include <fsfw/serviceinterface/ServiceInterface.h>
|
|
#include <mission/devices/RwHandler.h>
|
|
#include <fsfw/hal/linux/spi/SpiCookie.h>
|
|
#include <fsfw/hal/linux/UnixFileGuard.h>
|
|
#include "devices/gpioIds.h"
|
|
|
|
namespace rwSpiCallback {
|
|
|
|
ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sendData,
|
|
size_t sendLen, void* args) {
|
|
|
|
ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
|
|
|
|
RwHandler* handler = reinterpret_cast<RwHandler*>(args);
|
|
if(handler == nullptr) {
|
|
sif::error << "rwSpiCallback::spiCallback: Pointer to handler is invalid"
|
|
<< std::endl;
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
|
|
uint8_t writeBuffer[2];
|
|
uint8_t writeSize = 0;
|
|
|
|
int fileDescriptor = 0;
|
|
std::string device = cookie->getSpiDevice();
|
|
UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "rwSpiCallback::spiCallback: ");
|
|
if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
|
|
sif::error << "rwSpiCallback::spiCallback: Failed to open device file" << std::endl;
|
|
return SpiComIF::OPENING_FILE_FAILED;
|
|
}
|
|
spi::SpiModes spiMode = spi::SpiModes::MODE_0;
|
|
uint32_t spiSpeed = 0;
|
|
cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
|
|
comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
|
|
|
|
gpioId_t gpioId = cookie->getChipSelectPin();
|
|
GpioIF* gpioIF = comIf->getGpioInterface();
|
|
MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
|
|
uint32_t timeoutMs = 0;
|
|
MutexIF* mutex = comIf->getMutex(&timeoutType, &timeoutMs);
|
|
if(mutex == nullptr or gpioIF == nullptr) {
|
|
sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl;
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
|
|
result = mutex->lockMutex(timeoutType, timeoutMs);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
sif::debug << "rwSpiCallback::spiCallback: Failed to lock mutex" << std::endl;
|
|
return result;
|
|
}
|
|
|
|
/** Disconnect PS SPI peripheral and select AXI SPI core */
|
|
if(gpioIF->pullHigh(gpioIds::SPI_MUX) != HasReturnvaluesIF::RETURN_OK) {
|
|
sif::error << "rwSpiCallback::spiCallback: Failed to pull spi mux gpio high" << std::endl;
|
|
}
|
|
|
|
/** Sending frame start sign */
|
|
writeBuffer[0] = 0x7E;
|
|
writeSize = 1;
|
|
|
|
// Pull SPI CS low. For now, no support for active high given
|
|
if(gpioId != gpio::NO_GPIO) {
|
|
if(gpioIF->pullLow(gpioId) != HasReturnvaluesIF::RETURN_OK) {
|
|
sif::error << "rwSpiCallback::spiCallback: Failed to pull chip select low" << std::endl;
|
|
}
|
|
}
|
|
|
|
if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
|
|
sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
|
|
closeSpi(gpioId, gpioIF, mutex);
|
|
return RwHandler::SPI_WRITE_FAILURE;
|
|
}
|
|
|
|
/** 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;
|
|
}
|
|
if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
|
|
sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
|
|
closeSpi(gpioId, gpioIF, mutex);
|
|
return RwHandler::SPI_WRITE_FAILURE;
|
|
}
|
|
idx++;
|
|
}
|
|
|
|
/** Sending frame end sign */
|
|
writeBuffer[0] = 0x7E;
|
|
writeSize = 1;
|
|
|
|
if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
|
|
sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
|
|
closeSpi(gpioId, gpioIF, mutex);
|
|
return RwHandler::SPI_WRITE_FAILURE;
|
|
}
|
|
|
|
uint8_t* rxBuf = nullptr;
|
|
result = comIf->getReadBuffer(cookie->getSpiAddress(), &rxBuf);
|
|
if(result != HasReturnvaluesIF::RETURN_OK) {
|
|
closeSpi(gpioId, gpioIF, mutex);
|
|
return result;
|
|
}
|
|
|
|
size_t replyBufferSize = cookie->getMaxBufferSize();
|
|
|
|
/** There must be a delay of 20 ms after sending the command */
|
|
usleep(RwDefinitions::SPI_REPLY_DELAY);
|
|
|
|
/**
|
|
* 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 < 10; idx++) {
|
|
if(read(fileDescriptor, &byteRead, 1) != 1) {
|
|
sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl;
|
|
closeSpi(gpioId, gpioIF, mutex);
|
|
return RwHandler::SPI_READ_FAILURE;
|
|
}
|
|
|
|
if (byteRead != 0x7E) {
|
|
break;
|
|
}
|
|
|
|
if (idx == 9) {
|
|
sif::error << "rwSpiCallback::spiCallback: Empty frame timeout" << std::endl;
|
|
closeSpi(gpioId, gpioIF, mutex);
|
|
return RwHandler::NO_REPLY;
|
|
}
|
|
}
|
|
|
|
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;
|
|
result = RwHandler::SPI_READ_FAILURE;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (byteRead == 0x7E) {
|
|
/** Reached end of frame */
|
|
break;
|
|
}
|
|
else if (byteRead == 0x7D) {
|
|
if(read(fileDescriptor, &byteRead, 1) != 1) {
|
|
sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl;
|
|
result = RwHandler::SPI_READ_FAILURE;
|
|
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;
|
|
closeSpi(gpioId, gpioIF, mutex);
|
|
result = RwHandler::INVALID_SUBSTITUTE;
|
|
break;
|
|
}
|
|
}
|
|
else {
|
|
*(rxBuf + decodedFrameLen) = byteRead;
|
|
decodedFrameLen++;
|
|
continue;
|
|
}
|
|
|
|
/**
|
|
* 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 == replyBufferSize) {
|
|
if(read(fileDescriptor, &byteRead, 1) != 1) {
|
|
sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl;
|
|
result = RwHandler::SPI_READ_FAILURE;
|
|
break;
|
|
}
|
|
if (byteRead != 0x7E) {
|
|
sif::error << "rwSpiCallback::spiCallback: Missing end sign 0x7E" << std::endl;
|
|
decodedFrameLen--;
|
|
result = RwHandler::MISSING_END_SIGN;
|
|
break;
|
|
}
|
|
}
|
|
result = HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
cookie->assignTransferSize(decodedFrameLen);
|
|
|
|
closeSpi(gpioId, gpioIF, mutex);
|
|
|
|
return result;
|
|
}
|
|
|
|
void closeSpi (gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex) {
|
|
if(gpioId != gpio::NO_GPIO) {
|
|
if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
|
|
sif::error << "closeSpi: Failed to pull chip select high" << std::endl;
|
|
}
|
|
}
|
|
if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
|
|
sif::error << "rwSpiCallback::closeSpi: Failed to unlock mutex" << std::endl;;
|
|
}
|
|
|
|
/** Route SPI interface again to PS SPI peripheral */
|
|
if(gpioIF->pullLow(gpioIds::SPI_MUX) != HasReturnvaluesIF::RETURN_OK) {
|
|
sif::error << "rwSpiCallback::spiCallback: Failed to pull spi mux gpio low" << std::endl;
|
|
}
|
|
}
|
|
}
|