From 63c7bd5640fe9221a5c012b981b668d32b700d53 Mon Sep 17 00:00:00 2001 From: KubaPro010 Date: Fri, 20 Feb 2026 13:51:44 +0100 Subject: [PATCH] better i2c protocol implentation --- src/comms.cpp | 256 +++++++++++++++++++++++++++++--------------------- 1 file changed, 149 insertions(+), 107 deletions(-) diff --git a/src/comms.cpp b/src/comms.cpp index 1e40d62..6c52420 100644 --- a/src/comms.cpp +++ b/src/comms.cpp @@ -937,131 +937,173 @@ uint8_t crc8(const uint8_t *data, size_t len) { return crc; } +bool execute_pc_command(uint8_t *data, uint8_t *&p, uint8_t *output, uint8_t len, uint32_t* baud_change) { + switch (data[0]) { + case 0: { // Set clock + 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) { + *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) { + *p++ = 2; + return true; + } + uint8_t addr = data[1]; + uint8_t datalen = data[2]; + + 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)) { + *p++ = 2; + return true; + } + + *p++ = out; + while(Wire.available()) *p++ = Wire.read(); + } break; + case 3: { // Quit + i2c_pc_control = false; + MuteScreen(false); + *baud_change = 115200; + } break; + case 4: { // Version + *p++ = 3; + } break; + case 5: { // Reboot + Serial.write(1); + Serial.write(5); + Serial.flush(); + esp_restart(); + } break; + case 6: { // Change baud + if(len < 5) { + *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) { + *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) { + *p++ = 2; + return true; + } + auto address = (data[1] << 8) | data[2]; + *p++ = data[3] + 1; + *p++ = 8; + for(uint16_t i = 0; i < data[3]; i++) *p++ = EEPROM.read(address + i); + } break; + case 0xfd: { // Get EEPROM address for userdata + *p++ = (uint8_t)(EE_START_CONTROLMODE_DATA >> 8); + *p++ = EE_START_CONTROLMODE_DATA & 0xff; + *p++ = (uint8_t)(EE_LEN_CONTROLMODE_DATA >> 8); + *p++ = EE_LEN_CONTROLMODE_DATA & 0xff; + } break; + case 0xfe: { // Get EEPROM address for starting control mode on boot + *p++ = (uint8_t)(EE_BYTE_CONTROLMODE >> 8); + *p++ = EE_BYTE_CONTROLMODE & 0xff; + } break; + case 0xff: + 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) { - Serial.write(1); - Serial.write(0xff); - Serial.flush(); + error = true; + done = true; i2c_pc_control_init = false; } - if(Serial.available()) { + if(Serial.available() && !done) { 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); + if (userlen == 0) { + Serial.flush(); return; } - bool has_crc = (userlen >> 7) == 1; - uint8_t orig_userlen = userlen; - userlen &= 127; + if(Serial.available() && userlen == '~' && Serial.peek() == '/') { + Serial.read(); + error = true; + done = true; + } - auto len = Serial.read(data, userlen); - if(len != userlen) return; + if(!done) { + bool has_crc = (userlen >> 7) == 1; + send_crc = has_crc; + uint8_t orig_userlen = userlen; + userlen &= 127; - 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; + 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; } + } - switch (data[0]) { - case 0: { // Set clock - if(len < 5) return; - 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; - 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 - uint8_t addr = data[1]; - uint8_t datalen = data[2]; - - if(len < 3 + datalen + 1) return; // Validate buffer size - - 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; - - *p++ = out; - while(Wire.available()) *p++ = Wire.read(); - } break; - case 3: { // Quit - i2c_pc_control = false; - MuteScreen(false); - baud_change = 115200; - } break; - case 4: { // Version - *p++ = 2; - } break; - case 5: { // Reboot - Serial.write(1); - Serial.write(5); - Serial.flush(); - esp_restart(); - } break; - case 6: { // Change baud - if(len < 5) return; - 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; - EEPROM.writeBytes((data[1] << 8) | data[2], data + 3, len - 3); - EEPROM.commit(); - } break; - case 8: { // Read from EEPROM - if(len < 4) return; - auto address = (data[1] << 8) | data[2]; - *p++ = data[3] + 1; - *p++ = 8; - for(uint16_t i = 0; i < data[3]; i++) *p++ = EEPROM.read(address + i); - } break; - case 0xfd: { // Get EEPROM address for userdata - *p++ = (uint8_t)(EE_START_CONTROLMODE_DATA >> 8); - *p++ = EE_START_CONTROLMODE_DATA & 0xff; - *p++ = (uint8_t)(EE_LEN_CONTROLMODE_DATA >> 8); - *p++ = EE_LEN_CONTROLMODE_DATA & 0xff; - } break; - case 0xfe: { // Get EEPROM address for starting control mode on boot - *p++ = (uint8_t)(EE_BYTE_CONTROLMODE >> 8); - *p++ = EE_BYTE_CONTROLMODE & 0xff; - } break; - case 0xff: - default: - break; - } - + if(done) { output[0] = (p - output) - 1; - output[1] = data[0]; - if(has_crc) { + if(error) output[1] = 0xff; + 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); } } \ No newline at end of file