From fb6029b66c53dab19d15b2f0c8672ca659b80468 Mon Sep 17 00:00:00 2001 From: David Cermak Date: Thu, 17 Jun 2021 16:22:31 +0200 Subject: [PATCH] CMUX: Refactor the protocol decoder to multiple methods per cmux state --- .../include/cxx_include/esp_modem_cmux.hpp | 67 ++++- components/esp_modem/src/esp_modem_cmux.cpp | 235 ++++++++++-------- 2 files changed, 192 insertions(+), 110 deletions(-) diff --git a/components/esp_modem/include/cxx_include/esp_modem_cmux.hpp b/components/esp_modem/include/cxx_include/esp_modem_cmux.hpp index 8be1b74cf..f88a85ac2 100644 --- a/components/esp_modem/include/cxx_include/esp_modem_cmux.hpp +++ b/components/esp_modem/include/cxx_include/esp_modem_cmux.hpp @@ -55,33 +55,74 @@ class CMuxInstance; class CMux { public: explicit CMux(std::unique_ptr t, std::unique_ptr b, size_t buff_size): - term(std::move(t)), buffer_size(buff_size), buffer(std::move(b)), payload_start(nullptr), total_payload_size(0) {} + term(std::move(t)), payload_start(nullptr), total_payload_size(0), buffer_size(buff_size), buffer(std::move(b)) {} ~CMux() = default; + + /** + * @brief Initializes CMux protocol + * @return true on success + */ [[nodiscard]] bool init(); + + /** + * @brief Sets read callback for the appropriate terminal + * @param inst Index of the terminal + * @param f function pointer + */ void set_read_cb(int inst, std::function f); + /** + * @brief Writes to the appropriate terminal + * @param i Index of the terminal + * @param data Data to write + * @param len Data length to write + * @return The actual written length + */ int write(int i, uint8_t *data, size_t len); + private: - std::function read_cb[max_terms]; - void data_available(uint8_t *data, size_t len); - void send_sabm(size_t i); - std::unique_ptr term; - cmux_state state; + static uint8_t fcs_crc(const uint8_t frame[6]); /*!< Utility to calculate FCS CRC */ + void data_available(uint8_t *data, size_t len); /*!< Called when valid data available */ + void send_sabm(size_t i); /*!< Sending initial SABM */ + bool on_cmux(uint8_t *data, size_t len); /*!< Called from terminal layer when raw CMUX protocol data available */ + + struct CMuxFrame; /*!< Forward declare the Frame struct, used in protocol decoders */ + /** + * These methods serve different states of the CMUX protocols + * @param frame Currently available cmux frame (basically data, size, methods) + * @return - true if the state processed successfully + * - false if more data needed to process the current state + */ + bool on_recovery(CMuxFrame &frame); + bool on_init(CMuxFrame &frame); + bool on_header(CMuxFrame &frame); + bool on_payload(CMuxFrame &frame); + bool on_footer(CMuxFrame &frame); + + std::function read_cb[MAX_TERMINALS_NUM]; /*!< Function pointers to read callbacks */ + std::unique_ptr term; /*!< The original terminal */ + cmux_state state; /*!< CMux protocol state */ + + /** + * CMux control fields and offsets + */ uint8_t dlci; uint8_t type; size_t payload_len; uint8_t frame_header[6]; size_t frame_header_offset; - size_t buffer_size; - std::unique_ptr buffer; - bool on_cmux(uint8_t *data, size_t len); - static uint8_t fcs_crc(const uint8_t frame[6]); - Lock lock; - int instance; - int sabm_ack; uint8_t *payload_start; size_t total_payload_size; + int instance; + int sabm_ack; + /** + * Processing buffer size and pointer + */ + size_t buffer_size; + std::unique_ptr buffer; + + Lock lock; }; /** diff --git a/components/esp_modem/src/esp_modem_cmux.cpp b/components/esp_modem/src/esp_modem_cmux.cpp index 332f646bd..7597a6d31 100644 --- a/components/esp_modem/src/esp_modem_cmux.cpp +++ b/components/esp_modem/src/esp_modem_cmux.cpp @@ -91,11 +91,21 @@ void CMux::send_sabm(size_t i) } +struct CMux::CMuxFrame { + uint8_t *ptr; /*!< pointer to the currently processing byte of the CMUX frame */ + size_t len; /*!< length of available data in the current CMUX frame */ + void advance(int size = 1) + { + ptr += size; + len -= size; + } +}; + void CMux::data_available(uint8_t *data, size_t len) { if (data && type == 0xFF && len > 0 && dlci > 0) { int virtual_term = dlci - 1; - if (virtual_term < max_terms && read_cb[virtual_term]) { + if (virtual_term < MAX_TERMINALS_NUM && read_cb[virtual_term]) { // Post partial data (or defragment to post on CMUX footer) #ifdef DEFRAGMENT_CMUX_PAYLOAD if (payload_start == nullptr) { @@ -112,15 +122,126 @@ void CMux::data_available(uint8_t *data, size_t len) sabm_ack = dlci; } else if (data == nullptr) { int virtual_term = dlci - 1; - if (virtual_term < max_terms && read_cb[virtual_term]) { + if (virtual_term < MAX_TERMINALS_NUM && read_cb[virtual_term]) { #ifdef DEFRAGMENT_CMUX_PAYLOAD read_cb[virtual_term](payload_start, total_payload_size); #endif } - } } +bool CMux::on_init(CMuxFrame &frame) +{ + if (frame.ptr[0] != SOF_MARKER) { + ESP_LOGW("CMUX", "Protocol mismatch: Missed leading SOF, recovering..."); + state = cmux_state::RECOVER; + return true; + } + if (frame.len > 1 && frame.ptr[1] == SOF_MARKER) { + // empty frame + frame.advance(); + return true; + } + state = cmux_state::HEADER; + frame.advance(); + frame_header_offset = 1; + return true; +} + +bool CMux::on_recovery(CMuxFrame &frame) +{ + uint8_t *recover_ptr; + if (frame.ptr[0] == SOF_MARKER) { + // already init state + state = cmux_state::INIT; + return true; + } + recover_ptr = static_cast(memchr(frame.ptr, SOF_MARKER, frame.len)); + if (recover_ptr && frame.len > recover_ptr - frame.ptr) { + frame.len -= (recover_ptr - frame.ptr); + frame.ptr = recover_ptr; + state = cmux_state::INIT; + ESP_LOGI("CMUX", "Protocol recovered"); + if (frame.len > 1 && frame.ptr[1] == SOF_MARKER) { + // empty frame + frame.advance(); + } + return true; + } + // marker not found, continue with recovery + return false; +} + + +bool CMux::on_header(CMuxFrame &frame) +{ + if (frame.len > 0 && frame_header_offset == 1 && frame.ptr[0] == SOF_MARKER) { + // Previously trailing SOF interpreted as heading SOF, remove it and restart HEADER + frame.advance(); + return true; + } + if (frame.len + frame_header_offset < 4) { + memcpy(frame_header + frame_header_offset, frame.ptr, frame.len); + frame_header_offset += frame.len; + return false; // need read more + } + size_t payload_offset = std::min(frame.len, 4 - frame_header_offset); + memcpy(frame_header + frame_header_offset, frame.ptr, payload_offset); + frame_header_offset += payload_offset; + dlci = frame_header[1] >> 2; + type = frame_header[2]; + payload_len = (frame_header[3] >> 1); + frame.advance(payload_offset); + state = cmux_state::PAYLOAD; + return true; +} + +bool CMux::on_payload(CMuxFrame &frame) +{ + ESP_LOGD("CMUX", "Payload frame: dlci:%02x type:%02x payload:%d available:%d", dlci, type, payload_len, frame.len); + if (frame.len < payload_len) { // payload + state = cmux_state::PAYLOAD; + data_available(frame.ptr, frame.len); // partial read + payload_len -= frame.len; + return false; + } else { // complete + if (payload_len > 0) { + data_available(&frame.ptr[0], payload_len); // rest read + } + frame.advance((payload_len)); + state = cmux_state::FOOTER; + payload_len = 0; + } + return true; +} + +bool CMux::on_footer(CMuxFrame &frame) +{ + size_t footer_offset = 0; + if (frame.len + frame_header_offset < 6) { + memcpy(frame_header + frame_header_offset, frame.ptr, frame.len); + frame_header_offset += frame.len; + return false; // need read more + } else { + footer_offset = std::min(frame.len, 6 - frame_header_offset); + memcpy(frame_header + frame_header_offset, frame.ptr, footer_offset); + if (frame_header[5] != SOF_MARKER) { + ESP_LOGW("CMUX", "Protocol mismatch: Missed trailing SOF, recovering..."); + payload_start = nullptr; + total_payload_size = 0; + state = cmux_state::RECOVER; + return true; + } + frame.advance(footer_offset); + state = cmux_state::INIT; + frame_header_offset = 0; + data_available(nullptr, 0); + payload_start = nullptr; + total_payload_size = 0; + } + return true; +} + bool CMux::on_cmux(uint8_t *data, size_t actual_len) { if (!data) { @@ -138,113 +259,33 @@ bool CMux::on_cmux(uint8_t *data, size_t actual_len) actual_len = term->read(data, buffer_size); #endif } - ESP_LOG_BUFFER_HEXDUMP("CMUX Received", data, actual_len, ESP_LOG_DEBUG); - uint8_t *frame = data; - uint8_t *recover_ptr; - auto available_len = actual_len; - size_t payload_offset = 0; - size_t footer_offset = 0; - while (available_len > 0) { + ESP_LOG_BUFFER_HEXDUMP("CMUX Received", data, actual_len, ESP_LOG_VERBOSE); + CMuxFrame frame = { .ptr = data, .len = actual_len }; + while (frame.len > 0) { switch (state) { case cmux_state::RECOVER: - if (frame[0] == SOF_MARKER) { - // already init state - state = cmux_state::INIT; - break; + if (!on_recovery(frame)) { + return false; } - recover_ptr = static_cast(memchr(frame, SOF_MARKER, available_len)); - if (recover_ptr && available_len > recover_ptr - frame) { - available_len -= (recover_ptr - frame); - frame = recover_ptr; - state = cmux_state::INIT; - ESP_LOGI("CMUX", "Protocol recovered"); - if (available_len > 1 && frame[1] == SOF_MARKER) { - // empty frame - available_len -= 1; - frame += 1; - } - break; - } - // marker not found, continue with recovery - return false; + break; case cmux_state::INIT: - if (frame[0] != SOF_MARKER) { - ESP_LOGW("CMUX", "Protocol mismatch: Missed leading SOF, recovering..."); - state = cmux_state::RECOVER; - break; + if (!on_init(frame)) { + return false; } - if (available_len > 1 && frame[1] == SOF_MARKER) { - // empty frame - available_len -= 1; - frame += 1; - break; - } - state = cmux_state::HEADER; - available_len--; - frame_header_offset = 1; - frame++; break; case cmux_state::HEADER: - if (available_len > 0 && frame_header_offset == 1 && frame[0] == SOF_MARKER) { - // Previously trailing SOF interpreted as heading SOF, remove it and restart HEADER - available_len--; - frame++; - break; + if (!on_header(frame)) { + return false; } - if (available_len + frame_header_offset < 4) { - memcpy(frame_header + frame_header_offset, frame, available_len); - frame_header_offset += available_len; - return false; // need read more - } - payload_offset = std::min(available_len, 4 - frame_header_offset); - memcpy(frame_header + frame_header_offset, frame, payload_offset); - frame_header_offset += payload_offset; - dlci = frame_header[1] >> 2; - type = frame_header[2]; - payload_len = (frame_header[3] >> 1); - frame += payload_offset; - available_len -= payload_offset; - state = cmux_state::PAYLOAD; break; case cmux_state::PAYLOAD: - ESP_LOGD("CMUX", "Payload frame: dlci:%02x type:%02x payload:%d available:%d", dlci, type, payload_len, available_len); - if (available_len < payload_len) { // payload - state = cmux_state::PAYLOAD; - data_available(frame, available_len); // partial read - payload_len -= available_len; + if (!on_payload(frame)) { return false; - } else { // complete - if (payload_len > 0) { - data_available(&frame[0], payload_len); // rest read - } - available_len -= payload_len; - frame += payload_len; - state = cmux_state::FOOTER; - payload_len = 0; } break; case cmux_state::FOOTER: - if (available_len + frame_header_offset < 6) { - memcpy(frame_header + frame_header_offset, frame, available_len); - frame_header_offset += available_len; - return false; // need read more - } else { - footer_offset = std::min(available_len, 6 - frame_header_offset); - memcpy(frame_header + frame_header_offset, frame, footer_offset); - if (frame_header[5] != SOF_MARKER) { - ESP_LOGW("CMUX", "Protocol mismatch: Missed trailing SOF, recovering..."); - payload_start = nullptr; - total_payload_size = 0; - state = cmux_state::RECOVER; - break; - } - frame += footer_offset; - available_len -= footer_offset; - state = cmux_state::INIT; - frame_header_offset = 0; - data_available(nullptr, 0); - payload_start = nullptr; - total_payload_size = 0; + if (!on_footer(frame)) { + return false; } break; } @@ -313,7 +354,7 @@ int CMux::write(int virtual_term, uint8_t *data, size_t len) void CMux::set_read_cb(int inst, std::function f) { - if (inst < max_terms) { + if (inst < MAX_TERMINALS_NUM) { read_cb[inst] = std::move(f); } }