bno08x_shtp_uart.hpp

Go to the documentation of this file.

#pragma once
#include <cstddef>
#include <cstdint>
 
namespace bno08x::shtp_uart {
 
inline constexpr uint32_t BAUD_RATE = 3000000;
inline constexpr uint32_t HOST_BYTE_GAP_US = 100;
 
inline constexpr uint8_t FLAG = 0x7E;             
inline constexpr uint8_t ESCAPE = 0x7D;           
inline constexpr uint8_t ESCAPE_XOR = 0x20;       
inline constexpr uint8_t PROTOCOL_CONTROL = 0x00; 
inline constexpr uint8_t PROTOCOL_SHTP = 0x01;    
 
inline constexpr uint8_t BUFFER_STATUS_QUERY[3] = {FLAG, PROTOCOL_CONTROL, FLAG};
 
constexpr std::size_t MaxEncodedSize(std::size_t payloadLen) noexcept {
  return 2 * payloadLen + 3;
}
 
inline std::size_t EncodeFrame(const uint8_t* shtp, std::size_t len, uint8_t* out,
                               std::size_t cap) noexcept {
  std::size_t n = 0;
  auto put = [&](uint8_t b) {
    if (n < cap) {
      out[n] = b;
    }
    ++n;
  };
  put(FLAG);
  put(PROTOCOL_SHTP);
  for (std::size_t i = 0; i < len; ++i) {
    const uint8_t b = shtp[i];
    if (b == FLAG || b == ESCAPE) {
      put(ESCAPE);
      put(static_cast<uint8_t>(b ^ ESCAPE_XOR));
    } else {
      put(b);
    }
  }
  put(FLAG);
  return n <= cap ? n : 0;
}
 
template <std::size_t Capacity>
class FrameDecoder {
public:
  enum class Event : uint8_t {
    None,         
    ShtpFrame,    
    BufferStatus, 
    Dropped       
  };
 
  Event Push(uint8_t c) noexcept {
    switch (state_) {
    case State::Outside:
      if (c == FLAG) {
        startFrame();
      }
      return Event::None;
    case State::Escaped:
      store(static_cast<uint8_t>(c ^ ESCAPE_XOR));
      state_ = State::Inside;
      return Event::None;
    case State::Inside:
      break;
    }
    if (c == ESCAPE) {
      state_ = State::Escaped;
      return Event::None;
    }
    if (c != FLAG) {
      store(c);
      return Event::None;
    }
    // A flag closes the frame; it may also open the next one, so stay inside.
    const Event ev = finishFrame();
    startFrame();
    return ev;
  }
 
  const uint8_t* Data() const noexcept {
    return frame_ + 1;
  }
 
  std::size_t Size() const noexcept {
    return shtpLen_;
  }
 
  uint16_t BufferSpace() const noexcept {
    return bufferSpace_;
  }
 
  void ClearBufferSpace() noexcept {
    bufferSpace_ = 0;
  }
 
  void Reset() noexcept {
    state_ = State::Outside;
    len_ = 0;
    overflow_ = false;
    shtpLen_ = 0;
    bufferSpace_ = 0;
  }
 
private:
  enum class State : uint8_t { Outside, Inside, Escaped };
 
  void startFrame() noexcept {
    state_ = State::Inside;
    len_ = 0;
    overflow_ = false;
  }
 
  void store(uint8_t c) noexcept {
    if (len_ < Capacity) {
      frame_[len_++] = c;
    } else {
      overflow_ = true;
    }
  }
 
  Event finishFrame() noexcept {
    if (len_ == 0) {
      return Event::None; // two flags in a row: an empty frame, or a closing flag reused
    }
    if (overflow_) {
      return Event::Dropped;
    }
    if (frame_[0] == PROTOCOL_SHTP) {
      shtpLen_ = len_ - 1;
      return Event::ShtpFrame;
    }
    if (frame_[0] == PROTOCOL_CONTROL && len_ >= 3) {
      bufferSpace_ = static_cast<uint16_t>(frame_[1] | (frame_[2] << 8));
      return Event::BufferStatus;
    }
    return Event::Dropped;
  }
 
  static_assert(Capacity >= 3, "a frame needs room for the protocol ID and a buffer status");
 
  uint8_t frame_[Capacity]{};
  std::size_t len_{0};
  std::size_t shtpLen_{0};
  uint16_t bufferSpace_{0};
  bool overflow_{false};
  State state_{State::Outside};
};
 
} // namespace bno08x::shtp_uart