From 128c0a2d876e206e5e127df97090a9f2e828af43 Mon Sep 17 00:00:00 2001 From: David Cermak Date: Mon, 6 Jun 2022 15:05:38 +0200 Subject: [PATCH] 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 | 16 ++++++- .../test/host_test/main/LoopbackTerm.cpp | 32 +++++++++++++- .../test/host_test/main/LoopbackTerm.h | 9 ++++ .../test/host_test/main/test_modem.cpp | 44 +++++++++++++++++++ 4 files changed, 97 insertions(+), 4 deletions(-) diff --git a/components/esp_modem/src/esp_modem_cmux.cpp b/components/esp_modem/src/esp_modem_cmux.cpp index f3e85f6ff..f37bd8512 100644 --- a/components/esp_modem/src/esp_modem_cmux.cpp +++ b/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; diff --git a/components/esp_modem/test/host_test/main/LoopbackTerm.cpp b/components/esp_modem/test/host_test/main/LoopbackTerm.cpp index 4f33d64c2..0377372df 100644 --- a/components/esp_modem/test/host_test/main/LoopbackTerm.cpp +++ b/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; diff --git a/components/esp_modem/test/host_test/main/LoopbackTerm.h b/components/esp_modem/test/host_test/main/LoopbackTerm.h index d7778dcfe..04b29cd31 100644 --- a/components/esp_modem/test/host_test/main/LoopbackTerm.h +++ b/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 loopback_data; size_t data_len; bool pin_ok; bool is_bg96; + size_t inject_by; }; diff --git a/components/esp_modem/test/host_test/main/test_modem.cpp b/components/esp_modem/test/host_test/main/test_modem.cpp index d8c4c9746..f8d8458a5 100644 --- a/components/esp_modem/test/host_test/main/test_modem.cpp +++ b/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(); + auto loopback = term.get(); + auto dte = std::make_shared(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); + } +}