heading towards completion for low level rw polling task

linux/devices/RwPollingTask.cpp

@@ -15,35 +15,66 @@ RwPollingTask::RwPollingTask(object_id_t objectId, SpiComIF* spiIF)
   semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
   semaphore->acquire();
   ipcLock = MutexFactory::instance()->createMutex();
+  spiLock = spiIF->getCsMutex();
+  spiDev = spiIF->getSpiDev().c_str();
 }
 
 ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) {
-  ipcLock->lockMutex();
-  state = InternalState::IDLE;
-  ipcLock->unlockMutex();
   while (true) {
+    ipcLock->lockMutex();
+    state = InternalState::IDLE;
+    ipcLock->unlockMutex();
     semaphore->acquire();
+    int fd = 0;
+    ReturnValue_t result = openSpi(O_RDWR, fd);
+    if (result != returnvalue::OK) {
+      continue;
+    }
     for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
       prepareSetSpeedCmd(idx);
-      writeOneRw(idx);
+      if (writeOneRwCmd(idx, fd) != returnvalue::OK) {
+        continue;
+      }
     }
-    readAllRws(fd, spiLock, dev)
-        // writeAndReadAllRws(sendData, sendDataLen)
-        int bytesRead = 0;
+    closeSpi(fd);
+    usleep(rws::SPI_REPLY_DELAY);
+    if (readAllRws(fd, rws::SET_SPEED) != returnvalue::OK) {
+      continue;
+    }
+    prepareSimpleCommand(rws::GET_LAST_RESET_STATUS);
+    if (writeAndReadAllRws(rws::GET_LAST_RESET_STATUS) != returnvalue::OK) {
+      continue;
+    }
+    prepareSimpleCommand(rws::GET_RW_STATUS);
+    if (writeAndReadAllRws(rws::GET_RW_STATUS) != returnvalue::OK) {
+      continue;
+    }
+    prepareSimpleCommand(rws::GET_TEMPERATURE);
+    if (writeAndReadAllRws(rws::GET_TEMPERATURE) != returnvalue::OK) {
+      continue;
+    }
+    prepareSimpleCommand(rws::CLEAR_LAST_RESET_STATUS);
+    if (writeAndReadAllRws(rws::CLEAR_LAST_RESET_STATUS) != returnvalue::OK) {
+      continue;
+    }
+  }
+
+  return returnvalue::OK;
+}
+
+ReturnValue_t RwPollingTask::initialize() {
+  if (spiDev == nullptr) {
+    sif::error << "SPI device is invalid" << std::endl;
+    return returnvalue::FAILED;
   }
   return returnvalue::OK;
 }
 
-ReturnValue_t RwPollingTask::initialize() { return returnvalue::OK; }
-
 ReturnValue_t RwPollingTask::initializeInterface(CookieIF* cookie) {
-  spiIF->getSpiDev();
-  // 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 modeAndSpeedWasSet) {
-    auto& dev = spiIF->getSpiDev();
-    int fd = open(dev.c_str(), O_RDWR);
+    int fd = open(spiDev, O_RDWR);
     if (fd < 0) {
       sif::error << "could not open RW SPI bus" << std::endl;
       return returnvalue::FAILED;
@@ -66,16 +97,20 @@ ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendDa
   SerializeAdapter::deSerialize(&speed, &sendData, &sendLen, SerializeIF::Endianness::MACHINE);
   SerializeAdapter::deSerialize(&rampTime, &sendData, &sendLen, SerializeIF::Endianness::MACHINE);
   rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE;
-  if (sendLen == 7 and sendData[6] < rws::SpecialRwRequest::NUM_REQUESTS) {
+  if (sendLen == 7 and sendData[6] < static_cast<uint8_t>(rws::SpecialRwRequest::NUM_REQUESTS)) {
     specialRequest = static_cast<rws::SpecialRwRequest>(sendData[6]);
   }
   RwCookie* rwCookie = dynamic_cast<RwCookie*>(cookie);
+  if (rwCookie == nullptr) {
+    return returnvalue::FAILED;
+  }
   {
     MutexGuard mg(ipcLock);
     rwCookie->currentRwSpeed = speed;
     rwCookie->currentRampTime = rampTime;
     rwCookie->specialRequest = specialRequest;
     if (state == InternalState::IDLE and rwCookie->rwIdx == 3) {
+      state = InternalState::BUSY;
       semaphore->release();
     }
   }
@@ -98,19 +133,17 @@ ReturnValue_t RwPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** buf
   return returnvalue::OK;
 }
 
-ReturnValue_t RwPollingTask::writeAndReadAllRws(const uint8_t* sendData, size_t sendDataLen) {
+ReturnValue_t RwPollingTask::writeAndReadAllRws(DeviceCommandId_t id) {
   // Stopwatch watch;
   ReturnValue_t result = returnvalue::OK;
 
   int fd = 0;
-  const std::string& dev = spiIF->getSpiDev();
-  MutexIF* spiLock = spiIF->getCsMutex();
-  result = openSpi(dev, O_RDWR, fd);
+  result = openSpi(O_RDWR, fd);
   if (result != returnvalue::OK) {
     return result;
   }
   for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
-    ReturnValue_t result = sendOneMessage(fd, *rwCookies[idx], spiLock, sendData, sendDataLen);
+    ReturnValue_t result = sendOneMessage(fd, *rwCookies[idx]);
     if (result != returnvalue::OK) {
       closeSpi(fd);
       return returnvalue::FAILED;
@@ -119,11 +152,11 @@ ReturnValue_t RwPollingTask::writeAndReadAllRws(const uint8_t* sendData, size_t
 
   closeSpi(fd);
   usleep(rws::SPI_REPLY_DELAY);
-  return readAllRws(fd, spiLock, dev.c_str());
+  return readAllRws(fd, id);
 }
 
-ReturnValue_t RwPollingTask::openSpi(const std::string& devname, int flags, int& fd) {
-  fd = open(devname.c_str(), flags);
+ReturnValue_t RwPollingTask::openSpi(int flags, int& fd) {
+  fd = open(spiDev, flags);
   if (fd < 0) {
     sif::error << "rwSpiCallback::spiCallback: Failed to open device file" << std::endl;
     return SpiComIF::OPENING_FILE_FAILED;
@@ -132,15 +165,15 @@ ReturnValue_t RwPollingTask::openSpi(const std::string& devname, int flags, int&
   return returnvalue::OK;
 }
 
-ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCookie, MutexIF* spiLock,
-                                           uint8_t* replyBuf) {
+ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf,
+                                           size_t maxReplyLen) {
   ReturnValue_t result = returnvalue::OK;
   int fd = 0;
   gpioId_t gpioId = rwCookie.getChipSelectPin();
   GpioIF& gpioIF = spiIF->getGpioInterface();
   pullCsLow(gpioId, spiLock, gpioIF);
   for (unsigned idx = 0; idx < MAX_RETRIES_REPLY; idx++) {
-    result = openSpi(spiDev, O_RDWR, fd);
+    result = openSpi(O_RDWR, fd);
     if (result != returnvalue::OK) {
       return result;
     }
@@ -184,7 +217,7 @@ ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCooki
 
     size_t decodedFrameLen = 0;
 
-    while (decodedFrameLen < processingBuf.size()) {
+    while (decodedFrameLen < maxReplyLen) {
       /** First byte already read in */
       if (decodedFrameLen != 0) {
         byteRead = 0;
@@ -205,11 +238,11 @@ ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCooki
           break;
         }
         if (byteRead == 0x5E) {
-          *(processingBuf.data() + decodedFrameLen) = 0x7E;
+          *(replyBuf + decodedFrameLen) = 0x7E;
           decodedFrameLen++;
           continue;
         } else if (byteRead == 0x5D) {
-          *(processingBuf.data() + decodedFrameLen) = 0x7D;
+          *(replyBuf + decodedFrameLen) = 0x7D;
           decodedFrameLen++;
           continue;
         } else {
@@ -219,7 +252,7 @@ ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCooki
           break;
         }
       } else {
-        *(processingBuf.data() + decodedFrameLen) = byteRead;
+        *(replyBuf + decodedFrameLen) = byteRead;
         decodedFrameLen++;
         continue;
       }
@@ -229,7 +262,7 @@ ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCooki
        * replaced by its substitute. Than the next byte must correspond to the end sign 0x7E.
        * Otherwise there might be something wrong.
        */
-      if (decodedFrameLen == processingBuf.size()) {
+      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;
@@ -249,16 +282,8 @@ ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCooki
   return returnvalue::OK;
 }
 
-ReturnValue_t RwPollingTask::writeOneRw(uint8_t rwIdx) {
-  int fd = 0;
-  const std::string& dev = spiIF->getSpiDev();
-  MutexIF* spiLock = spiIF->getCsMutex();
-  ReturnValue_t result = openSpi(dev, O_RDWR, fd);
-  if (result != returnvalue::OK) {
-    return result;
-  }
-  ReturnValue_t result =
-      sendOneMessage(fd, *rwCookies[rwIdx], spiLock, writeBuffer.data(), writeLen);
+ReturnValue_t RwPollingTask::writeOneRwCmd(uint8_t rwIdx, int fd) {
+  ReturnValue_t result = sendOneMessage(fd, *rwCookies[rwIdx]);
   if (result != returnvalue::OK) {
     closeSpi(fd);
     return returnvalue::FAILED;
@@ -266,20 +291,83 @@ ReturnValue_t RwPollingTask::writeOneRw(uint8_t rwIdx) {
   return returnvalue::OK;
 }
 
-ReturnValue_t RwPollingTask::readAllRws(int fd, MutexIF* spiLock, const char* dev) {
+ReturnValue_t RwPollingTask::readAllRws(int fd, DeviceCommandId_t id) {
+  ReturnValue_t result = openSpi(O_RDWR, fd);
+  if (result != returnvalue::OK) {
+    return result;
+  }
   for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
     if (spiLock == nullptr) {
       sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl;
       return returnvalue::FAILED;
     }
+    // TODO: Fix buffer to write to
     uint8_t* replyBuf;
-    readNextReply(dev, *rwCookies[idx], spiLock, replyBuf);
+    size_t maxReadLen = idAndIdxToReadBuffer(id, idx, &replyBuf);
+    readNextReply(*rwCookies[idx], replyBuf, maxReadLen);
   }
 
   closeSpi(fd);
   return returnvalue::OK;
 }
 
+size_t RwPollingTask::idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, uint8_t** ptr) {
+  uint8_t* rawStart = rwCookies[rwIdx]->replyBuf.data();
+  RwReplies replies(rawStart);
+  switch (id) {
+    case (rws::GET_RW_STATUS): {
+      *ptr = replies.rwStatusReply;
+      return rws::SIZE_GET_RW_STATUS;
+    }
+    case (rws::SET_SPEED): {
+      *ptr = replies.setSpeedReply;
+      return rws::SIZE_SET_SPEED_REPLY;
+    }
+    case (rws::CLEAR_LAST_RESET_STATUS): {
+      *ptr = replies.clearLastResetStatusReply;
+      return rws::SIZE_CLEAR_RESET_STATUS;
+    }
+    case (rws::GET_LAST_RESET_STATUS): {
+      *ptr = replies.getLastResetStatusReply;
+      return rws::SIZE_GET_RESET_STATUS;
+    }
+    case (rws::GET_TEMPERATURE): {
+      *ptr = replies.readTemperatureReply;
+      return rws::SIZE_GET_TEMPERATURE_REPLY;
+    }
+    case (rws::GET_TM): {
+      *ptr = replies.hkDataReply;
+      return rws::SIZE_GET_TELEMETRY_REPLY;
+    }
+    case (rws::INIT_RW_CONTROLLER): {
+      *ptr = replies.initRwControllerReply;
+      return rws::SIZE_INIT_RW;
+    }
+    default: {
+      sif::error << "no reply buffer for rw command " << id << std::endl;
+      *ptr = replies.dummyPointer;
+      return 0;
+    }
+  }
+}
+
+void RwPollingTask::encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, size_t& encodedLen) {
+  encodedBuffer[0] = FLAG_BYTE;
+  encodedLen = 1;
+  for (size_t sourceIdx = 0; sourceIdx < sourceLen; sourceIdx++) {
+    if (sourceBuf[sourceIdx] == 0x7E) {
+      encodedBuffer[encodedLen++] = 0x7D;
+      encodedBuffer[encodedLen++] = 0x5E;
+    } else if (sourceBuf[sourceIdx] == 0x7D) {
+      encodedBuffer[encodedLen++] = 0x7D;
+      encodedBuffer[encodedLen++] = 0x5D;
+    } else {
+      encodedBuffer[encodedLen++] = sourceBuf[sourceIdx];
+    }
+  }
+  encodedBuffer[encodedLen++] = FLAG_BYTE;
+}
+
 // This closes the SPI
 void RwPollingTask::closeSpi(int fd) {
   // This will perform the function to close the SPI
@@ -287,8 +375,7 @@ void RwPollingTask::closeSpi(int fd) {
   // The SPI is now closed.
 }
 
-ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie, MutexIF* spiLock,
-                                            const uint8_t* data, size_t dataLen) {
+ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) {
   gpioId_t gpioId = rwCookie.getChipSelectPin();
   GpioIF& gpioIF = spiIF->getGpioInterface();
   if (spiLock == nullptr) {
@@ -296,15 +383,8 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie, MutexIF*
     return returnvalue::FAILED;
   }
   pullCsLow(gpioId, spiLock, gpioIF);
-  /** Sending frame start sign */
-  writeBuffer[0] = FLAG_BYTE;
-  size_t writeSize = 1;
-  if (write(fd, writeBuffer.data(), writeSize) != static_cast<ssize_t>(writeSize)) {
-    sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
-    pullCsHigh(gpioId, spiLock, gpioIF);
-    return rws::SPI_WRITE_FAILURE;
-  }
-  /** Encoding and sending command */
+  /*
+  //Encoding and sending command
   size_t idx = 0;
   while (idx < dataLen) {
     switch (*(data + idx)) {
@@ -324,21 +404,16 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie, MutexIF*
         break;
     }
   }
-  if (write(fd, writeBuffer.data(), writeSize) != static_cast<ssize_t>(writeSize)) {
-    sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
-    pullCsHigh(gpioId, spiLock, gpioIF);
-    return rws::SPI_WRITE_FAILURE;
-  }
-  idx++;
-  /** Sending frame end sign */
-  writeBuffer[0] = FLAG_BYTE;
-  writeSize = 1;
-
-  if (write(fd, writeBuffer.data(), writeSize) != static_cast<ssize_t>(writeSize)) {
+  */
+  // Add datalinklayer like specified in the datasheet.
+  size_t lenToSend = 0;
+  encodeHdlc(writeBuffer.data(), writeLen, lenToSend);
+  if (write(fd, encodedBuffer.data(), lenToSend) != static_cast<ssize_t>(lenToSend)) {
     sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
     pullCsHigh(gpioId, spiLock, gpioIF);
     return rws::SPI_WRITE_FAILURE;
   }
+  pullCsHigh(gpioId, spiLock, gpioIF);
   return returnvalue::OK;
 }
 
@@ -376,6 +451,7 @@ void RwPollingTask::prepareSimpleCommand(DeviceCommandId_t id) {
   uint16_t crc = CRC::crc16ccitt(writeBuffer.data(), 1, 0xFFFF);
   writeBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
   writeBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
+  writeLen = 3;
 }
 
 ReturnValue_t RwPollingTask::prepareSetSpeedCmd(uint8_t rwIdx) {
@@ -397,5 +473,6 @@ ReturnValue_t RwPollingTask::prepareSetSpeedCmd(uint8_t rwIdx) {
   uint16_t crc = CRC::crc16ccitt(writeBuffer.data(), 7, 0xFFFF);
   writeBuffer[7] = static_cast<uint8_t>(crc & 0xFF);
   writeBuffer[8] = static_cast<uint8_t>((crc >> 8) & 0xFF);
+  writeLen = 9;
   return returnvalue::OK;
 }

linux/devices/RwPollingTask.h

@@ -14,15 +14,16 @@ class RwCookie : public SpiCookie {
   friend class RwPollingTask;
 
  public:
+  static constexpr size_t REPLY_BUF_LEN = 524;
   RwCookie(uint8_t rwIdx, address_t spiAddress, gpioId_t chipSelect, const size_t maxSize,
            spi::SpiModes spiMode, uint32_t spiSpeed)
       : SpiCookie(spiAddress, chipSelect, maxSize, spiMode, spiSpeed), rwIdx(rwIdx) {}
 
  private:
-  std::array<uint8_t, 524> replyBuf{};
+  std::array<uint8_t, REPLY_BUF_LEN> replyBuf{};
   int32_t currentRwSpeed = 0;
   uint16_t currentRampTime = 0;
-  rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::NONE;
+  rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE;
   uint8_t rwIdx;
 };
 
@@ -39,24 +40,26 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev
   bool debugMode = false;
   bool modeAndSpeedWasSet = false;
   MutexIF* ipcLock;
+  MutexIF* spiLock;
+  const char* spiDev;
   SpiComIF* spiIF;
   std::array<RwCookie*, 4> rwCookies;
   std::array<uint8_t, rws::MAX_CMD_SIZE> writeBuffer;
+  std::array<uint8_t, rws::MAX_CMD_SIZE * 2> encodedBuffer;
+
   size_t writeLen = 0;
-  std::array<uint8_t, 256> processingBuf;
   //! This is the end and start marker of the frame datalinklayer
   static constexpr uint8_t FLAG_BYTE = 0x7E;
   static constexpr MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING;
   static constexpr uint32_t TIMEOUT_MS = 20;
   static constexpr uint8_t MAX_RETRIES_REPLY = 5;
 
-  ReturnValue_t writeAndReadAllRws(const uint8_t* sendData, size_t sendDataLen);
-  ReturnValue_t writeOneRw(uint8_t rwIdx);
-  ReturnValue_t readAllRws(int fd, MutexIF* spiLock, const char* dev);
-  ReturnValue_t sendOneMessage(int fd, RwCookie& rwCookie, MutexIF* spiLock, const uint8_t* data,
-                               size_t dataLen);
-  ReturnValue_t readNextReply(const char* spiDev, RwCookie& rwCookie, MutexIF* spiLock,
-                              uint8_t* replyBuf);
+  ReturnValue_t writeAndReadAllRws(DeviceCommandId_t id);
+  ReturnValue_t writeOneRwCmd(uint8_t rwIdx, int fd);
+  ReturnValue_t readAllRws(int fd, DeviceCommandId_t id);
+
+  ReturnValue_t sendOneMessage(int fd, RwCookie& rwCookie);
+  ReturnValue_t readNextReply(RwCookie& rwCookie, uint8_t* replyBuf, size_t maxReplyLen);
   ReturnValue_t initializeInterface(CookieIF* cookie) override;
 
   ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override;
@@ -66,11 +69,14 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev
   ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override;
 
   ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
-  ReturnValue_t openSpi(const std::string& devname, int flags, int& fd);
+  ReturnValue_t openSpi(int flags, int& fd);
   ReturnValue_t pullCsLow(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF);
   void prepareSimpleCommand(DeviceCommandId_t id);
   ReturnValue_t prepareSetSpeedCmd(uint8_t rwIdx);
 
+  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 closeSpi(int);
 };

mission/devices/devicedefinitions/rwHelpers.h

@@ -19,7 +19,7 @@ static const size_t SIZE_GET_TEMPERATURE_REPLY = 8;
 /** Max size when requesting telemetry */
 static const size_t SIZE_GET_TELEMETRY_REPLY = 91;
 
-enum SpecialRwRequest : uint8_t {
+enum class SpecialRwRequest : uint8_t {
   REQUEST_NONE = 0,
   RESET_MCU = 1,
   INIT_RW_CONTROLLER = 2,
@@ -27,47 +27,6 @@ enum SpecialRwRequest : uint8_t {
   NUM_REQUESTS
 };
 
-struct RwReplies {
-  friend class RwPollingTask;
-
- public:
-  RwReplies(const uint8_t* rawData) : rawData(rawData) {
-    rwStatusReply = rawData;
-    setSpeedReply = rawData + SIZE_GET_RW_STATUS;
-    getLastResetStatusReply = setSpeedReply + SIZE_SET_SPEED_REPLY;
-    clearLastResetStatusReply = getLastResetStatusReply + SIZE_GET_RESET_STATUS;
-    readTemperatureReply = clearLastResetStatusReply + SIZE_CLEAR_RESET_STATUS;
-    hkDataReply = readTemperatureReply + SIZE_GET_TEMPERATURE_REPLY;
-    initRwControllerReply = hkDataReply + SIZE_GET_TELEMETRY_REPLY;
-  }
-
-  const uint8_t* getClearLastResetStatusReply() const { return clearLastResetStatusReply; }
-
-  const uint8_t* getGetLastResetStatusReply() const { return getLastResetStatusReply; }
-
-  const uint8_t* getHkDataReply() const { return hkDataReply; }
-
-  const uint8_t* getInitRwControllerReply() const { return initRwControllerReply; }
-
-  const uint8_t* getRawData() const { return rawData; }
-
-  const uint8_t* getReadTemperatureReply() const { return readTemperatureReply; }
-
-  const uint8_t* getRwStatusReply() const { return rwStatusReply; }
-
-  const uint8_t* getSetSpeedReply() const { return setSpeedReply; }
-
- private:
-  const uint8_t* rawData;
-  const uint8_t* rwStatusReply;
-  const uint8_t* setSpeedReply;
-  const uint8_t* getLastResetStatusReply;
-  const uint8_t* clearLastResetStatusReply;
-  const uint8_t* readTemperatureReply;
-  const uint8_t* hkDataReply;
-  const uint8_t* initRwControllerReply;
-};
-
 static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER;
 
 static const ReturnValue_t SPI_WRITE_FAILURE = MAKE_RETURN_CODE(0xB0);
@@ -299,4 +258,50 @@ class RwSpeedActuationSet : public StaticLocalDataSet<2> {
 
 }  // namespace rws
 
+struct RwReplies {
+  friend class RwPollingTask;
+
+ public:
+  RwReplies(const uint8_t* rawData) : rawData(const_cast<uint8_t*>(rawData)) { initPointers(); }
+
+  const uint8_t* getClearLastResetStatusReply() const { return clearLastResetStatusReply; }
+
+  const uint8_t* getGetLastResetStatusReply() const { return getLastResetStatusReply; }
+
+  const uint8_t* getHkDataReply() const { return hkDataReply; }
+
+  const uint8_t* getInitRwControllerReply() const { return initRwControllerReply; }
+
+  const uint8_t* getRawData() const { return rawData; }
+
+  const uint8_t* getReadTemperatureReply() const { return readTemperatureReply; }
+
+  const uint8_t* getRwStatusReply() const { return rwStatusReply; }
+
+  const uint8_t* getSetSpeedReply() const { return setSpeedReply; }
+
+ private:
+  RwReplies(uint8_t* rwData) : rawData(rwData) { initPointers(); }
+
+  void initPointers() {
+    rwStatusReply = rawData;
+    setSpeedReply = rawData + rws::SIZE_GET_RW_STATUS;
+    getLastResetStatusReply = setSpeedReply + rws::SIZE_SET_SPEED_REPLY;
+    clearLastResetStatusReply = getLastResetStatusReply + rws::SIZE_GET_RESET_STATUS;
+    readTemperatureReply = clearLastResetStatusReply + rws::SIZE_CLEAR_RESET_STATUS;
+    hkDataReply = readTemperatureReply + rws::SIZE_GET_TEMPERATURE_REPLY;
+    initRwControllerReply = hkDataReply + rws::SIZE_GET_TELEMETRY_REPLY;
+    dummyPointer = initRwControllerReply + rws::SIZE_INIT_RW;
+  }
+  uint8_t* rawData;
+  uint8_t* rwStatusReply;
+  uint8_t* setSpeedReply;
+  uint8_t* getLastResetStatusReply;
+  uint8_t* clearLastResetStatusReply;
+  uint8_t* readTemperatureReply;
+  uint8_t* hkDataReply;
+  uint8_t* initRwControllerReply;
+  uint8_t* dummyPointer;
+};
+
 #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_H_ */