better i2c protocol implentation

src/comms.cpp

@@ -937,78 +937,48 @@ uint8_t crc8(const uint8_t *data, size_t len) {
     return crc;
 }
 
-void total_pc_control() {
+bool execute_pc_command(uint8_t *data, uint8_t *&p, uint8_t *output, uint8_t len, uint32_t* baud_change) {
-  static uint8_t data[127];
-  static uint8_t output[257];
-  uint8_t *p = output + 2;
-  uint32_t baud_change = 0;
-
-  if(i2c_pc_control_init) {
-    Serial.write(1);
-    Serial.write(0xff);
-    Serial.flush();
-    i2c_pc_control_init = false;
-  }
-  if(Serial.available()) {
-    uint8_t userlen = Serial.read();
-    if (userlen == 0) return;
-
-    if(Serial.available() && userlen == '~' && Serial.peek() == '/') {
-      Serial.read();
-      Serial.write(1);
-      Serial.write(0xff);
-      Serial.flush(true);
-      return;
-    }
-
-    bool has_crc = (userlen >> 7) == 1;
-    uint8_t orig_userlen = userlen;
-    userlen &= 127;
-
-    auto len = Serial.read(data, userlen);
-    if(len != userlen) return;
-
-    if(has_crc && Serial.available()) {
-      uint8_t crc = Serial.read();
-
-      uint8_t expected_crc = 0;
-      expected_crc = crc8_update(expected_crc, orig_userlen);
-      for (int i = 0; i < len; i++) expected_crc = crc8_update(expected_crc, data[i]);
-      if(crc != expected_crc) {
-        Serial.write(0x02);
-        Serial.write(0xFF);
-        Serial.write(0x01);
-        Serial.flush();
-        return;
-      }
-    }
-  
   switch (data[0]) {
   case 0: { // Set clock
-      if(len < 5) return;
+    if(len < 5) {
+      *p++ = 2;
+      return true;
+    }
     uint32_t clock = ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | ((uint32_t)data[3] << 8)  | ((uint32_t)data[4]);
     Wire.setClock(clock);
   } break;
   case 1: { // Send data
-      if(len < 3) return;
+    if(len < 3) {
+      *p++ = 2;
+      return true;
+    }
     Wire.beginTransmission(data[1]);
     Wire.write(data + 2, len - 2);
     auto out = Wire.endTransmission();
     *p++ = out;
   } break;
   case 2: { // Send and receive data
-      if(len < 4) return; // Need at least: cmd, addr, datalen, recvlen
+    if(len < 4) {
+      *p++ = 2;
+      return true;
+    }
     uint8_t addr = data[1];
     uint8_t datalen = data[2];
     
-      if(len < 3 + datalen + 1) return; // Validate buffer size
+    if(len < 3 + datalen + 1) {
+      *p++ = 2;
+      return true;
+    }
     
     Wire.beginTransmission(addr);
     Wire.write(data + 3, datalen);
     auto out = Wire.endTransmission(false);
     
     uint8_t recvlen = Wire.requestFrom(addr, data[3+datalen]);
-      if ((p - output) + recvlen >= sizeof(output)) return;
+    if ((p - output) + recvlen >= sizeof(output)) {
+      *p++ = 2;
+      return true;
+    }
 
     *p++ = out;
     while(Wire.available()) *p++ = Wire.read();
@@ -1016,10 +986,10 @@ void total_pc_control() {
   case 3: { // Quit
     i2c_pc_control = false;
     MuteScreen(false);
-      baud_change = 115200;
+    *baud_change = 115200;
   } break;
   case 4: { // Version
-      *p++ = 2;
+    *p++ = 3;
   } break;
   case 5: { // Reboot
     Serial.write(1);
@@ -1028,17 +998,26 @@ void total_pc_control() {
     esp_restart();
   } break;
   case 6: { // Change baud
-      if(len < 5) return;
+    if(len < 5) {
-      baud_change = ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) |
+      *p++ = 2;
+      return true;
+    }
+    *baud_change = ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) |
               ((uint32_t)data[3] << 8) | ((uint32_t)data[4]);
   } break;
   case 7: { // Write to EEPROM
-      if(len < 4) return;
+    if(len < 4) {
+      *p++ = 2;
+      return true;
+    }
     EEPROM.writeBytes((data[1] << 8) | data[2], data + 3, len - 3);
     EEPROM.commit();
   } break;
   case 8: { // Read from EEPROM
-      if(len < 4) return;
+    if(len < 4) {
+      *p++ = 2;
+      return true;
+    }
     auto address = (data[1] << 8) | data[2];
     *p++ = data[3] + 1;
     *p++ = 8;
@@ -1055,13 +1034,76 @@ void total_pc_control() {
     *p++ = EE_BYTE_CONTROLMODE & 0xff;
   } break;
   case 0xff:
-    default:
     break;
+  default:
+    *p++ = 3;
+    return true;
+  }
+  return false;
 }
 
+void total_pc_control() {
+  static uint8_t data[127];
+  static uint8_t output[257];
+  uint8_t *p = output + 2;
+  uint32_t baud_change = 0;
+  bool error = false;
+  bool done = false;
+  bool send_crc = false;
+
+  if(i2c_pc_control_init) {
+    error = true;
+    done = true;
+    i2c_pc_control_init = false;
+  }
+  if(Serial.available() && !done) {
+    uint8_t userlen = Serial.read();
+    if (userlen == 0) {
+      Serial.flush();
+      return;
+    }
+
+    if(Serial.available() && userlen == '~' && Serial.peek() == '/') {
+      Serial.read();
+      error = true;
+      done = true;
+    }
+
+    if(!done) {
+      bool has_crc = (userlen >> 7) == 1;
+      send_crc = has_crc;
+      uint8_t orig_userlen = userlen;
+      userlen &= 127;
+
+      auto len = Serial.read(data, userlen);
+      if(len != userlen) {
+        error = true;
+        *p++ = 0;
+      }
+
+      if(has_crc && Serial.available()) {
+        uint8_t crc = Serial.read();
+
+        uint8_t expected_crc = 0;
+        expected_crc = crc8_update(expected_crc, orig_userlen);
+        for (int i = 0; i < len; i++) expected_crc = crc8_update(expected_crc, data[i]);
+        if(crc != expected_crc) {
+          error = true;
+          send_crc = false;
+          *p++ = 1;
+        }
+      }
+      if(!error) error = execute_pc_command(data, p, output, len, &baud_change);
+      done = true;
+    }
+  }
+  
+  if(done) {
     output[0] = (p - output) - 1;
-    output[1] = data[0];
+    if(error) output[1] = 0xff;
-    if(has_crc) {
+    else output[1] = data[0];
+  
+    if(send_crc) {
       output[0] |= 0x80;
       uint8_t crc = crc8(output, p - output);
       *p++ = crc;
@@ -1075,6 +1117,6 @@ void total_pc_control() {
       baud_change = 0;
     }
 
-    Serial.flush(true);
+    Serial.flush(!error);
   }
 }