diff --git a/src/SerialMuxProtServer.hpp b/src/SerialMuxProtServer.hpp index c57a631..533ad81 100644 --- a/src/SerialMuxProtServer.hpp +++ b/src/SerialMuxProtServer.hpp @@ -400,14 +400,16 @@ class SerialMuxProtServer */ void processRxData() { - uint8_t expectedBytes = 0; - uint8_t dlc = 0; + uint8_t expectedBytes = 0; + uint8_t dlc = 0; + bool expectingHeader = false; /* Determine how many bytes to read. */ if (HEADER_LEN > m_receivedBytes) { /* Header must be read. */ - expectedBytes = (HEADER_LEN - m_receivedBytes); + expectedBytes = (HEADER_LEN - m_receivedBytes); + expectingHeader = true; } else { @@ -431,6 +433,27 @@ class SerialMuxProtServer m_receivedBytes += m_stream.readBytes(&m_receiveFrame.raw[m_receivedBytes], expectedBytes); } + if ((HEADER_LEN == m_receivedBytes) && (true == expectingHeader)) + { + /* Header has been read. Get DLC of Rx Channel using Header. */ + dlc = m_receiveFrame.fields.header.headerFields.m_dlc; + + /* DLC = 0 means that the channel does not exist. */ + if ((0U != dlc) && (MAX_RX_ATTEMPTS >= m_rxAttempts)) + { + expectedBytes = (dlc - (m_receivedBytes - HEADER_LEN)); + m_rxAttempts++; + } + + if (0U != expectedBytes) + { + if (expectedBytes <= m_stream.available()) + { + m_receivedBytes += m_stream.readBytes(&m_receiveFrame.raw[m_receivedBytes], expectedBytes); + } + } + } + /* Frame has been received. */ if ((0U != dlc) && ((HEADER_LEN + dlc) == m_receivedBytes)) {