fusion_engine_framer.cc

Go to the documentation of this file.

/**************************************************************************/ /**
 * @brief FusionEngine message framer.
 * @file
 ******************************************************************************/
 
#define P1_VMODULE_NAME fusion_engine_framer
 
#include "point_one/fusion_engine/parsers/fusion_engine_framer.h"
 
#include <cstring> // For memmove()
#if P1_HAVE_STD_OSTREAM
#  include <iomanip>
#  include <ostream>
#  include <type_traits>
#endif
 
#include "point_one/fusion_engine/common/logging.h"
#include "point_one/fusion_engine/messages/crc.h"
 
using namespace point_one::fusion_engine::messages;
using namespace point_one::fusion_engine::parsers;
 
/******************************************************************************/
template <typename T>
class HexPrintableIntegerInst {
 public:
  HexPrintableIntegerInst(T value) : value_(value) {}
 
  template <typename U> // all instantiations of this template are my friends
  friend p1_ostream& operator<<(p1_ostream&, const HexPrintableIntegerInst<U>&);
 
 private:
  const T value_;
};
 
/******************************************************************************/
template <typename T>
p1_ostream& operator<<(p1_ostream& stream,
                       const HexPrintableIntegerInst<T>& obj) {
#if P1_HAVE_STD_OSTREAM
  static_assert(std::is_integral<T>::value, "Integer required.");
 
  stream << "0x" << std::hex << std::setfill('0') << std::setw(sizeof(obj) * 2);
 
  if (sizeof(T) == 1) {
    stream << (((unsigned)obj.value_) & 0xFF);
  } else {
    stream << obj.value_;
  }
 
  stream << std::dec;
 
  if (sizeof(obj) == 1) {
    if (obj.value_ >= 0x20 && obj.value_ <= 0x7E) {
      stream << " ('" << (char)obj.value_ << "')";
    } else {
      stream << " (---)";
    }
  }
#endif
  return stream;
}
 
/**
 * @brief Wrap an integer so it will be output to a stream as its hex
 *        representation.
 *
 * For example:
 *
 * ```cpp
 * std::cout << HexPrintableValue((int16_t)-255) << std::endl;
 * std::cout << HexPrintableValue((uint32_t)255) << std::endl;
 * std::cout << HexPrintableValue((uint8_t)48) << std::endl;
 * ```
 *
 * generates the following output:
 *
 * ```
 * 0xff01
 * 0x000000ff
 * 0x30 ('0')
 * ```
 *
 * @tparam T The type of the value parameter (inferred implicitly).
 * @param value The integer value to wrap.
 *
 * @return The wrapped integer that can be used in an @ref p1_ostream.
 */
template <typename T>
HexPrintableIntegerInst<T> HexPrintableInteger(T value) {
  return HexPrintableIntegerInst<T>(value);
}
 
/******************************************************************************/
FusionEngineFramer::FusionEngineFramer(void* buffer, size_t capacity_bytes) {
  // Allocate a buffer internally.
  if (buffer == nullptr) {
    // We enforce 4B alignment, so we need to allocate 3 extra bytes to
    // guarantee that the buffer has at least the requested capacity.
    SetBuffer(nullptr, capacity_bytes + 3);
  }
  // Use user-provided storage.
  else {
    SetBuffer(buffer, capacity_bytes);
  }
}
 
/******************************************************************************/
FusionEngineFramer::~FusionEngineFramer() { ClearManagedBuffer(); }
 
/******************************************************************************/
void FusionEngineFramer::SetBuffer(void* buffer, size_t capacity_bytes) {
  if (capacity_bytes < sizeof(MessageHeader)) {
    LOG(ERROR) << "FusionEngine framing buffer too small. [capacity="
               << capacity_bytes << " B, min=" << sizeof(MessageHeader)
               << " B]";
    return;
  }
  // Restrict the buffer capacity to 2^31 bytes. We don't expect to ever have a
  // single message anywhere near that large, and don't expect users to ever
  // pass in a buffer that size. Using uint32_t instead of size_t internally
  // makes it easier to guarantee behavior between 64b and 32b architectures. We
  // do 2^31, not 2^32, so we can use int32_t for return values internally.
  else if (capacity_bytes > 0x7FFFFFFF) {
    LOG(WARNING) << "Limiting buffer capacity to 2^31 B. [original_capacity="
                 << capacity_bytes << " B]";
    capacity_bytes = 0x7FFFFFFF;
  }
 
  ClearManagedBuffer();
  if (buffer == nullptr) {
    buffer = new uint8_t[capacity_bytes];
    is_buffer_managed_ = true;
  }
 
  // Enforce 4B alignment at the beginning of the buffer.
  uint8_t* buffer_unaligned = static_cast<uint8_t*>(buffer);
  buffer_ = reinterpret_cast<uint8_t*>(
      (reinterpret_cast<size_t>(buffer_unaligned) + 3) &
      ~(static_cast<size_t>(3)));
  capacity_bytes_ =
      static_cast<uint32_t>(capacity_bytes - (buffer_ - buffer_unaligned));
 
  Reset();
}
 
/******************************************************************************/
void FusionEngineFramer::Reset() {
  state_ = State::SYNC0;
  next_byte_index_ = 0;
  current_message_size_ = 0;
}
 
/******************************************************************************/
size_t FusionEngineFramer::OnData(const uint8_t* buffer, size_t length_bytes) {
  // Process each byte. If the user-supplied buffer was too small, we can't
  // parse messages.
  if (buffer_ != nullptr) {
    VLOG(2) << "Received " << length_bytes << " bytes.";
    size_t total_dispatched_bytes = 0;
    for (size_t idx = 0; idx < length_bytes; ++idx) {
      uint8_t byte = buffer[idx];
      buffer_[next_byte_index_++] = byte;
      int32_t dispatched_message_size = OnByte(false);
      if (dispatched_message_size == 0) {
        // Waiting for more data. Nothing to do.
      } else if (dispatched_message_size > 0) {
        // Message framed successfully. Reset for the next one.
        next_byte_index_ = 0;
        total_dispatched_bytes += (size_t)dispatched_message_size;
      } else if (next_byte_index_ > 0) {
        // If OnByte() indicated an error (size < 0) and there is still data in
        // the buffer, either the CRC failed or the payload was too big to fit
        // in the buffer.
        //
        // In either case, it is possible we found what looked like the preamble
        // somewhere within the data stream but it wasn't actually a valid
        // message. In that case, the data we processed may contain the start of
        // a valid message, or even one or more complete messages, starting
        // somewhere after byte 0. Perform a resync operation to find valid
        // messages.
        total_dispatched_bytes += Resync();
      } else {
        // OnByte() caught an unrecoverable error and reset the buffer. Nothing
        // to do.
      }
    }
    return total_dispatched_bytes;
  } else {
    return 0;
  }
}
 
/******************************************************************************/
int32_t FusionEngineFramer::OnByte(bool quiet) {
  // User-supplied buffer was too small. Can't parse messages.
  if (buffer_ == nullptr) {
    return 0;
  }
 
  // If warnings are disabled, run in quiet mode.
  if (!warn_on_error_) {
    quiet = true;
  }
 
  // Pull out the byte being processed.
  if (next_byte_index_ == 0) {
    LOG(ERROR) << "Byte not found in buffer.";
    return 0;
  }
 
  uint8_t byte = buffer_[next_byte_index_ - 1];
 
  // Look for the first sync byte.
  //
  // Note that we always put the first byte at offset 0 in the buffer, and the
  // buffer is guaranteed to be 4B-aligned by the constructor, so the framed
  // message will always be 4B aligned.
  bool crc_check_needed = false;
  if (state_ == State::SYNC0) {
    VLOG(4) << "Searching for sync byte 0. [byte=" << HexPrintableInteger(byte)
            << "]";
    if (byte == MessageHeader::SYNC0) {
      VLOG(4) << "Found sync byte 0.";
      state_ = State::SYNC1;
    } else {
      --next_byte_index_;
    }
  }
  // Look for the second sync byte.
  else if (state_ == State::SYNC1) {
    VLOG(4) << "Searching for sync byte 1. [byte=" << HexPrintableInteger(byte)
            << "]";
    if (byte == MessageHeader::SYNC0) {
      VLOG(4) << "Found duplicate sync byte 0.";
      state_ = State::SYNC1;
      --next_byte_index_;
    } else if (byte == MessageHeader::SYNC1) {
      VLOG(3) << "Preamble found. Waiting for header.";
      state_ = State::HEADER;
    } else {
      VLOG(4) << "Did not find sync byte 1. Resetting. [byte="
              << HexPrintableInteger(byte) << "]";
      state_ = State::SYNC0;
      next_byte_index_ = 0;
      current_message_size_ = 0;
    }
  }
  // Search for a message header.
  else if (state_ == State::HEADER) {
    VLOG(4) << "Received " << next_byte_index_ << "/" << sizeof(MessageHeader)
            << " header bytes. [byte=" << HexPrintableInteger(byte) << "]";
 
    // Check if the header is complete.
    if (next_byte_index_ == sizeof(MessageHeader)) {
      auto* header = reinterpret_cast<MessageHeader*>(buffer_);
      current_message_size_ =
          sizeof(MessageHeader) + header->payload_size_bytes;
      VLOG(3) << "Header complete. Waiting for payload. [message="
              << header->message_type << ", seq=" << header->sequence_number
              << ", payload_size=" << header->payload_size_bytes
              << " B, message_size=" << current_message_size_ << " B]";
 
      // Check for overflow of the messge size uint32_t variable. We don't
      // currently expect to have _extremely_ large packets, so this should
      // never happen for a valid message. If it does, assume this is not a
      // valid message, and instead the sync pattern likely showed up at random
      // in the data stream.
      if (current_message_size_ < header->payload_size_bytes) {
        if (quiet) {
          VLOG(2) << "Message size overflow. Dropping suspected invalid sync. "
                     "[size="
                  << current_message_size_
                  << " B (payload=" << header->payload_size_bytes << " B)]";
        } else {
          LOG(WARNING)
              << "Message size overflow. Dropping suspected invalid sync. "
                 "[size="
              << current_message_size_
              << " B (payload=" << header->payload_size_bytes << " B)]";
        }
 
        state_ = State::SYNC0;
        return -1;
      }
      // The reserved bytes in the header are currently always set to 0. If the
      // incoming bytes are not zero, assume this an invalid sync.
      //
      // This may change in the future, but for now it prevents us from
      // needing to collect a ton of bytes before performing a CRC check if a
      // bogus header from an invalid sync has a very large payload size that
      // happens to still be smaller than the buffer size.
      else if (header->reserved[0] != 0 || header->reserved[1] != 0) {
        if (quiet) {
          VLOG(2) << "Reserved bytes nonzero. Dropping suspected invalid sync. "
                     "[size="
                  << current_message_size_
                  << " B (payload=" << header->payload_size_bytes << " B)]";
        } else {
          LOG(WARNING) << "Reserved bytes nonzero. Dropping suspected invalid "
                          "sync. [size="
                       << current_message_size_
                       << " B (payload=" << header->payload_size_bytes
                       << " B)]";
        }
 
        state_ = State::SYNC0;
        return -1;
      }
      // If the message is too large to fit in the buffer, we cannot parse it.
      //
      // If this is an invalid sync, the parsed (invalid) payload length may
      // exceed the buffer size and the invalid header will be dropped. If it
      // does not exceed the buffer size, it'll get caught later during the CRC
      // check.
      else if (current_message_size_ > capacity_bytes_) {
        if (quiet) {
          VLOG(2) << "Message too large for buffer. [size="
                  << current_message_size_
                  << " B (payload=" << header->payload_size_bytes
                  << " B), buffer_capacity=" << capacity_bytes_
                  << " B (max_payload="
                  << capacity_bytes_ - sizeof(MessageHeader) << " B)]";
        } else {
          LOG(WARNING) << "Message too large for buffer. [size="
                       << current_message_size_
                       << " B (payload=" << header->payload_size_bytes
                       << " B), buffer_capacity=" << capacity_bytes_
                       << " B (max_payload="
                       << capacity_bytes_ - sizeof(MessageHeader) << " B)]";
        }
 
        state_ = State::SYNC0;
        return -1;
      }
      // Sanity checks passed. Start collecting the message payload next.
      else {
        // If there's no payload, do the CRC check now.
        if (header->payload_size_bytes == 0) {
          VLOG(3) << "Message has no payload. Checking CRC.";
          crc_check_needed = true;
        }
        // Otherwise, collect the payload, then do the CRC check.
        else {
          state_ = State::DATA;
        }
      }
    }
  }
  // Collect the message payload.
  else if (state_ == State::DATA) {
    VLOG(4) << "Received " << next_byte_index_ << "/" << current_message_size_
            << " message bytes (" << next_byte_index_ - sizeof(MessageHeader)
            << "/" << current_message_size_ - sizeof(MessageHeader)
            << " payload bytes). [byte=" << HexPrintableInteger(byte) << "]";
 
    // If we received the full payload, check the CRC and dispatch it.
    if (next_byte_index_ == current_message_size_) {
      VLOG(3) << "Payload complete. Checking CRC.";
      crc_check_needed = true;
    }
  }
  // Illegal state.
  else {
    LOG(ERROR) << "Impossible parsing state.";
    Reset();
    return -1;
  }
 
  // Payload complete (or message has no payload). Check the CRC.
  if (crc_check_needed) {
    uint32_t crc = CalculateCRC(buffer_);
    auto* header = reinterpret_cast<MessageHeader*>(buffer_);
    if (crc == header->crc) {
      VLOG(1) << "CRC passed. Dispatching message. [message="
              << header->message_type << " (" << (unsigned)header->message_type
              << "), seq=" << header->sequence_number
              << ", size=" << current_message_size_
              << " B, crc=" << HexPrintableInteger(crc) << "]";
      if (callback_) {
        auto* payload = reinterpret_cast<uint8_t*>(header + 1);
        callback_(*header, payload);
      }
      state_ = State::SYNC0;
      return static_cast<int32_t>(current_message_size_);
    } else {
      if (quiet) {
        VLOG(2) << "CRC check failed. [message=" << header->message_type << " ("
                << (unsigned)header->message_type
                << "), seq=" << header->sequence_number
                << ", size=" << current_message_size_
                << " B, crc=" << HexPrintableInteger(crc)
                << ", expected_crc=" << HexPrintableInteger(header->crc) << "]";
      } else {
        LOG(WARNING) << "CRC check failed. [message=" << header->message_type
                     << " (" << (unsigned)header->message_type
                     << "), seq=" << header->sequence_number
                     << ", size=" << current_message_size_
                     << " B, crc=" << HexPrintableInteger(crc)
                     << ", expected_crc=" << HexPrintableInteger(header->crc)
                     << "]";
      }
      state_ = State::SYNC0;
      return -1;
    }
  }
 
  // No messages completed.
  return 0;
}
 
/******************************************************************************/
uint32_t FusionEngineFramer::Resync() {
  // If the message preamble shows up randomly somewhere in the data stream, we
  // may sync to it and try to parse a message starting at that arbitrary
  // location. We will eventually detect the bad sync either by CRC failure or
  // because the payload size we extract from the bogus message header is too
  // large.
  //
  // In either case, the bytes we collected - both the bogus header and the
  // payload bytes based on the size in the header - may contain the start of a
  // valid message, or even one or more complete messages. We need to resync and
  // try to find those messages. Any of the following scenarios is possible:
  //   ...validvalidval       <-- Contiguous valid messages
  //   ...valid...valid...val <-- Multiple valid messages, separated by invalid
  //   ...valid...valid...    <-- Similar, but ending with invalid
  //   ...val                 <-- Start of a valid message, no complete messages
  //   ...                    <-- No valid content
  //
  // Ideally, we would search for these messages in-place and avoid shifting the
  // data around in the buffer. However, since we need the messages to be
  // 4B-aligned and there's only a 1-in-4 chance of that happening naturally,
  // we'd have to shift the data 75% of the time regardless.
  //
  // Given that, we simply shift all data left in the buffer and process one
  // message at a time. This is not as efficient, but it's the simplest option.
  uint32_t available_bytes = next_byte_index_;
  VLOG(1) << "Attempting resynchronization. [" << available_bytes - 1
          << " candidate bytes]";
  uint32_t total_message_size = 0;
  state_ = State::SYNC0;
  next_byte_index_ = 0;
  for (uint32_t offset = 1; offset < available_bytes; ++offset) {
    uint8_t current_byte = buffer_[offset];
 
    // Skip forward until we see a SYNC0.
    if (state_ == State::SYNC0) {
      if (current_byte == MessageHeader::SYNC0) {
        VLOG(1) << "Candidate message start found @ offset " << offset << "/"
                << available_bytes << ".";
        // Shift all of the data left in the buffer.
        available_bytes -= offset;
        std::memmove(buffer_, buffer_ + offset, available_bytes);
        offset = 0;
      } else {
        VLOG(4) << "Skipping non-sync byte 0 @ offset " << offset << "/"
                << available_bytes
                << ". [byte=" << HexPrintableInteger(current_byte) << "]";
        continue;
      }
    }
 
    // Process this byte. If we end up back in the SYNC0 state, either A) the
    // SYNC0 we found was a valid message and got dispatched, or B) was not the
    // start of a valid message.
    //
    // In (A), message_size > 0 indicating there was a valid message, so we know
    // we can just keep going with the rest of the data in the buffer.
    //
    // In (B), message_size == 0. In that case we'll rewind back to the byte
    // just after we located the SYNC0 and see if there's another one.
    //
    // Note that next_byte_index_ always points to the next open slot, i.e.,
    // one byte _after_ the current byte.
    next_byte_index_ = offset + 1;
    int32_t message_size = OnByte(true);
 
    if (state_ == State::SYNC0) {
      // Note that offset will be incremented when we loop around, so we set it
      // to N-1, where N is wherever we want to start searching next.
      if (message_size > 0) {
        total_message_size += message_size;
        offset = message_size - 1;
        VLOG(1)
            << "Resync found a complete message. Continuing search @ offset "
            << offset + 1 << "/" << available_bytes
            << ". [message_size=" << message_size << ", "
            << (available_bytes - message_size - 1)
            << " candidate bytes remaining]";
      } else {
        size_t prev_offset = offset;
        offset = 0;
        VLOG(1) << "Candidate message rejected after " << prev_offset
                << " bytes. Restarting search @ offset " << offset + 1 << "/"
                << available_bytes << ". [" << available_bytes - 1
                << " candidate bytes remaining]";
      }
 
      next_byte_index_ = 0;
    }
  }
 
  VLOG(1) << "Resynchronization finished. " << next_byte_index_
          << " bytes remaining in buffer.";
 
  return total_message_size;
}
 
/******************************************************************************/
void FusionEngineFramer::ClearManagedBuffer() {
  if (is_buffer_managed_ && buffer_ != nullptr) {
    delete[] buffer_;
    is_buffer_managed_ = false;
    buffer_ = nullptr;
  }
}