12#ifndef BNO08X_SHTP_HPP
13#define BNO08X_SHTP_HPP
26namespace bno08x::detail {
35 friend constexpr bool operator==(
const ShtpHeader& lhs,
const ShtpHeader& rhs)
noexcept =
default;
39inline constexpr uint32_t kLengthError = 0xFFFF;
42[[nodiscard]]
constexpr uint16_t Le16(std::span<const uint8_t> p, std::size_t off)
noexcept {
43 return static_cast<uint16_t
>(
static_cast<uint32_t
>(p[off]) |
44 (
static_cast<uint32_t
>(p[off + 1U]) << 8U));
48[[nodiscard]]
constexpr uint32_t Le32(std::span<const uint8_t> p, std::size_t off)
noexcept {
49 return static_cast<uint32_t
>(Le16(p, off)) | (
static_cast<uint32_t
>(Le16(p, off + 2U)) << 16U);
53[[nodiscard]]
constexpr int32_t S16(uint16_t raw)
noexcept {
54 return static_cast<int32_t
>((
static_cast<uint32_t
>(raw) ^ 0x8000U)) - 0x8000;
58[[nodiscard]]
constexpr int32_t S32(uint32_t raw)
noexcept {
59 return static_cast<int32_t
>(
static_cast<int64_t
>(raw ^ 0x8000'0000U) - 0x8000'0000LL);
63constexpr void PutLe16(std::span<uint8_t> p, std::size_t off, uint32_t v)
noexcept {
64 p[off] =
static_cast<uint8_t
>(v & 0xFFU);
65 p[off + 1U] =
static_cast<uint8_t
>((v >> 8U) & 0xFFU);
69constexpr void PutLe32(std::span<uint8_t> p, std::size_t off, uint32_t v)
noexcept {
70 PutLe16(p, off, v & 0xFFFFU);
71 PutLe16(p, off + 2U, (v >> 16U) & 0xFFFFU);
75[[nodiscard]]
constexpr ShtpHeader DecodeHeader(std::span<const uint8_t> bytes)
noexcept {
76 const uint32_t raw = Le16(bytes, 0U);
77 return ShtpHeader{.length =
static_cast<uint16_t
>(raw & 0x7FFFU),
78 .cont = (raw & 0x8000U) != 0U,
84constexpr void EncodeHeader(std::span<uint8_t> out, uint32_t total_len, uint8_t channel,
85 uint8_t seq)
noexcept {
86 PutLe16(out, 0U, total_len & 0x7FFFU);
92enum class RxResult : uint8_t {
111template <std::
size_t N>
115 RxEngine(Stats& stats,
const Config& cfg) noexcept : stats_(stats), cfg_(cfg) {}
118 void Reset() noexcept {
123 seq_valid_.fill(
false);
127 void Abort() noexcept {
135 [[nodiscard]]
bool Open() const noexcept {
141 [[nodiscard]] std::size_t NextReadLen(std::size_t max_transfer)
const noexcept {
146 n = cfg_.fixed_read_bytes;
150 return std::min(n, max_transfer);
154 [[nodiscard]] std::span<const uint8_t> Cargo() const noexcept {
155 return std::span<const uint8_t>{buf_}.first(done_len_);
159 [[nodiscard]] uint8_t Channel() const noexcept {
164 RxResult Accept(std::span<const uint8_t> transfer)
noexcept {
166 ++stats_.short_transfer;
168 return RxResult::Violation;
170 const uint32_t raw = Le16(transfer, 0U);
171 if (raw == kLengthError) {
172 ++stats_.length_ffff;
174 return RxResult::Violation;
176 const ShtpHeader h = DecodeHeader(transfer);
177 if (h.length == 0U) {
178 ++stats_.null_header;
179 return RxResult::Null;
182 ++stats_.length_short;
184 return RxResult::Violation;
187 return badChannel(h, transfer);
189 if (!checkSequence(h)) {
190 return RxResult::Dropped;
192 const RxResult head = h.cont ? continuation(h) : startCargo(h);
193 if (head != RxResult::Partial) {
202 RxResult badChannel(
const ShtpHeader& h, std::span<const uint8_t> transfer)
noexcept {
203 const bool again = h.cont && open_ && discard_ && (chan_ == h.channel);
205 ++stats_.bad_channel;
208 return RxResult::Dropped;
224 [[nodiscard]]
static bool restartSignal(
const ShtpHeader& h)
noexcept {
227 return !h.cont && (command || executable);
230 bool checkSequence(
const ShtpHeader& h)
noexcept {
231 const std::size_t c = h.channel;
235 const auto expected =
static_cast<uint8_t
>(
static_cast<uint32_t
>(last_seq_[c] + 1U) & 0xFFU);
236 if ((h.seq == last_seq_[c]) && !restartSignal(h)) {
239 }
else if (h.seq != expected) {
247 seq_valid_[c] =
true;
248 last_seq_[c] = h.seq;
254 RxResult continuation(
const ShtpHeader& h)
noexcept {
256 ++stats_.orphan_cont;
257 return RxResult::Dropped;
259 if (h.channel != chan_) {
262 return RxResult::Violation;
267 return RxResult::Dropped;
269 const std::size_t remaining = want_ - have_;
271 ++stats_.bad_cont_len;
273 return RxResult::Violation;
275 return RxResult::Partial;
279 RxResult startCargo(
const ShtpHeader& h)
noexcept {
286 ++stats_.null_header;
287 return RxResult::Null;
293 discard_ = cargo > N;
295 ++stats_.oversize_cargo;
297 return RxResult::Partial;
301 RxResult take(std::span<const uint8_t> data)
noexcept {
302 const std::size_t room = want_ - have_;
303 const std::size_t n = std::min(room, data.size());
305 static_cast<void>(std::copy(data.begin(), data.begin() +
static_cast<std::ptrdiff_t
>(n),
306 buf_.begin() +
static_cast<std::ptrdiff_t
>(have_)));
309 if (cfg_.strict_padding) {
310 for (std::size_t i = n; i < data.size(); ++i) {
312 ++stats_.nonzero_pad;
318 return RxResult::Partial;
323 return RxResult::Dropped;
326 return RxResult::CargoComplete;
331 std::array<uint8_t, N> buf_{};
332 std::size_t have_{0};
333 std::size_t want_{0};
334 std::size_t done_len_{0};
337 bool discard_{
false};
339 std::array<uint8_t, reg::kChannelCount> last_seq_{};
340 std::array<bool, reg::kChannelCount> seq_valid_{};
344inline constexpr uint8_t kUartFlag = 0x7E;
346inline constexpr uint8_t kUartEscape = 0x7D;
348inline constexpr uint8_t kUartProtoControl = 0;
350inline constexpr uint8_t kUartProtoShtp = 1;
353enum class FrameEvent : uint8_t {
359enum class UartRxMode : uint8_t { Idle, InFrame, Escape };
365template <std::
size_t N>
369 explicit UartFramer(Stats& stats) noexcept : stats_(stats) {}
372 void Reset() noexcept {
373 state_ = UartRxMode::Idle;
378 FrameEvent Feed(uint8_t b)
noexcept {
379 if (b == kUartFlag) {
382 if (state_ == UartRxMode::Idle) {
383 return FrameEvent::None;
385 if (state_ == UartRxMode::Escape) {
386 state_ = UartRxMode::InFrame;
387 store(
static_cast<uint8_t
>(
static_cast<uint32_t
>(b) ^ 0x20U));
388 return FrameEvent::None;
390 if (b == kUartEscape) {
391 state_ = UartRxMode::Escape;
392 return FrameEvent::None;
395 return FrameEvent::None;
399 [[nodiscard]] std::span<const uint8_t> Frame() const noexcept {
400 return std::span<const uint8_t>{buf_}.first(done_len_);
404 FrameEvent flag() noexcept {
405 if (state_ == UartRxMode::Escape) {
408 state_ = UartRxMode::InFrame;
410 return FrameEvent::None;
412 if ((state_ == UartRxMode::InFrame) && (len_ > 0U)) {
413 state_ = UartRxMode::Idle;
414 const std::size_t got = len_;
417 ++stats_.oversize_frame;
418 return FrameEvent::None;
421 if (buf_[0] > kUartProtoShtp) {
422 ++stats_.bad_protocol_id;
423 return FrameEvent::None;
425 return FrameEvent::Frame;
427 state_ = UartRxMode::InFrame;
429 return FrameEvent::None;
432 void store(uint8_t b)
noexcept {
442 std::array<uint8_t, N> buf_{};
444 std::size_t done_len_{0};
445 UartRxMode state_{UartRxMode::Idle};
449[[nodiscard]]
constexpr bool PutEscaped(std::span<uint8_t> out, std::size_t& pos,
450 uint8_t b)
noexcept {
451 const bool special = (b == kUartFlag) || (b == kUartEscape);
452 const std::size_t need = special ? 2U : 1U;
453 if ((pos + need) > out.size()) {
457 out[pos] = kUartEscape;
459 out[pos] =
static_cast<uint8_t
>(
static_cast<uint32_t
>(b) ^ 0x20U);
474[[nodiscard]]
constexpr std::size_t EncodeUartFrame(std::span<uint8_t> out, uint8_t proto,
475 std::span<const uint8_t> payload)
noexcept {
477 if (out.size() < 3U) {
480 out[pos] = kUartFlag;
482 bool ok = PutEscaped(out, pos, proto);
483 for (
const uint8_t b : payload) {
484 ok = ok && PutEscaped(out, pos, b);
486 if (!ok || (pos >= out.size())) {
489 out[pos] = kUartFlag;
497 uint16_t max_cargo_write{0};
498 uint16_t max_cargo_read{0};
499 uint16_t max_transfer_write{0};
500 uint16_t max_transfer_read{0};
501 uint32_t uart_timeout_ms{0};
502 std::array<uint8_t, reg::kChannelCount> channel{
505 std::array<uint8_t, kSensorSlots>
510inline constexpr uint8_t kTagGuid = 1;
511inline constexpr uint8_t kTagMaxCargoWrite = 2;
512inline constexpr uint8_t kTagMaxCargoRead = 3;
513inline constexpr uint8_t kTagMaxTransferWrite = 4;
514inline constexpr uint8_t kTagMaxTransferRead = 5;
515inline constexpr uint8_t kTagNormalChannel = 6;
516inline constexpr uint8_t kTagWakeChannel = 7;
517inline constexpr uint8_t kTagChannelName = 9;
518inline constexpr uint8_t kTagVersion = 0x80;
519inline constexpr uint8_t kTagExtra =
523[[nodiscard]]
constexpr Advert DefaultAdvert() noexcept {
525 for (std::size_t i = 0; i < reg::detail::kSensorTableSize; ++i) {
526 a.report_len[i] = reg::detail::kReportLengths[i];
532[[nodiscard]]
constexpr std::size_t ChannelRole(std::span<const uint8_t> name)
noexcept {
533 const auto is = [&name](
const char* s)
noexcept {
534 const auto at = [s](std::size_t k)
noexcept {
return std::bit_cast<uint8_t>(s[k]); };
536 while ((i < name.size()) && (at(i) != 0U) && (name[i] == at(i))) {
539 return (at(i) == 0U) && ((i == name.size()) || (name[i] == 0U));
542 if (is(
"device") || is(
"executable")) {
544 }
else if (is(
"control")) {
546 }
else if (is(
"inputNormal")) {
548 }
else if (is(
"inputWake")) {
550 }
else if (is(
"inputGyroRv")) {
561 uint8_t pending_chan{0};
562 bool bad_length{
false};
566inline void ApplyLimit(uint8_t tag, std::span<const uint8_t> val, Advert& adv)
noexcept {
567 const uint16_t v = Le16(val, 0U);
568 if (tag == kTagMaxCargoWrite) {
569 adv.max_cargo_write = v;
570 }
else if (tag == kTagMaxCargoRead) {
571 adv.max_cargo_read = v;
572 }
else if (tag == kTagMaxTransferWrite) {
573 adv.max_transfer_write = v;
575 adv.max_transfer_read = v;
581inline void ApplyReportLengths(std::span<const uint8_t> val, Advert& adv,
582 AdvertState& st)
noexcept {
583 for (std::size_t i = 0; (i + 1U) < val.size(); i += 2U) {
587 st.bad_length =
true;
589 adv.report_len[val[i]] = val[i + 1U];
596inline void ApplyChannelTlv(uint8_t tag, std::span<const uint8_t> val, Advert& adv,
597 AdvertState& st)
noexcept {
598 if (((tag == kTagNormalChannel) || (tag == kTagWakeChannel)) && (val.size() == 1U)) {
599 st.pending_chan = val[0];
600 }
else if (tag == kTagChannelName) {
601 const std::size_t role = ChannelRole(val);
603 adv.channel[role] = st.pending_chan;
611inline void ApplyExtraTlv(std::span<const uint8_t> val, Advert& adv, AdvertState& st)
noexcept {
612 if ((st.guid == 0U) && (val.size() == 4U)) {
613 adv.uart_timeout_ms = Le32(val, 0U);
614 }
else if ((st.guid != 0U) && ((val.size() % 2U) == 0U)) {
615 ApplyReportLengths(val, adv, st);
622inline void ApplyTlv(uint8_t tag, std::span<const uint8_t> val, Advert& adv,
623 AdvertState& st)
noexcept {
624 if (tag == kTagGuid) {
625 st.guid = (val.size() == 4U) ? Le32(val, 0U) : st.guid;
626 }
else if ((tag >= kTagMaxCargoWrite) && (tag <= kTagMaxTransferRead)) {
627 if ((val.size() == 2U) && (st.guid == 0U)) {
628 ApplyLimit(tag, val, adv);
630 }
else if (tag == kTagExtra) {
631 ApplyExtraTlv(val, adv, st);
633 ApplyChannelTlv(tag, val, adv, st);
643inline void ParseAdvertisement(std::span<const uint8_t> tlv, Advert& adv,
bool& bad)
noexcept {
647 while ((pos + 2U) <= tlv.size()) {
648 const std::size_t len = tlv[pos + 1U];
649 if ((pos + 2U + len) > tlv.size()) {
653 ApplyTlv(tlv[pos], tlv.subspan(pos + 2U, len), adv, st);
656 bad = bad || st.bad_length;
Compile-time helpers of the BNO08X driver: typed integer literals, exact hook matching,...
BNO08x protocol constants: SHTP channels, SH-2 report, command and record identifiers,...
Public types of the BNO08x driver: errors, Result<T>, configuration, events and the integer unit help...
constexpr std::size_t kShtpHeaderLen
SHTP header: length (2), channel, sequence.
Definition bno08x_registers.hpp:103
constexpr std::size_t kChannelCount
Number of channels the driver tracks.
Definition bno08x_registers.hpp:40
constexpr std::size_t kLenSensorHeader
Report ID, sequence, status, delay.
Definition bno08x_registers.hpp:102
constexpr uint8_t kChanGyroRv
Gyro-integrated rotation vector, no report ID.
Definition bno08x_registers.hpp:39
constexpr uint8_t kChanInputWake
Input reports of wake-up sensors.
Definition bno08x_registers.hpp:38
constexpr uint8_t kChanInputNormal
Input reports of sensors that are not wake-up.
Definition bno08x_registers.hpp:37
constexpr uint8_t kChanCommand
SHTP command channel (advertisement, error list).
Definition bno08x_registers.hpp:33
constexpr uint8_t kChanExecutable
Executable ("device") channel: reset, on, sleep.
Definition bno08x_registers.hpp:34
constexpr uint8_t kChanControl
Sensor hub control: features, product ID, FRS, commands.
Definition bno08x_registers.hpp:35
constexpr std::size_t kSensorSlots
Number of slots of the per-sensor tables: sensor IDs 0x01 to 0x2E.
Definition bno08x_types.hpp:250
@ Fixed
Read Config::fixed_read_bytes per transfer; the hub zero-pads.
@ Reset
The hub restarted; detail is the reset cause when known.