CMUX: Refactor the protocol decoder to multiple methods per cmux state

components/esp_modem/include/cxx_include/esp_modem_cmux.hpp

@@ -55,33 +55,74 @@ class CMuxInstance;
 class CMux {
 public:
     explicit CMux(std::unique_ptr<Terminal> t, std::unique_ptr<uint8_t[]> 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<bool(uint8_t *data, size_t len)> 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<bool(uint8_t *data, size_t len)> read_cb[max_terms];
+    static uint8_t fcs_crc(const uint8_t frame[6]);     /*!< Utility to calculate FCS CRC */
-    void data_available(uint8_t *data, size_t len);
+    void data_available(uint8_t *data, size_t len);     /*!< Called when valid data available */
-    void send_sabm(size_t i);
+    void send_sabm(size_t i);                           /*!< Sending initial SABM */
-    std::unique_ptr<Terminal> term;
+    bool on_cmux(uint8_t *data, size_t len);            /*!< Called from terminal layer when raw CMUX protocol data available */
-    cmux_state state;
+
+    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<bool(uint8_t *data, size_t len)> read_cb[MAX_TERMINALS_NUM];  /*!< Function pointers to read callbacks */
+    std::unique_ptr<Terminal> 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<uint8_t[]> 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<uint8_t[]> buffer;
+
+    Lock lock;
 };
 
 /**

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<uint8_t *>(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);
+    ESP_LOG_BUFFER_HEXDUMP("CMUX Received", data, actual_len, ESP_LOG_VERBOSE);
-    uint8_t *frame = data;
+    CMuxFrame frame = { .ptr = data, .len = actual_len };
-    uint8_t *recover_ptr;
+    while (frame.len > 0) {
-    auto available_len = actual_len;
-    size_t payload_offset = 0;
-    size_t footer_offset = 0;
-    while (available_len > 0) {
         switch (state) {
         case cmux_state::RECOVER:
-            if (frame[0] == SOF_MARKER) {
+            if (!on_recovery(frame)) {
-                // already init state
+                return false;
-                state = cmux_state::INIT;
-                break;
             }
-            recover_ptr = static_cast<uint8_t *>(memchr(frame, SOF_MARKER, available_len));
+            break;
-            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;
         case cmux_state::INIT:
-            if (frame[0] != SOF_MARKER) {
+            if (!on_init(frame)) {
-                ESP_LOGW("CMUX", "Protocol mismatch: Missed leading SOF, recovering...");
+                return false;
-                state = cmux_state::RECOVER;
-                break;
             }
-            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) {
+            if (!on_header(frame)) {
-                // Previously trailing SOF interpreted as heading SOF, remove it and restart HEADER
+                return false;
-                available_len--;
-                frame++;
-                break;
             }
-            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 (!on_payload(frame)) {
-            if (available_len < payload_len) { // payload
-                state = cmux_state::PAYLOAD;
-                data_available(frame, available_len); // partial read
-                payload_len -= available_len;
                 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) {
+            if (!on_footer(frame)) {
-                memcpy(frame_header + frame_header_offset, frame, available_len);
+                return false;
-                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;
             }
             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<bool(uint8_t *, size_t)> f)
 {
-    if (inst < max_terms) {
+    if (inst < MAX_TERMINALS_NUM) {
         read_cb[inst] = std::move(f);
     }
 }