diff --git a/src/improv.cpp b/src/improv.cpp index 8a540e9..ad145c0 100644 --- a/src/improv.cpp +++ b/src/improv.cpp @@ -7,6 +7,18 @@ ImprovCommand parse_improv_data(const std::vector &data, bool check_che return parse_improv_data(data.data(), data.size(), check_checksum); } +ImprovCommand parse_data_command(Command command, const uint8_t *data, size_t length) { + std::vector> data_fields; + size_t field_start = 3; + size_t field_end = field_start + data[2]; + while (field_end < length) { + data_fields.push_back(std::vector(data + field_start, data + field_end)); + field_start = field_end + 1; + field_end = field_start + data[field_start - 1]; + }; + return {.command = command, .data = data_fields}; +} + ImprovCommand parse_improv_data(const uint8_t *data, size_t length, bool check_checksum) { ImprovCommand improv_command; Command command = (Command) data[0]; @@ -32,25 +44,16 @@ ImprovCommand parse_improv_data(const uint8_t *data, size_t length, bool check_c } if (command == WIFI_SETTINGS) { - uint8_t ssid_length = data[2]; - uint8_t ssid_start = 3; - size_t ssid_end = ssid_start + ssid_length; - if (ssid_end > length) { + improv_command = parse_data_command(command, data, length); + // There must be two data fields for the SSID and password + if (improv_command.data.size() != 2) { improv_command.command = UNKNOWN; - return improv_command; - } - - uint8_t pass_length = data[ssid_end]; - size_t pass_start = ssid_end + 1; - size_t pass_end = pass_start + pass_length; - if (pass_end > length) { - improv_command.command = UNKNOWN; - return improv_command; } + return improv_command; + } - std::string ssid(data + ssid_start, data + ssid_end); - std::string password(data + pass_start, data + pass_end); - return {.command = command, .ssid = ssid, .password = password}; + if (command == CUSTOM) { + return parse_data_command(command, data, length); } improv_command.command = command; diff --git a/src/improv.h b/src/improv.h index d6befbd..7c12b01 100644 --- a/src/improv.h +++ b/src/improv.h @@ -42,6 +42,7 @@ enum Command : uint8_t { GET_CURRENT_STATE = 0x02, GET_DEVICE_INFO = 0x03, GET_WIFI_NETWORKS = 0x04, + CUSTOM = 0x80, BAD_CHECKSUM = 0xFF, }; @@ -57,8 +58,19 @@ enum ImprovSerialType : uint8_t { struct ImprovCommand { Command command; - std::string ssid; - std::string password; + std::vector> data; + std::string ssid() const { + if (!data.empty() && !data[0].empty() && command == WIFI_SETTINGS) { + return std::string(data[0].begin(), data[0].end()); + } + return ""; + } + std::string password() const { + if (data.size() > 1 && !data[1].empty() && command == WIFI_SETTINGS) { + return std::string(data[1].begin(), data[1].end()); + } + return ""; + } }; ImprovCommand parse_improv_data(const std::vector &data, bool check_checksum = true);