From fbb4791af105ca52df5d84cc8911932516406d54 Mon Sep 17 00:00:00 2001 From: David Cermak Date: Thu, 4 Mar 2021 20:19:18 +0100 Subject: [PATCH] CMUX: Experimental implementation --- esp_modem/CMakeLists.txt | 2 +- .../simple_cxx_client/main/simple_client.cpp | 43 ++- .../cxx_include/esp_modem_commands.hpp | 109 ++++++- .../include/cxx_include/esp_modem_dce.hpp | 10 +- .../cxx_include/esp_modem_dce_commands.hpp | 24 +- .../cxx_include/esp_modem_device_factory.hpp | 4 +- .../include/cxx_include/esp_modem_dte.hpp | 84 ++++-- esp_modem/include/cxx_include/ppp_netif.hpp | 20 +- .../include/cxx_include/terminal_objects.hpp | 5 + .../include/cxx_include/uart_terminal.hpp | 2 +- esp_modem/include/esp_modem.h | 2 +- esp_modem/src/esp_modem_cmux.cpp | 271 ++++++++++++++++++ esp_modem/src/esp_modem_commands.cpp | 79 ----- esp_modem/src/esp_modem_dce.cpp | 24 +- esp_modem/src/esp_modem_device.cpp | 16 ++ esp_modem/src/esp_modem_device_factory.cpp | 3 +- esp_modem/src/esp_modem_dte.cpp | 14 +- esp_modem/src/esp_modem_uart.cpp | 30 +- esp_modem/src/ppp_netif.cpp | 59 ++-- 19 files changed, 631 insertions(+), 170 deletions(-) create mode 100644 esp_modem/src/esp_modem_cmux.cpp delete mode 100644 esp_modem/src/esp_modem_commands.cpp diff --git a/esp_modem/CMakeLists.txt b/esp_modem/CMakeLists.txt index 58e4c7908..1724bf949 100644 --- a/esp_modem/CMakeLists.txt +++ b/esp_modem/CMakeLists.txt @@ -10,11 +10,11 @@ set(srcs "src/esp_modem.c" "src/esp_bg96.c" "src/esp_modem_dte.cpp" "src/ppp_netif.cpp" - "src/esp_modem_commands.cpp" "src/esp_modem_api.cpp" "src/esp_modem_dce.cpp" "src/esp_modem_device.cpp" "src/esp_modem_device_factory.cpp" + "src/esp_modem_cmux.cpp" "src/esp_modem_uart.cpp") set(include_dirs "include") diff --git a/esp_modem/examples/simple_cxx_client/main/simple_client.cpp b/esp_modem/examples/simple_cxx_client/main/simple_client.cpp index fa198375b..8c13b0963 100644 --- a/esp_modem/examples/simple_cxx_client/main/simple_client.cpp +++ b/esp_modem/examples/simple_cxx_client/main/simple_client.cpp @@ -272,7 +272,7 @@ static void modem_test_app(esp_modem_dte_config_t *dte_config, esp_modem_dce_con ESP_LOGI("in the lambda", "len=%d data %s", len, (char*)data); std::cout << response << std::endl; return command_result::OK; - }, 1000); + }, 500); // uart_dte->command("AT+CPIN?\r", [&](uint8_t *data, size_t len) { // std::string response((char*)data, len); @@ -280,7 +280,7 @@ static void modem_test_app(esp_modem_dte_config_t *dte_config, esp_modem_dce_con // std::cout << response << std::endl; // return command_result::OK; // }, 1000); - +// // uart_dte->command("AT+CPIN=1234\r", [&](uint8_t *data, size_t len) { // std::string response((char*)data, len); // ESP_LOGI("in the lambda", "len=%d data %s", len, (char*)data); @@ -294,8 +294,38 @@ static void modem_test_app(esp_modem_dte_config_t *dte_config, esp_modem_dce_con std::string apn = "internet"; auto device = create_device(uart_dte, apn); +// bool pin_ok = true; +// if (device->read_pin(pin_ok) == command_result::OK && !pin_ok) { +// throw_if_false(device->set_pin("1234") == command_result::OK, "Cannot set PIN!"); +// } + +// +// std::string number; +// std::cout << "----" << std::endl; +// device->get_imsi(number); +// std::cout << "----" << std::endl; +// std::cout << "|" << number << "|" << std::endl; +// ESP_LOG_BUFFER_HEXDUMP("TEST", number.c_str(), number.length(), ESP_LOG_INFO); +// std::cout << "----" << std::endl; +// device->get_imei(number); +// std::cout << "|" << number << "|" << std::endl; +// device->get_module_name(number); +// std::cout << "|" << number << "|" << std::endl; +// std::cout << "----" << std::endl; auto my_dce = create_dce(uart_dte, device, esp_netif); +// return; + my_dce->set_cmux(); +// my_dce->set_cmux(); + + while (1) { + vTaskDelay(pdMS_TO_TICKS(1000)); + uart_dte->send_cmux_command(1, "AT+CPIN?\r"); + + } +// uart_dte->send_cmux_command(2, "AT+CPIN?"); + return; + my_dce->command("AT+CPIN?\r", [&](uint8_t *data, size_t len) { std::string response((char*)data, len); ESP_LOGI("in the lambda", "len=%d data %s", len, (char*)data); @@ -303,6 +333,8 @@ static void modem_test_app(esp_modem_dte_config_t *dte_config, esp_modem_dce_con return command_result::OK; }, 1000); + while (1) { + my_dce->set_data(); /* Config MQTT */ xEventGroupWaitBits(event_group, CONNECT_BIT, pdTRUE, pdTRUE, portMAX_DELAY); @@ -316,6 +348,13 @@ static void modem_test_app(esp_modem_dte_config_t *dte_config, esp_modem_dce_con // vTaskDelay(pdMS_TO_TICKS(20000)); my_dce->exit_data(); + uart_dte->command("AT+CPIN?\r", [&](uint8_t *data, size_t len) { + std::string response((char*)data, len); +// ESP_LOGI("in the lambda", "len=%d data %s", len, (char*)data); + std::cout << response << std::endl; + return command_result::OK; + }, 1000); + } // ddd->send_command("AT+COPS=?\r", [&](uint8_t *data, size_t len) { // std::string response((char*)data, len); diff --git a/esp_modem/include/cxx_include/esp_modem_commands.hpp b/esp_modem/include/cxx_include/esp_modem_commands.hpp index 9bef58c3a..5c928f0a8 100644 --- a/esp_modem/include/cxx_include/esp_modem_commands.hpp +++ b/esp_modem/include/cxx_include/esp_modem_commands.hpp @@ -6,14 +6,11 @@ #define SIMPLE_CXX_CLIENT_ESP_MODEM_COMMANDS_HPP #include "esp_modem_dte.hpp" -//#include "esp_modem_dce_commands.hpp" +#include "esp_modem_dce_commands.hpp" +#include + enum class command_result; -struct PdpContext { - PdpContext(std::string& apn): context_id(1), protocol_type("IP"), apn(apn) {} - size_t context_id; - std::string protocol_type; - std::string apn; -}; + #include @@ -38,6 +35,46 @@ template static inline command_result generic_command(T t, const st return command_result::TIMEOUT; }, timeout_ms); } +static inline void strip_cr_lf_tail(char *str, uint32_t len) +{ + if (str[len - 2] == '\r') { + str[len - 2] = '\0'; + } else if (str[len - 1] == '\r') { + str[len - 1] = '\0'; + } +} + +template static inline command_result generic_get_string(T t, const std::string& command, std::string& output, uint32_t timeout_ms) +{ + std::cout << command << std::endl; + return t->command(command.c_str(), [&](uint8_t *data, size_t len) { + size_t pos = 0; + std::string response((char*)data, len); + while ((pos = response.find('\n')) != std::string::npos) { + std::string token = response.substr(0, pos); + for (auto i = 0; i<2; ++i) // trip trailing \n\r of last two chars + if (pos >= 1 && (token[pos-1] == '\r' || token[pos-1] == '\n')) + token.pop_back(); + std::cout << "###" << token << "#### " << std::endl; + + if (token.find("OK") != std::string::npos) { + return command_result::OK; + } else if (token.find("ERROR") != std::string::npos) { + return command_result::FAIL; + } else if (token.length() > 2) { +// response.resize(response.find('\r')); +// response.erase(std::find(response.begin(), response.end(), '\r'), response.end()); +// std::cout << "|" << output << "|" << std::endl; +// const std::string& out(response); + output = token; //.substr(0,-1); + std::cout << "|" << token << "|" << std::endl; +// std::cout << output << std::endl; + } + response = response.substr(pos+1); + } + return command_result::TIMEOUT; + }, timeout_ms); +} template static inline command_result generic_command_common(T t, std::string command) { @@ -76,10 +113,66 @@ template command_result resume_data_mode(T t) template command_result set_command_mode(T t) { - return generic_command(t, "+++", "OK", "ERROR", 50000); + std::cout << "Sending +++" << std::endl; + return t->command("+++", [&](uint8_t *data, size_t len) { + std::string response((char*)data, len); + std::cout << response << std::endl; + if (response.find("OK") != std::string::npos) { + return command_result::OK; + } else if (response.find("NO CARRIER") != std::string::npos) { + return command_result::OK; + } else if (response.find("ERROR") != std::string::npos) { + return command_result::FAIL; + } + return command_result::TIMEOUT; + }, 5000); } +template command_result get_imsi(T t, std::string& imsi_number) +{ + return generic_get_string(t, "AT+CIMI\r", imsi_number, 5000); +} +template command_result get_imei(T t, std::string& out) +{ + return generic_get_string(t, "AT+CGSN\r", out, 5000); +} + +template command_result get_module_name(T t, std::string& out) +{ + return generic_get_string(t, "AT+CGMM\r", out, 5000); +} + +template command_result set_cmux(T t) +{ + return generic_command_common(t, "AT+CMUX=0\r"); +} + +template command_result read_pin(T t, bool& pin_ok) +{ + std::cout << "Sending read_pin" << std::endl; + return t->command("AT+CPIN?\r", [&](uint8_t *data, size_t len) { + std::string response((char*)data, len); + std::cout << response << std::endl; + if (response.find("READY") != std::string::npos) { + pin_ok = true; + return command_result::OK; + } else if (response.find("PIN") != std::string::npos || response.find("PUK") != std::string::npos ) { + pin_ok = false; + return command_result::OK; + } else if (response.find("ERROR") != std::string::npos) { + return command_result::FAIL; + } + return command_result::TIMEOUT; + }, 5000); +} + +template command_result set_pin(T t, const std::string& pin) +{ + std::cout << "Sending set_pin" << std::endl; + std::string set_pin_command = "AT+CPIN=" + pin + "\r"; + return generic_command_common(t, set_pin_command); +} } // dce_commands } // esp_modem diff --git a/esp_modem/include/cxx_include/esp_modem_dce.hpp b/esp_modem/include/cxx_include/esp_modem_dce.hpp index e2fb16109..9102e32db 100644 --- a/esp_modem/include/cxx_include/esp_modem_dce.hpp +++ b/esp_modem/include/cxx_include/esp_modem_dce.hpp @@ -16,17 +16,13 @@ public: dce_dte->set_mode(dte_mode::DATA_MODE); ppp_netif.start(); } - void exit_data() { - ppp_netif.stop(); - device->set_mode(dte_mode::COMMAND_MODE); - ppp_netif.wait_until_ppp_exits(); - dce_dte->set_mode(dte_mode::COMMAND_MODE); - } + void exit_data(); command_result command(const std::string& command, got_line_cb got_line, uint32_t time_ms) { return dce_dte->command(command, got_line, time_ms); } + void set_cmux(); private: std::shared_ptr dce_dte; std::shared_ptr device; - ppp ppp_netif; + PPP ppp_netif; }; \ No newline at end of file diff --git a/esp_modem/include/cxx_include/esp_modem_dce_commands.hpp b/esp_modem/include/cxx_include/esp_modem_dce_commands.hpp index 4fdc59b23..74e6564f5 100644 --- a/esp_modem/include/cxx_include/esp_modem_dce_commands.hpp +++ b/esp_modem/include/cxx_include/esp_modem_dce_commands.hpp @@ -1,9 +1,15 @@ #pragma once #include "cxx_include/esp_modem_dce_commands_if.hpp" -#include "cxx_include/esp_modem_commands.hpp" #include #include +struct PdpContext { + PdpContext(std::string& apn): context_id(1), protocol_type("IP"), apn(apn) {} + size_t context_id; + std::string protocol_type; + std::string apn; +}; + enum class command_result; class DTE; @@ -14,11 +20,17 @@ public: bool setup_data_mode() override; bool set_mode(dte_mode mode) override; - command_result set_echo(bool on) { return esp_modem::dce_commands::set_echo(dte, on); } - command_result set_data_mode() { return esp_modem::dce_commands::set_data_mode(dte); } - command_result resume_data_mode() { return esp_modem::dce_commands::resume_data_mode(dte); } - command_result set_pdp_context(PdpContext& pdp_context) { return esp_modem::dce_commands::set_pdp_context(dte.get(), pdp_context); } - command_result set_command_mode() { return esp_modem::dce_commands::set_command_mode(dte); } + command_result set_echo(bool on); + command_result set_data_mode(); + command_result resume_data_mode(); + command_result set_pdp_context(PdpContext& pdp_context); + command_result set_command_mode(); + command_result set_cmux(); + command_result get_imsi(std::string& imsi_number); + command_result set_pin(const std::string& pin); + command_result read_pin(bool& pin_ok); + command_result get_imei(std::string& imei); + command_result get_module_name(std::string& imei); private: std::shared_ptr dte; diff --git a/esp_modem/include/cxx_include/esp_modem_device_factory.hpp b/esp_modem/include/cxx_include/esp_modem_device_factory.hpp index 240565e6c..093dc3deb 100644 --- a/esp_modem/include/cxx_include/esp_modem_device_factory.hpp +++ b/esp_modem/include/cxx_include/esp_modem_device_factory.hpp @@ -1,3 +1,5 @@ #pragma once -std::shared_ptr create_device(const std::shared_ptr& dte, std::string &apn); \ No newline at end of file +#include "cxx_include/esp_modem_dce_commands.hpp" +//std::shared_ptr create_device(const std::shared_ptr& dte, std::string &apn); +std::shared_ptr create_device(const std::shared_ptr& dte, std::string &apn); \ No newline at end of file diff --git a/esp_modem/include/cxx_include/esp_modem_dte.hpp b/esp_modem/include/cxx_include/esp_modem_dte.hpp index 6c20b1aeb..70d9658e6 100644 --- a/esp_modem/include/cxx_include/esp_modem_dte.hpp +++ b/esp_modem/include/cxx_include/esp_modem_dte.hpp @@ -10,7 +10,7 @@ #include "esp_err.h" #include "terminal_objects.hpp" #include "ppp_netif.hpp" - +#include enum class terminal_error { BUFFER_OVERFLOW, @@ -18,10 +18,10 @@ enum class terminal_error { UNEXPECTED_CONTROL_FLOW, }; -class terminal { +class Terminal { public: - virtual ~terminal() = default; - void set_data_cb(std::function f) { on_data = std::move(f); } + virtual ~Terminal() = default; + virtual void set_data_cb(std::function f) { on_data = std::move(f); } void set_error_cb(std::function f) { on_error = std::move(f); } virtual int write(uint8_t *data, size_t len) = 0; virtual int read(uint8_t *data, size_t len) = 0; @@ -29,26 +29,36 @@ public: virtual void stop() = 0; protected: - std::function on_data; + std::function on_data; std::function on_error; }; -class dte_adapter: public terminal { +class CMUXedTerminal: public Terminal { public: - dte_adapter(std::unique_ptr terminal): - original_dte(std::move(terminal)) {} - ~dte_adapter() override = default; - int write(uint8_t *data, size_t len) override { return original_dte->write(data, len); } - int read(uint8_t *data, size_t len) override { return original_dte->read(data, len); } + explicit CMUXedTerminal(std::unique_ptr t, std::shared_ptr b): + term(std::move(t)), buffer(std::move(b)) {} + ~CMUXedTerminal() override = default; + void setup_cmux() { + + } + void set_data_cb(std::function f) override {} + int write(uint8_t *data, size_t len) override { return term->write(data, len); } + int read(uint8_t *data, size_t len) override { return term->read(data, len); } + void start() override; + void stop() override {} private: - std::unique_ptr original_dte; + static bool process_cmux_recv(size_t len); + void send_sabm(size_t i); + std::unique_ptr term; + std::shared_ptr buffer; }; enum class dte_mode { UNDEF, COMMAND_MODE, - DATA_MODE + DATA_MODE, + CMUX_MODE }; enum class command_result { @@ -60,12 +70,27 @@ enum class command_result { const int DTE_BUFFER_SIZE = 1024; +struct CMUXHelper { + void send_sabm(size_t dlci); +}; + +enum class cmux_state { + INIT, + HEADER, + PAYLOAD, + FOOTER, + +}; + typedef std::function got_line_cb; class DTE { public: - explicit DTE(std::unique_ptr t); + explicit DTE(std::unique_ptr t); ~DTE() = default; + +// std::unique_ptr detach() { return std::move(term); } +// void attach(std::unique_ptr t) { term = std::move(t); } // void set_line_cb(got_line f) { on_line_cb = std::move(f); } int write(uint8_t *data, size_t len) { return term->write(data, len); } int read(uint8_t **d, size_t len) { @@ -75,28 +100,51 @@ public: *d = data; return actual_len; } - void set_data_cb(std::function f) { on_data = std::move(f); } + void set_data_cb(std::function f) + { +// on_data = std::move(f); + term->set_data_cb(std::move(f)); + } +// std::shared_ptr get_buffer() { return buffer;} void start() { term->start(); } void data_mode_closed() { term->stop(); } void set_mode(dte_mode m) { term->start(); mode = m; if (m == dte_mode::DATA_MODE) { term->set_data_cb(on_data); + } else if (m == dte_mode::CMUX_MODE) { + setup_cmux(); } } command_result command(const std::string& command, got_line_cb got_line, uint32_t time_ms); +// std::shared_ptr buffer; + void send_cmux_command(uint8_t i, const std::string& command); private: - const size_t GOT_LINE = BIT0; + + void setup_cmux(); + void send_sabm(size_t dlci); +// CMUXHelper cmux; + static const size_t GOT_LINE = BIT0; size_t buffer_size; size_t consumed; +// std::shared_ptr> buffer; std::unique_ptr buffer; - std::unique_ptr term; + std::unique_ptr term; got_line_cb on_line; dte_mode mode; signal_group signal; - std::function on_data; + std::function on_data; + + bool on_cmux(size_t len); + static bool s_on_cmux(size_t len); + cmux_state state; + uint8_t dlci; + uint8_t type; + size_t payload_len; + uint8_t frame_header[6]; + size_t frame_header_offset; }; diff --git a/esp_modem/include/cxx_include/ppp_netif.hpp b/esp_modem/include/cxx_include/ppp_netif.hpp index 33ec34e70..956162400 100644 --- a/esp_modem/include/cxx_include/ppp_netif.hpp +++ b/esp_modem/include/cxx_include/ppp_netif.hpp @@ -9,29 +9,35 @@ #include "cxx_include/terminal_objects.hpp" class DTE; +class PPP; //struct ppp_netif_driver; struct ppp_netif_driver { esp_netif_driver_base_t base; - DTE *e; + PPP *ppp; }; -class ppp { +class PPP { public: - explicit ppp(std::shared_ptr e, esp_netif_t *netif); + explicit PPP(std::shared_ptr e, esp_netif_t *netif); void start(); - void notify_ppp_exit() { signal.set(PPP_EXIT); } +// void notify_ppp_exit() { signal.set(PPP_EXIT); } void wait_until_ppp_exits() { signal.wait(PPP_EXIT, 50000); } void stop(); private: - void receive(uint8_t *data, size_t len) const; + void receive(uint8_t *data, size_t len); + static esp_err_t esp_modem_dte_transmit(void *h, void *buffer, size_t len); + static esp_err_t esp_modem_post_attach(esp_netif_t * esp_netif, void * args); + static void on_ppp_changed(void *arg, esp_event_base_t event_base, int32_t event_id, void *event_data); + std::shared_ptr ppp_dte; esp_netif_t *netif; - struct ppp_netif_driver driver; + struct ppp_netif_driver driver{}; signal_group signal; - const size_t PPP_EXIT = BIT0; + static const size_t PPP_STARTED = BIT0; + static const size_t PPP_EXIT = BIT1; }; diff --git a/esp_modem/include/cxx_include/terminal_objects.hpp b/esp_modem/include/cxx_include/terminal_objects.hpp index a218c7ece..3f18b8966 100644 --- a/esp_modem/include/cxx_include/terminal_objects.hpp +++ b/esp_modem/include/cxx_include/terminal_objects.hpp @@ -56,6 +56,11 @@ struct signal_group { xEventGroupSetBits(event_group, bits); } + void clear(uint32_t bits) + { + xEventGroupClearBits(event_group, bits); + } + bool wait(uint32_t flags, uint32_t time_ms) // waiting for all and clearing if set { EventBits_t bits = xEventGroupWaitBits(event_group, flags, pdTRUE, pdTRUE, pdMS_TO_TICKS(time_ms)); diff --git a/esp_modem/include/cxx_include/uart_terminal.hpp b/esp_modem/include/cxx_include/uart_terminal.hpp index 67749cec7..d960101ce 100644 --- a/esp_modem/include/cxx_include/uart_terminal.hpp +++ b/esp_modem/include/cxx_include/uart_terminal.hpp @@ -10,7 +10,7 @@ struct dte_config; -std::unique_ptr create_uart_terminal(const dte_config *config); +std::unique_ptr create_uart_terminal(const dte_config *config); diff --git a/esp_modem/include/esp_modem.h b/esp_modem/include/esp_modem.h index c1b4725ec..7ebd431fa 100644 --- a/esp_modem/include/esp_modem.h +++ b/esp_modem/include/esp_modem.h @@ -103,7 +103,7 @@ esp_modem_dte_config_t; .tx_buffer_size = 512, \ .pattern_queue_size = 20, \ .event_queue_size = 30, \ - .event_task_stack_size = 2048, \ + .event_task_stack_size = 4096, \ .event_task_priority = 5, \ .line_buffer_size = 512 \ } diff --git a/esp_modem/src/esp_modem_cmux.cpp b/esp_modem/src/esp_modem_cmux.cpp new file mode 100644 index 000000000..fa410cfc8 --- /dev/null +++ b/esp_modem/src/esp_modem_cmux.cpp @@ -0,0 +1,271 @@ +// +// Created by david on 3/4/21. +// +#include +#include "cxx_include/esp_modem_dte.hpp" +#include "esp_log.h" +/* CRC8 is the reflected CRC8/ROHC algorithm */ +#define FCS_POLYNOMIAL 0xe0 /* reversed crc8 */ +#define FCS_INIT_VALUE 0xFF +#define FCS_GOOD_VALUE 0xCF + +#define EA 0x01 /* Extension bit */ +#define CR 0x02 /* Command / Response */ +#define PF 0x10 /* Poll / Final */ + +/* Frame types */ +#define FT_RR 0x01 /* Receive Ready */ +#define FT_UI 0x03 /* Unnumbered Information */ +#define FT_RNR 0x05 /* Receive Not Ready */ +#define FT_REJ 0x09 /* Reject */ +#define FT_DM 0x0F /* Disconnected Mode */ +#define FT_SABM 0x2F /* Set Asynchronous Balanced Mode */ +#define FT_DISC 0x43 /* Disconnect */ +#define FT_UA 0x63 /* Unnumbered Acknowledgement */ +#define FT_UIH 0xEF /* Unnumbered Information with Header check */ + +/* Control channel commands */ +#define CMD_NSC 0x08 /* Non Supported Command Response */ +#define CMD_TEST 0x10 /* Test Command */ +#define CMD_PSC 0x20 /* Power Saving Control */ +#define CMD_RLS 0x28 /* Remote Line Status Command */ +#define CMD_FCOFF 0x30 /* Flow Control Off Command */ +#define CMD_PN 0x40 /* DLC parameter negotiation */ +#define CMD_RPN 0x48 /* Remote Port Negotiation Command */ +#define CMD_FCON 0x50 /* Flow Control On Command */ +#define CMD_CLD 0x60 /* Multiplexer close down */ +#define CMD_SNC 0x68 /* Service Negotiation Command */ +#define CMD_MSC 0x70 /* Modem Status Command */ + +/* Flag sequence field between messages (start of frame) */ +#define SOF_MARKER 0xF9 +static uint8_t crc8(const uint8_t *src, size_t len, uint8_t polynomial, uint8_t initial_value, + bool reversed) +{ + uint8_t crc = initial_value; + size_t i, j; + + for (i = 0; i < len; i++) { + crc ^= src[i]; + + for (j = 0; j < 8; j++) { + if (reversed) { + if (crc & 0x01) { + crc = (crc >> 1) ^ polynomial; + } else { + crc >>= 1; + } + } else { + if (crc & 0x80) { + crc = (crc << 1) ^ polynomial; + } else { + crc <<= 1; + } + } + } + } + + return crc; +} + +void CMUXedTerminal::start() +{ + for (size_t i = 0; i < 3; i++) + { + send_sabm(i); + vTaskDelay(100 / portTICK_PERIOD_MS); // Waiting before open next DLC + } +} + +void DTE::send_sabm(size_t dlci) +{ + uint8_t frame[6]; + frame[0] = SOF_MARKER; + frame[1] = (dlci << 2) | 0x3; + frame[2] = FT_SABM | PF; + frame[3] = 1; + frame[4] = 0xFF - crc8(&frame[1], 3, FCS_POLYNOMIAL, FCS_INIT_VALUE, true); + frame[5] = SOF_MARKER; +// term->set_data_cb([&](size_t len) { +//// consumed = 0; +// auto data_to_read = std::min(len, DTE_BUFFER_SIZE - consumed); +// auto data = buffer.get() + consumed; +// auto actual_len = term->read(data, data_to_read); +// ESP_LOG_BUFFER_HEXDUMP("TEST", data, actual_len, ESP_LOG_INFO); +// auto available_len = consumed + actual_len; +// if (available_len > 4) { +// if (data[0] != SOF_MARKER) { +// ESP_LOGE("CMUX", "TODO: Recover!"); +// return true; +// } +// auto frame = buffer.get(); +// uint8_t dlci = frame[1] >> 2; +// uint8_t type = frame[2]; +// uint8_t length = frame[3] >> 1; +// ESP_LOGW("CMUX", "CMUX FR: A:%02x T:%02x L:%d consumed:%d", dlci, type, length, consumed); +// size_t frame_len = length + 6; +// if (available_len >= frame_len) { // we have entire frame +// if (frame[frame_len-1] != SOF_MARKER) { +// ESP_LOGE("CMUX", "TODO: Recover!"); +// return true; +// } +// if (type == (FT_UA | PF)) { +// ESP_LOGI("CMUX", "SAMB ok"); +// } +// +// } +// } +// return false; +// }); + term->write(frame, 6); +} + +bool CMUXedTerminal::process_cmux_recv(size_t len) +{ + return false; +} + +static DTE * s_dte; +bool DTE::s_on_cmux(size_t len) +{ + s_dte->on_cmux(len); + return false; +} + +bool output(uint8_t *data, size_t len, std::string message) +{ + printf("OUTPUT: %s len=%d \n", message.c_str(), len); + for (int i=0; i< len; ++i) { + printf("0x%02x, ",data[i]); + } + printf("----\n"); + + printf("%.*s", len, data); + return true; +} + +bool DTE::on_cmux(size_t len) +{ + auto data_to_read = std::min(len, buffer_size); + auto data = buffer.get(); + auto actual_len = term->read(data, data_to_read); +// consumed += actual_len; + ESP_LOG_BUFFER_HEXDUMP("Received", data, actual_len, ESP_LOG_INFO); + for (int i=0; i< len; ++i) { + printf("0x%02x, ",data[i]); + } + printf("\n"); + uint8_t* frame = data; + auto available_len = len; + size_t payload_offset = 0; + size_t footer_offset = 0; + while (available_len > 0) { + switch (state) { + case cmux_state::INIT: + if (frame[0] != SOF_MARKER) { + ESP_LOGW("CMUX", "TODO: Recover!"); + return true; + } + state = cmux_state::HEADER; + available_len--; + frame_header_offset = 1; + frame++; + break; + case cmux_state::HEADER: + 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); + ESP_LOGW("CMUX", "CMUX FR: A:%02x T:%02x L:%d consumed:%d", dlci, type, payload_len, 0); + frame += payload_offset; + available_len -= payload_offset; + state = cmux_state::PAYLOAD; + break; + case cmux_state::PAYLOAD: + if (available_len < payload_len) { // payload + state = cmux_state::PAYLOAD; + output(frame, available_len, "PAYLOAD partial read"); // partial read + payload_len -= available_len; + return false; + } else { // complete + if (payload_len > 0) { + output(&frame[0], payload_len, "PAYLOAD full read"); // rest read + } + available_len -= payload_len; + frame += payload_len; + state = cmux_state::FOOTER; + } + 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", "TODO: Recover!"); + return true; + } + if (payload_len == 0) { + output(frame_header, 0, "Null payload"); + } + frame += footer_offset; + available_len -= footer_offset; + state = cmux_state::INIT; + frame_header_offset = 0; + } + break; + } + } + return true; +} + + +void DTE::setup_cmux() +{ +// if (type == (FT_UA | PF)) { +// ESP_LOGI("CMUX", "SAMB ok"); +// } + + s_dte = this; + frame_header_offset = 0; + state = cmux_state::INIT; + term->set_data_cb(s_on_cmux); + + for (size_t i = 0; i < 3; i++) + { + send_sabm(i); + vTaskDelay(100 / portTICK_PERIOD_MS); // Waiting before open next DLC + } +} + + +void DTE::send_cmux_command(uint8_t i, const std::string& command) +{ +// send_sabm(i); +// vTaskDelay(100 / portTICK_PERIOD_MS); // Waiting before open next DLC + + uint8_t frame[6]; + frame[0] = SOF_MARKER; + frame[1] = (i << 2) + 1; + frame[2] = FT_UIH; + frame[3] = (command.length() << 1) + 1; + frame[4] = 0xFF - crc8(&frame[1], 3, FCS_POLYNOMIAL, FCS_INIT_VALUE, true); + frame[5] = SOF_MARKER; + + term->write(frame, 4); + term->write((uint8_t *)command.c_str(), command.length()); + term->write(frame + 4, 2); + ESP_LOG_BUFFER_HEXDUMP("Send", frame, 4, ESP_LOG_INFO); + ESP_LOG_BUFFER_HEXDUMP("Send", (uint8_t *)command.c_str(), command.length(), ESP_LOG_INFO); + ESP_LOG_BUFFER_HEXDUMP("Send", frame+4, 2, ESP_LOG_INFO); +} diff --git a/esp_modem/src/esp_modem_commands.cpp b/esp_modem/src/esp_modem_commands.cpp deleted file mode 100644 index 95d118bbf..000000000 --- a/esp_modem/src/esp_modem_commands.cpp +++ /dev/null @@ -1,79 +0,0 @@ -#include "cxx_include/esp_modem_commands.hpp" -#include "cxx_include/esp_modem_dte.hpp" -#include -#include - -namespace esp_modem::dce_commands { - - - -//template static inline command_result generic_command(T t, std::string command, -// std::string pass_phrase, std::string fail_phrase, uint32_t timeout_ms) -//{ -// return t->command(command.c_str(), [&](uint8_t *data, size_t len) { -// std::string response((char*)data, len); -// if (response.find(pass_phrase) != std::string::npos) { -// return command_result::OK; -// } else if (response.find(fail_phrase) != std::string::npos) { -// return command_result::FAIL; -// } -// return command_result::TIMEOUT; -// }, timeout_ms); -//} -// -//template static inline command_result generic_command_common(T t, std::string command) -//{ -// return generic_command(t, command, "OK", "ERROR", 500); -//} -// - -//template command_result sync(T t) -//{ -// return t->command("AT\r", [&](uint8_t *data, size_t len) { -// std::string response((char*)data, len); -// if (response.find("OK") != std::string::npos) { -// return command_result::OK; -// } else if (response.find("ERROR") != std::string::npos) { -// return command_result::FAIL; -// } -// return command_result::TIMEOUT; -// }, 1000); -//} - -//template command_result set_echo(T t, bool on) { return command_result::OK; } - - - -//template command_result set_echo(T* t, bool on) -//{ -// if (on) -// return generic_command_common(t, "ATE1\r"); -// return generic_command_common(t, "ATE0\r"); -//} -// -//template command_result set_data_mode(T t) -//{ -// return generic_command(t, "ATD*99##\r", "CONNECT", "ERROR", 5000); -//} -// -//template command_result set_pdp_context(T t, PdpContext& pdp_context) -//{ -// return generic_command(t, "ATD*99##\r", "CONNECT", "ERROR", 5000); -//} - -} - -namespace esp_modem { - -template -class generic_dce { -public: - explicit generic_dce(std::shared_ptr e) : dce_dte(std::move(e)) {} - - esp_err_t sync() { return dce_commands::sync(dce_dte); } - -private: - std::shared_ptr dce_dte; -}; - -} \ No newline at end of file diff --git a/esp_modem/src/esp_modem_dce.cpp b/esp_modem/src/esp_modem_dce.cpp index db5dc94ca..9efb60c2f 100644 --- a/esp_modem/src/esp_modem_dce.cpp +++ b/esp_modem/src/esp_modem_dce.cpp @@ -1,6 +1,6 @@ #include "cxx_include/esp_modem_dte.hpp" #include "cxx_include/esp_modem_dce.hpp" -#include +#include #include #include "esp_log.h" @@ -9,3 +9,25 @@ DCE::DCE(const std::shared_ptr& dte, std::shared_ptr device, esp_ dce_dte(dte), device(std::move(device)), ppp_netif(dte, netif) { } +void DCE::exit_data() { + ppp_netif.stop(); + device->set_mode(dte_mode::COMMAND_MODE); + uint8_t* data; + dce_dte->set_data_cb([&](size_t len) -> bool { + auto actual_len = dce_dte->read(&data, 64); + ESP_LOG_BUFFER_HEXDUMP("esp-modem: debug_data", data, actual_len, ESP_LOG_INFO); + return false; + }); + ppp_netif.wait_until_ppp_exits(); + dce_dte->set_data_cb(nullptr); + dce_dte->set_mode(dte_mode::COMMAND_MODE); +} + +void DCE::set_cmux() +{ + device->set_mode(dte_mode::CMUX_MODE); + dce_dte->set_mode(dte_mode::CMUX_MODE); +// auto t = dce_dte->detach(); +// auto cmux = std::make_unique(std::move(t), dce_dte->get_buffer()); +// dce_dte->attach(std::move(cmux)); +} \ No newline at end of file diff --git a/esp_modem/src/esp_modem_device.cpp b/esp_modem/src/esp_modem_device.cpp index c1d86ff8c..ad684d38d 100644 --- a/esp_modem/src/esp_modem_device.cpp +++ b/esp_modem/src/esp_modem_device.cpp @@ -5,6 +5,8 @@ #include "cxx_include/esp_modem_dce_commands.hpp" #include "cxx_include/esp_modem_dte.hpp" +#include "cxx_include/esp_modem_commands.hpp" + bool Device::setup_data_mode() { @@ -24,7 +26,21 @@ bool Device::set_mode(dte_mode mode) { return true; } else if (mode == dte_mode::COMMAND_MODE) { return set_command_mode() == command_result::OK; + } else if (mode == dte_mode::CMUX_MODE) { + return set_cmux() == command_result::OK; } return true; } + +command_result Device::set_echo(bool on) { return esp_modem::dce_commands::set_echo(dte, on); } +command_result Device::set_data_mode() { return esp_modem::dce_commands::set_data_mode(dte); } +command_result Device::resume_data_mode() { return esp_modem::dce_commands::resume_data_mode(dte); } +command_result Device::set_pdp_context(PdpContext& pdp_context) { return esp_modem::dce_commands::set_pdp_context(dte.get(), pdp_context); } +command_result Device::set_command_mode() { return esp_modem::dce_commands::set_command_mode(dte); } +command_result Device::set_cmux() { return esp_modem::dce_commands::set_cmux(dte); } +command_result Device::get_imsi(std::string& imsi_number) { return esp_modem::dce_commands::get_imsi(dte, imsi_number); } +command_result Device::set_pin(const std::string& pin) { return esp_modem::dce_commands::set_pin(dte, pin); } +command_result Device::read_pin(bool& pin_ok) { return esp_modem::dce_commands::read_pin(dte, pin_ok); } +command_result Device::get_imei(std::string &imei) { return esp_modem::dce_commands::get_imei(dte, imei); } +command_result Device::get_module_name(std::string &name) { return esp_modem::dce_commands::get_module_name(dte, name); } diff --git a/esp_modem/src/esp_modem_device_factory.cpp b/esp_modem/src/esp_modem_device_factory.cpp index 5e16a5edd..2fd49a573 100644 --- a/esp_modem/src/esp_modem_device_factory.cpp +++ b/esp_modem/src/esp_modem_device_factory.cpp @@ -1,7 +1,8 @@ #include "cxx_include/esp_modem_dce_commands.hpp" #include "cxx_include/esp_modem_dte.hpp" -std::shared_ptr create_device(const std::shared_ptr& dte, std::string &apn) +//std::shared_ptr create_device(const std::shared_ptr& dte, std::string &apn) +std::shared_ptr create_device(const std::shared_ptr& dte, std::string &apn) { auto pdp = std::make_unique(apn); return std::make_shared(dte, std::move(pdp)); diff --git a/esp_modem/src/esp_modem_dte.cpp b/esp_modem/src/esp_modem_dte.cpp index 44675e753..9feb80a59 100644 --- a/esp_modem/src/esp_modem_dte.cpp +++ b/esp_modem/src/esp_modem_dte.cpp @@ -2,11 +2,18 @@ #include #include "esp_log.h" -DTE::DTE(std::unique_ptr terminal): +DTE::DTE(std::unique_ptr terminal): buffer_size(DTE_BUFFER_SIZE), consumed(0), +// buffer(std::make_shared>(buffer_size)), +// buffer(new uint8_t[buffer_size], std::default_delete()), +// buffer(new uint8_t[buffer_size], std::default_delete()), +// buffer(nullptr), buffer(std::make_unique(buffer_size)), - term(std::move(terminal)), mode(dte_mode::UNDEF) {} + term(std::move(terminal)), mode(dte_mode::UNDEF) +{ +// buffer = new std::shared_ptr(buffer_size, [](uint8_t p[]){ delete[] p;}); +} command_result DTE::command(const std::string& command, got_line_cb got_line, uint32_t time_ms) { @@ -18,11 +25,14 @@ command_result DTE::command(const std::string& command, got_line_cb got_line, ui auto actual_len = term->read(data, data_to_read); consumed += actual_len; if (memchr(data, '\n', actual_len)) { +// ESP_LOGE("GOT", "LINE"); res = got_line(buffer.get(), consumed); if (res == command_result::OK || res == command_result::FAIL) { signal.set(GOT_LINE); + return true; } } + return false; }); auto got_lf = signal.wait(GOT_LINE, time_ms); if (got_lf && res == command_result::TIMEOUT) { diff --git a/esp_modem/src/esp_modem_uart.cpp b/esp_modem/src/esp_modem_uart.cpp index 50feae7ea..6c497e523 100644 --- a/esp_modem/src/esp_modem_uart.cpp +++ b/esp_modem/src/esp_modem_uart.cpp @@ -37,7 +37,7 @@ struct uart_task { explicit uart_task(size_t stack_size, size_t priority, void* task_param, TaskFunction_t task_function): task_handle(nullptr) { - BaseType_t ret = xTaskCreate(task_function, "uart_task", stack_size, task_param, priority, &task_handle); + BaseType_t ret = xTaskCreate(task_function, "uart_task", 10000, task_param, priority, &task_handle); throw_if_false(ret == pdTRUE, "create uart event task failed"); } ~uart_task() @@ -117,7 +117,7 @@ uart_resource::uart_resource(const struct dte_config *config): port = config->port_num; } -class uart_terminal: public terminal { +class uart_terminal: public Terminal { public: explicit uart_terminal(const struct dte_config *config): uart(config), event_loop(), signal(), @@ -135,7 +135,11 @@ public: int write(uint8_t *data, size_t len) override; int read(uint8_t *data, size_t len) override; - + void set_data_cb(std::function f) override + { + on_data = std::move(f); + signal.set(TASK_PARAMS); + } private: static void s_task(void * task_param) { @@ -145,9 +149,10 @@ private: } void task(); - const size_t TASK_INIT = BIT0; - const size_t TASK_START = BIT1; - const size_t TASK_STOP = BIT2; + static const size_t TASK_INIT = BIT0; + static const size_t TASK_START = BIT1; + static const size_t TASK_STOP = BIT2; + static const size_t TASK_PARAMS = BIT3; uart_resource uart; @@ -157,7 +162,7 @@ private: }; -std::unique_ptr create_uart_terminal(const dte_config *config) +std::unique_ptr create_uart_terminal(const dte_config *config) { try { auto term = std::make_unique(config); @@ -179,6 +184,7 @@ std::unique_ptr create_uart_terminal(const dte_config *config) void uart_terminal::task() { + std::function on_data_priv = nullptr; uart_event_t event; size_t len; signal.set(TASK_INIT); @@ -189,14 +195,20 @@ void uart_terminal::task() while(signal.is_any(TASK_START)) { event_loop.run(); if (uart.get_event(event, 100)) { + if (signal.is_any(TASK_PARAMS)) { + on_data_priv = on_data; + signal.clear(TASK_PARAMS); + } switch (event.type) { case UART_DATA: ESP_LOGI(TAG, "UART_DATA"); // ESP_LOG_BUFFER_HEXDUMP("esp-modem-pattern: debug_data", esp_dte->buffer, length, ESP_LOG_DEBUG); uart_get_buffered_data_len(uart.port, &len); ESP_LOGI(TAG, "UART_DATA len=%d, on_data=%d", len, (bool)on_data); - if (len && on_data) { - on_data(len); + if (len && on_data_priv) { + if (on_data_priv(len)) { + on_data_priv = nullptr; + } } break; case UART_FIFO_OVF: diff --git a/esp_modem/src/ppp_netif.cpp b/esp_modem/src/ppp_netif.cpp index 6ea1dc1b6..b0c077e33 100644 --- a/esp_modem/src/ppp_netif.cpp +++ b/esp_modem/src/ppp_netif.cpp @@ -15,38 +15,41 @@ // esp_netif_driver_base_t base; //}; -static void on_ppp_changed(void *arg, esp_event_base_t event_base, +void PPP::on_ppp_changed(void *arg, esp_event_base_t event_base, int32_t event_id, void *event_data) { - ppp *e = (ppp*)arg; + PPP *ppp = (PPP*)arg; // DTE *e = (DTE*)arg; std::cout << "on_ppp_changed " << std::endl; ESP_LOGW("TAG", "PPP state changed event %d", event_id); if (event_id < NETIF_PP_PHASE_OFFSET) { ESP_LOGI("TAG", "PPP state changed event %d", event_id); // only notify the modem on state/error events, ignoring phase transitions - e->notify_ppp_exit(); + ppp->signal.set(PPP_EXIT); +// e->notify_ppp_exit(); // e->data_mode_closed(); // esp_modem_notify_ppp_netif_closed(dte); } } -static esp_err_t esp_modem_dte_transmit(void *h, void *buffer, size_t len) +esp_err_t PPP::esp_modem_dte_transmit(void *h, void *buffer, size_t len) { - DTE *e = (DTE*)h; - std::cout << "sending data " << len << std::endl; - if (e->write((uint8_t*)buffer, len) > 0) { - return ESP_OK; + PPP *ppp = (PPP*)h; + if (ppp->signal.is_any(PPP_STARTED)) { + std::cout << "sending data " << len << std::endl; + if (ppp->ppp_dte->write((uint8_t*)buffer, len) > 0) { + return ESP_OK; + } } return ESP_FAIL; } -static esp_err_t esp_modem_post_attach(esp_netif_t * esp_netif, void * args) +esp_err_t PPP::esp_modem_post_attach(esp_netif_t * esp_netif, void * args) { auto d = (ppp_netif_driver*)args; esp_netif_driver_ifconfig_t driver_ifconfig = { }; - driver_ifconfig.transmit = esp_modem_dte_transmit; - driver_ifconfig.handle = (void*)d->e; + driver_ifconfig.transmit = PPP::esp_modem_dte_transmit; + driver_ifconfig.handle = (void*)d->ppp; std::cout << "esp_modem_post_attach " << std::endl; d->base.netif = esp_netif; ESP_ERROR_CHECK(esp_netif_set_driver_config(esp_netif, &driver_ifconfig)); @@ -54,47 +57,51 @@ static esp_err_t esp_modem_post_attach(esp_netif_t * esp_netif, void * args) // to notify the modem layer when switching modes esp_netif_ppp_config_t ppp_config; esp_netif_ppp_get_params(esp_netif, &ppp_config); -// if (!ppp_config.ppp_error_event_enabled) { + if (!ppp_config.ppp_error_event_enabled) { ppp_config.ppp_error_event_enabled = true; - ppp_config.ppp_phase_event_enabled = true; esp_netif_ppp_set_params(esp_netif, &ppp_config); -// } + } // ESP_ERROR_CHECK(esp_event_handler_register(NETIF_PPP_STATUS, ESP_EVENT_ANY_ID, &on_ppp_changed, 0)); return ESP_OK; } -void ppp::receive(uint8_t *data, size_t len) const +void PPP::receive(uint8_t *data, size_t len) { - std::cout << "received data " << len << std::endl; - esp_netif_receive(driver.base.netif, data, len, nullptr); + if (signal.is_any(PPP_STARTED)) { + std::cout << "received data " << len << std::endl; + esp_netif_receive(driver.base.netif, data, len, nullptr); + } } -ppp::ppp(std::shared_ptr e, esp_netif_t *ppp_netif): +PPP::PPP(std::shared_ptr e, esp_netif_t *ppp_netif): ppp_dte(std::move(e)), netif(ppp_netif) { driver.base.netif = ppp_netif; - driver.e = this->ppp_dte.get(); + driver.ppp = this; driver.base.post_attach = esp_modem_post_attach; - ppp_dte->set_data_cb([&](size_t len){ - uint8_t *data; - auto actual_len = ppp_dte->read(&data, len); - return receive(data, actual_len); - }); throw_if_esp_fail(esp_event_handler_register(NETIF_PPP_STATUS, ESP_EVENT_ANY_ID, &on_ppp_changed, (void*)this)); throw_if_esp_fail(esp_event_handler_register(IP_EVENT, IP_EVENT_PPP_GOT_IP, esp_netif_action_connected, ppp_netif)); throw_if_esp_fail(esp_event_handler_register(IP_EVENT, IP_EVENT_PPP_LOST_IP, esp_netif_action_disconnected, ppp_netif)); throw_if_esp_fail(esp_netif_attach(ppp_netif, &driver)); } -void ppp::start() +void PPP::start() { + ppp_dte->set_data_cb([&](size_t len) -> bool { + uint8_t *data; + auto actual_len = ppp_dte->read(&data, len); + receive(data, actual_len); + return false; + }); esp_netif_action_start(driver.base.netif, 0, 0, 0); + signal.set(PPP_STARTED); } -void ppp::stop() +void PPP::stop() { std::cout << "esp_netif_action_stop " << std::endl; esp_netif_action_stop(driver.base.netif, nullptr, 0, nullptr); + signal.clear(PPP_STARTED); } \ No newline at end of file