BNO08X Driver 0.1.0
API reference
Loading...
Searching...
No Matches
bno08x_shtp.hpp
Go to the documentation of this file.
1
12#ifndef BNO08X_SHTP_HPP
13#define BNO08X_SHTP_HPP
14
15#include <algorithm>
16#include <array>
17#include <bit>
18#include <cstddef>
19#include <cstdint>
20#include <span>
21
22#include "bno08x_detail.hpp"
23#include "bno08x_registers.hpp"
24#include "bno08x_types.hpp"
25
26namespace bno08x::detail {
27
29struct ShtpHeader {
30 uint16_t length{0};
31 bool cont{false};
32 uint8_t channel{0};
33 uint8_t seq{0};
35 friend constexpr bool operator==(const ShtpHeader& lhs, const ShtpHeader& rhs) noexcept = default;
36};
37
39inline constexpr uint32_t kLengthError = 0xFFFF;
40
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));
45}
46
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);
50}
51
53[[nodiscard]] constexpr int32_t S16(uint16_t raw) noexcept {
54 return static_cast<int32_t>((static_cast<uint32_t>(raw) ^ 0x8000U)) - 0x8000;
55}
56
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);
60}
61
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);
66}
67
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);
72}
73
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,
79 .channel = bytes[2],
80 .seq = bytes[3]};
81}
82
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);
87 out[2] = channel;
88 out[3] = seq;
89}
90
92enum class RxResult : uint8_t {
93 CargoComplete,
94 Partial,
95 Null,
96 Dropped,
97 Violation,
98};
99
111template <std::size_t N>
112class RxEngine {
113public:
115 RxEngine(Stats& stats, const Config& cfg) noexcept : stats_(stats), cfg_(cfg) {}
116
118 void Reset() noexcept {
119 open_ = false;
120 discard_ = false;
121 have_ = 0;
122 want_ = 0;
123 seq_valid_.fill(false);
124 }
125
127 void Abort() noexcept {
128 open_ = false;
129 discard_ = false;
130 have_ = 0;
131 want_ = 0;
132 }
133
135 [[nodiscard]] bool Open() const noexcept {
136 return open_;
137 }
138
141 [[nodiscard]] std::size_t NextReadLen(std::size_t max_transfer) const noexcept {
142 std::size_t n = 0;
143 if (open_) {
144 n = (want_ - have_) + reg::kShtpHeaderLen;
145 } else if (cfg_.read_policy == ReadPolicy::Fixed) {
146 n = cfg_.fixed_read_bytes;
147 } else {
149 }
150 return std::min(n, max_transfer);
151 }
152
154 [[nodiscard]] std::span<const uint8_t> Cargo() const noexcept {
155 return std::span<const uint8_t>{buf_}.first(done_len_);
156 }
157
159 [[nodiscard]] uint8_t Channel() const noexcept {
160 return chan_;
161 }
162
164 RxResult Accept(std::span<const uint8_t> transfer) noexcept {
165 if (transfer.size() < reg::kShtpHeaderLen) {
166 ++stats_.short_transfer;
167 Abort();
168 return RxResult::Violation;
169 }
170 const uint32_t raw = Le16(transfer, 0U);
171 if (raw == kLengthError) {
172 ++stats_.length_ffff;
173 Abort();
174 return RxResult::Violation;
175 }
176 const ShtpHeader h = DecodeHeader(transfer);
177 if (h.length == 0U) {
178 ++stats_.null_header;
179 return RxResult::Null;
180 }
181 if (h.length < reg::kShtpHeaderLen) {
182 ++stats_.length_short;
183 Abort();
184 return RxResult::Violation;
185 }
186 if (h.channel >= reg::kChannelCount) {
187 return badChannel(h, transfer);
188 }
189 if (!checkSequence(h)) {
190 return RxResult::Dropped;
191 }
192 const RxResult head = h.cont ? continuation(h) : startCargo(h);
193 if (head != RxResult::Partial) {
194 return head;
195 }
196 return take(transfer.subspan(reg::kShtpHeaderLen));
197 }
198
199private:
202 RxResult badChannel(const ShtpHeader& h, std::span<const uint8_t> transfer) noexcept {
203 const bool again = h.cont && open_ && discard_ && (chan_ == h.channel);
204 if (!again) {
205 ++stats_.bad_channel;
206 Abort();
207 if (h.cont || (h.length <= reg::kShtpHeaderLen)) {
208 return RxResult::Dropped;
209 }
210 chan_ = h.channel;
211 want_ = static_cast<std::size_t>(h.length) - reg::kShtpHeaderLen;
212 have_ = 0;
213 open_ = true;
214 discard_ = true;
215 }
216 return take(transfer.subspan(reg::kShtpHeaderLen));
217 }
218
224 [[nodiscard]] static bool restartSignal(const ShtpHeader& h) noexcept {
225 const bool command = h.channel == reg::kChanCommand;
226 const bool executable = h.channel == reg::kChanExecutable;
227 return !h.cont && (command || executable);
228 }
229
230 bool checkSequence(const ShtpHeader& h) noexcept {
231 const std::size_t c = h.channel;
232 bool keep = true;
233 gap_ = false;
234 if (seq_valid_[c]) {
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)) {
237 ++stats_.dup_seq;
238 keep = false;
239 } else if (h.seq != expected) {
240 ++stats_.seq_gap;
241 gap_ = true;
242 } else {
243 // the expected number
244 }
245 }
246 if (keep) {
247 seq_valid_[c] = true;
248 last_seq_[c] = h.seq;
249 }
250 return keep;
251 }
252
254 RxResult continuation(const ShtpHeader& h) noexcept {
255 if (!open_) {
256 ++stats_.orphan_cont;
257 return RxResult::Dropped;
258 }
259 if (h.channel != chan_) {
260 ++stats_.lost_cargo;
261 Abort();
262 return RxResult::Violation;
263 }
264 if (gap_) { // a fragment between is missing: the cargo cannot be put together (CEVA rxAssemble)
265 ++stats_.lost_cargo;
266 Abort();
267 return RxResult::Dropped;
268 }
269 const std::size_t remaining = want_ - have_;
270 if (cfg_.strict_cont_len && (h.length != (remaining + reg::kShtpHeaderLen))) {
271 ++stats_.bad_cont_len;
272 Abort();
273 return RxResult::Violation;
274 }
275 return RxResult::Partial;
276 }
277
279 RxResult startCargo(const ShtpHeader& h) noexcept {
280 if (open_) {
281 ++stats_.lost_cargo; // V7: the old cargo is lost, this header starts a new one
282 Abort();
283 }
284 const std::size_t cargo = static_cast<std::size_t>(h.length) - reg::kShtpHeaderLen;
285 if (cargo == 0U) {
286 ++stats_.null_header;
287 return RxResult::Null;
288 }
289 chan_ = h.channel;
290 want_ = cargo;
291 have_ = 0;
292 open_ = true;
293 discard_ = cargo > N;
294 if (discard_) {
295 ++stats_.oversize_cargo;
296 }
297 return RxResult::Partial;
298 }
299
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());
304 if (!discard_) {
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_)));
307 }
308 have_ += n;
309 if (cfg_.strict_padding) {
310 for (std::size_t i = n; i < data.size(); ++i) {
311 if (data[i] != 0U) {
312 ++stats_.nonzero_pad;
313 break;
314 }
315 }
316 }
317 if (have_ < want_) {
318 return RxResult::Partial;
319 }
320 open_ = false;
321 if (discard_) {
322 discard_ = false;
323 return RxResult::Dropped;
324 }
325 done_len_ = want_;
326 return RxResult::CargoComplete;
327 }
328
329 Stats& stats_;
330 const Config& cfg_;
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};
335 uint8_t chan_{0};
336 bool open_{false};
337 bool discard_{false};
338 bool gap_{false};
339 std::array<uint8_t, reg::kChannelCount> last_seq_{};
340 std::array<bool, reg::kChannelCount> seq_valid_{};
341};
342
344inline constexpr uint8_t kUartFlag = 0x7E;
346inline constexpr uint8_t kUartEscape = 0x7D;
348inline constexpr uint8_t kUartProtoControl = 0;
350inline constexpr uint8_t kUartProtoShtp = 1;
351
353enum class FrameEvent : uint8_t {
354 None,
355 Frame,
356};
357
359enum class UartRxMode : uint8_t { Idle, InFrame, Escape };
360
365template <std::size_t N>
366class UartFramer {
367public:
369 explicit UartFramer(Stats& stats) noexcept : stats_(stats) {}
370
372 void Reset() noexcept {
373 state_ = UartRxMode::Idle;
374 len_ = 0;
375 }
376
378 FrameEvent Feed(uint8_t b) noexcept {
379 if (b == kUartFlag) {
380 return flag();
381 }
382 if (state_ == UartRxMode::Idle) {
383 return FrameEvent::None; // bytes outside a frame are ignored until a flag
384 }
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;
389 }
390 if (b == kUartEscape) {
391 state_ = UartRxMode::Escape;
392 return FrameEvent::None;
393 }
394 store(b);
395 return FrameEvent::None;
396 }
397
399 [[nodiscard]] std::span<const uint8_t> Frame() const noexcept {
400 return std::span<const uint8_t>{buf_}.first(done_len_);
401 }
402
403private:
404 FrameEvent flag() noexcept {
405 if (state_ == UartRxMode::Escape) {
406 ++stats_.bad_escape; // an escape followed by a flag: the frame is garbage, this flag starts a
407 // new one
408 state_ = UartRxMode::InFrame;
409 len_ = 0;
410 return FrameEvent::None;
411 }
412 if ((state_ == UartRxMode::InFrame) && (len_ > 0U)) {
413 state_ = UartRxMode::Idle;
414 const std::size_t got = len_;
415 len_ = 0;
416 if (got > N) {
417 ++stats_.oversize_frame; // longer than the buffer: dropped, never handed out
418 return FrameEvent::None;
419 }
420 done_len_ = got;
421 if (buf_[0] > kUartProtoShtp) {
422 ++stats_.bad_protocol_id;
423 return FrameEvent::None;
424 }
425 return FrameEvent::Frame;
426 }
427 state_ = UartRxMode::InFrame; // start flag, or two flags in a row (S4.2)
428 len_ = 0;
429 return FrameEvent::None;
430 }
431
432 void store(uint8_t b) noexcept {
433 if (len_ < N) {
434 buf_[len_] = b;
435 ++len_;
436 } else {
437 len_ = N + 1U; // overflow marker: the frame is dropped at its end flag
438 }
439 }
440
441 Stats& stats_;
442 std::array<uint8_t, N> buf_{};
443 std::size_t len_{0};
444 std::size_t done_len_{0};
445 UartRxMode state_{UartRxMode::Idle};
446};
447
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()) {
454 return false;
455 }
456 if (special) {
457 out[pos] = kUartEscape;
458 ++pos;
459 out[pos] = static_cast<uint8_t>(static_cast<uint32_t>(b) ^ 0x20U);
460 } else {
461 out[pos] = b;
462 }
463 ++pos;
464 return true;
465}
466
474[[nodiscard]] constexpr std::size_t EncodeUartFrame(std::span<uint8_t> out, uint8_t proto,
475 std::span<const uint8_t> payload) noexcept {
476 std::size_t pos = 0;
477 if (out.size() < 3U) {
478 return 0;
479 }
480 out[pos] = kUartFlag;
481 ++pos;
482 bool ok = PutEscaped(out, pos, proto);
483 for (const uint8_t b : payload) {
484 ok = ok && PutEscaped(out, pos, b);
485 }
486 if (!ok || (pos >= out.size())) {
487 return 0;
488 }
489 out[pos] = kUartFlag;
490 ++pos;
491 return pos;
492}
493
495struct Advert {
496 bool valid{false};
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{
503 {0, 1, 2, 3, 4, 5}};
505 std::array<uint8_t, kSensorSlots>
506 report_len{};
507};
508
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 =
520 0x81;
521
523[[nodiscard]] constexpr Advert DefaultAdvert() noexcept {
524 Advert a{};
525 for (std::size_t i = 0; i < reg::detail::kSensorTableSize; ++i) {
526 a.report_len[i] = reg::detail::kReportLengths[i];
527 }
528 return a;
529}
530
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]); };
535 std::size_t i = 0;
536 while ((i < name.size()) && (at(i) != 0U) && (name[i] == at(i))) {
537 ++i;
538 }
539 return (at(i) == 0U) && ((i == name.size()) || (name[i] == 0U));
540 };
541 std::size_t role = reg::kChannelCount;
542 if (is("device") || is("executable")) {
544 } else if (is("control")) {
545 role = reg::kChanControl;
546 } else if (is("inputNormal")) {
548 } else if (is("inputWake")) {
549 role = reg::kChanInputWake;
550 } else if (is("inputGyroRv")) {
551 role = reg::kChanGyroRv;
552 } else {
553 // a channel this driver does not use
554 }
555 return role;
556}
557
559struct AdvertState {
560 uint32_t guid{0};
561 uint8_t pending_chan{0};
562 bool bad_length{false};
563};
564
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;
574 } else {
575 adv.max_transfer_read = v;
576 }
577}
578
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) {
584 if (val[i] >= kSensorSlots) {
585 // not a sensor report: its length is fixed by the RM
586 } else if (val[i + 1U] < reg::kLenSensorHeader) {
587 st.bad_length = true;
588 } else {
589 adv.report_len[val[i]] = val[i + 1U];
590 }
591 }
592}
593
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);
602 if ((role < reg::kChannelCount) && (st.pending_chan < reg::kChannelCount)) {
603 adv.channel[role] = st.pending_chan;
604 }
605 } else {
606 // other tags are ignored (SHTP S5.3)
607 }
608}
609
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);
616 } else {
617 // a form this driver does not know
618 }
619}
620
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);
629 }
630 } else if (tag == kTagExtra) {
631 ApplyExtraTlv(val, adv, st);
632 } else {
633 ApplyChannelTlv(tag, val, adv, st);
634 }
635}
636
643inline void ParseAdvertisement(std::span<const uint8_t> tlv, Advert& adv, bool& bad) noexcept {
644 std::size_t pos = 0;
645 AdvertState st{};
646 bad = false;
647 while ((pos + 2U) <= tlv.size()) {
648 const std::size_t len = tlv[pos + 1U];
649 if ((pos + 2U + len) > tlv.size()) {
650 bad = true;
651 break;
652 }
653 ApplyTlv(tlv[pos], tlv.subspan(pos + 2U, len), adv, st);
654 pos += 2U + len;
655 }
656 bad = bad || st.bad_length;
657 adv.valid = true;
658}
659
660} // namespace bno08x::detail
661
662#endif // BNO08X_SHTP_HPP
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.