Merge pull request #94 from xeniter/ble_less_byte_fix

Fix reboot of esp32 when bluetooth receives only one byte (happens with my phone with the vesc firmware update)
thankthemaker · Sep 28, 2024 · f323179 · f323179
2 parents e0f7de8 + 47fdedf
commit f323179
Showing 1 changed file with 27 additions and 9 deletions.
diff --git a/src/BleCanProxy.cpp b/src/BleCanProxy.cpp
@@ -9,19 +9,30 @@ BleCanProxy::BleCanProxy(CanDevice *candevice, Stream *stream, uint8_t vesc_id,
 }
 
 void BleCanProxy::proxyIn(std::string in) {
-    auto packet_type = (uint8_t) in.at(0);
+    ESP_LOGD(LOG_TAG_BLE_CAN_PROXY, "Proxy in, input=%d\n", in.size());
+    longPackBuffer += in;
+    ESP_LOGD(LOG_TAG_BLE_CAN_PROXY, "Proxy in, buffer=%d\n", longPackBuffer.size());
+
+    // we need at least 4 bytes for initial packet parsing
+    if (longPackBuffer.size() < 4) {
+        return;
+    }
+
+    auto packet_type = (uint8_t) longPackBuffer.at(0);
 
     if (!longPacket) {
         switch (packet_type) {
             case 2:
-                length = (uint8_t) in.at(1);
-                command = (uint8_t) in.at(2);
-                processing = true;
-                if (length > (in.size() - 5)) {
+                // packet_type(1) + length(1) + payload(length)) + crc(2) + end_byte(1) 
+                length = (uint8_t) longPackBuffer.at(1);
+                command = (uint8_t) longPackBuffer.at(2);
+                processing = true;                
+                if (length > 6) {
                     longPacket = true;
                 }
                 break;
             case 3:
+                // packet_type(1) + length(2) + payload(length)) + crc(2) + end_byte(1)
                 length = ((uint8_t) in.at(1) << 8) + (uint8_t) in.at(2);
                 command = (uint8_t) in.at(3);
                 longPacket = true;
@@ -33,7 +44,12 @@ void BleCanProxy::proxyIn(std::string in) {
         ESP_LOGD(LOG_TAG_BLE_CAN_PROXY, "Proxy in, command %d, length %d\n", command, length);
     }
 
-    if (length <= 6) {
+    if (length <= 6) { // short packet (nocrc?)
+
+        if (length + 3 > longPackBuffer.size()) {            
+            return;
+        }
+
         twai_message_t tx_frame = {};
         tx_frame.extd = 1;
         tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_PROCESS_SHORT_BUFFER) << 8) + vesc_id;
@@ -42,11 +58,10 @@ void BleCanProxy::proxyIn(std::string in) {
         tx_frame.data[1] = 0x00;
         tx_frame.data[2] = command;
         for (int i = 3; i < length + 2; i++) {
-            tx_frame.data[i] = (uint8_t) in.at(i);
+            tx_frame.data[i] = (uint8_t) longPackBuffer.at(i);
         }
         candevice->sendCanFrame(&tx_frame);
-    } else {
-        longPackBuffer += in;
+    } else { // long packet        
         if (length + 5 > longPackBuffer.size()) {
             //Serial.printf("Buffer not full, needed %d, is %d\n", length + 5, longPackBuffer.size());
             return;
@@ -67,6 +82,7 @@ void BleCanProxy::proxyIn(std::string in) {
             int sendLen = (length >= byteNum+ 7) ? 7 : length - byteNum;
             //Serial.printf("bufLen %d, byteNum %d, sendlen %d\n", length, byteNum , sendLen);
 
+            ESP_LOGD(LOG_TAG_BLE_CAN_PROXY, "Proxy in, CAN_PACKET_FILL_RX_BUFFER");
             twai_message_t tx_frame = {};
             tx_frame.extd = 1;
             tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_FILL_RX_BUFFER) << 8) + vesc_id;
@@ -85,6 +101,7 @@ void BleCanProxy::proxyIn(std::string in) {
             int sendLen = (length >= byteNum + 6) ? 6 : length - byteNum ;
             //Serial.printf("bufLen %d, byteNum %d, sendlen %d\n", length, byteNum , sendLen);
 
+            ESP_LOGD(LOG_TAG_BLE_CAN_PROXY, "Proxy in, CAN_PACKET_FILL_RX_BUFFER_LONG");
             twai_message_t tx_frame = {};
             tx_frame.extd = 1;
             tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_FILL_RX_BUFFER_LONG) << 8) + vesc_id;
@@ -100,6 +117,7 @@ void BleCanProxy::proxyIn(std::string in) {
             candevice->sendCanFrame(&tx_frame);
         }
 
+        ESP_LOGD(LOG_TAG_BLE_CAN_PROXY, "Proxy in, CAN_PACKET_PROCESS_RX_BUFFER");
         twai_message_t tx_frame = {};
         tx_frame.extd = 1;
         tx_frame.identifier = (uint32_t(0x8000) << 16) + (uint16_t(CAN_PACKET_PROCESS_RX_BUFFER) << 8) + vesc_id;