feat: Add support for custom commands

src/improv.cpp

@@ -7,6 +7,18 @@ ImprovCommand parse_improv_data(const std::vector<uint8_t> &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<std::vector<uint8_t>> 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<uint8_t>(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;

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<std::vector<uint8_t>> 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<uint8_t> &data, bool check_checksum = true);