fix(esp_modem): Support 2 byte size packets

Closes https://github.com/espressif/esp-protocols/issues/46

components/esp_modem/src/esp_modem_cmux.cpp

@@ -187,10 +187,22 @@ bool CMux::on_header(CMuxFrame &frame)
     }
     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;
+    if ((frame_header[3] & 1) == 0) {
+        if (frame_header_offset + frame.len <= 4) {
+            frame_header_offset += frame.len;
+            return false; // need read more
+        }
+        payload_offset = std::min(frame.len, 5 - frame_header_offset);
+        memcpy(frame_header + frame_header_offset, frame.ptr, payload_offset);
+        payload_len = frame_header[4] << 7;
+        frame_header_offset += payload_offset - 1; // rewind frame_header back to hold only 6 bytes size
+    } else {
+        payload_len = 0;
+        frame_header_offset += payload_offset;
+    }
     dlci = frame_header[1] >> 2;
     type = frame_header[2];
-    payload_len = (frame_header[3] >> 1);
+    payload_len += (frame_header[3] >> 1);
     frame.advance(payload_offset);
     state = cmux_state::PAYLOAD;
     return true;

components/esp_modem/test/host_test/main/LoopbackTerm.cpp

@@ -15,6 +15,10 @@ void LoopbackTerm::stop()
 
 int LoopbackTerm::write(uint8_t *data, size_t len)
 {
+    if (inject_by) {    // injection test: ignore what we write, but respond with injected data
+        auto ret = std::async(&LoopbackTerm::batch_read, this);
+        return len;
+    }
     if (len > 2 && (data[len - 1] == '\r' || data[len - 1] == '+') ) { // Simple AT responder
         std::string command((char *)data, len);
         std::string response;
@@ -67,6 +71,8 @@ int LoopbackTerm::write(uint8_t *data, size_t len)
 int LoopbackTerm::read(uint8_t *data, size_t len)
 {
     size_t read_len = std::min(data_len, len);
+    if (inject_by && read_len > inject_by)
+        read_len = inject_by;
     if (read_len) {
         if (loopback_data.capacity() < len) {
             loopback_data.reserve(len);
@@ -78,8 +84,30 @@ int LoopbackTerm::read(uint8_t *data, size_t len)
     return read_len;
 }
 
-LoopbackTerm::LoopbackTerm(bool is_bg96): loopback_data(), data_len(0), pin_ok(false), is_bg96(is_bg96) {}
+LoopbackTerm::LoopbackTerm(bool is_bg96): loopback_data(), data_len(0), pin_ok(false), is_bg96(is_bg96), inject_by(0) {}
 
-LoopbackTerm::LoopbackTerm(): loopback_data(), data_len(0), pin_ok(false), is_bg96(false) {}
+LoopbackTerm::LoopbackTerm(): loopback_data(), data_len(0), pin_ok(false), is_bg96(false), inject_by(0) {}
+
+int LoopbackTerm::inject(uint8_t *data, size_t len, size_t injected_by)
+{
+    if (data == nullptr) {
+        inject_by = 0;
+        return 0;
+    }
+
+    loopback_data.resize(len);
+    memcpy(&loopback_data[0], data, len);
+    data_len = len;
+    inject_by = injected_by;
+    return len;
+}
+
+void LoopbackTerm::batch_read()
+{
+    while (data_len > 0) {
+        on_read(nullptr, std::min(inject_by, data_len));
+        Task::Delay(1);
+    }
+}
 
 LoopbackTerm::~LoopbackTerm() = default;

components/esp_modem/test/host_test/main/LoopbackTerm.h

@@ -12,6 +12,13 @@ public:
 
     ~LoopbackTerm() override;
 
+    /**
+     * @brief Inject user data to the terminal, to respond.
+     * inject_by defines batch sizes: the read callback is called multiple times
+     * with partial data of `inject_by` size
+     */
+    int inject(uint8_t *data, size_t len, size_t inject_by);
+
     void start() override;
     void stop() override;
 
@@ -24,10 +31,12 @@ private:
         STARTED,
         STOPPED
     };
+    void batch_read();
     status_t status;
     SignalGroup signal;
     std::vector<uint8_t> loopback_data;
     size_t data_len;
     bool pin_ok;
     bool is_bg96;
+    size_t inject_by;
 };

components/esp_modem/test/host_test/main/test_modem.cpp

@@ -155,3 +155,47 @@ TEST_CASE("DCE CMUX test", "[esp_modem]")
     }, 1000);
     CHECK(ret == command_result::OK);
 }
+
+TEST_CASE("Test CMUX protocol by injecting payloads", "[esp_modem]")
+{
+    auto term = std::make_unique<LoopbackTerm>();
+    auto loopback = term.get();
+    auto dte = std::make_shared<DTE>(std::move(term));
+    CHECK(term == nullptr);
+
+    esp_modem_dce_config_t dce_config = ESP_MODEM_DCE_DEFAULT_CONFIG("APN");
+    esp_netif_t netif{};
+    auto dce = create_SIM7600_dce(&dce_config, dte, &netif);
+    CHECK(dce != nullptr);
+
+    CHECK(dce->set_mode(esp_modem::modem_mode::CMUX_MODE) == true);
+    const auto test_command = "Test\n";
+    // 1 byte payload size
+    uint8_t test_payload[] = {0xf9, 0x05, 0xff, 0x0b, 0x54, 0x65, 0x73, 0x74, 0x0a, 0xbb, 0xf9 };
+    loopback->inject(&test_payload[0], sizeof(test_payload), 1);
+    auto ret = dce->command(test_command, [&](uint8_t *data, size_t len) {
+        std::string response((char *) data, len);
+        CHECK(response == test_command);
+        return command_result::OK;
+    }, 1000);
+    CHECK(ret == command_result::OK);
+
+    // 2 byte payload size
+    uint8_t long_payload[453] = { 0xf9, 0x05, 0xef, 0x7c, 0x03, 0x7e }; // header
+    long_payload[5]   = 0x7e;   // payload to validate
+    long_payload[449] = 0x7e;
+    long_payload[450] = '\n';
+    long_payload[451] = 0x53;   // footer
+    long_payload[452] = 0xf9;
+    for (int i=0; i<5; ++i) {
+        // inject the whole payload (i=0) and then per 1,2,3,4 bytes (i)
+        loopback->inject(&long_payload[0], sizeof(long_payload), i==0?sizeof(long_payload):i);
+        auto ret = dce->command("ignore", [&](uint8_t *data, size_t len) {
+            CHECK(data[0]     == 0x7e);
+            CHECK(data[len-2] == 0x7e);
+            CHECK(data[len-1] == '\n');
+            return command_result::OK;
+        }, 1000);
+        CHECK(ret == command_result::OK);
+    }
+}