1  #include "USBHandler.h"
  2  #include "diag_log.h"
  3  #include <cstring>
  4  
  5  USBHandler::USBHandler() {
  6      DIAG("USB", "USBHandler constructed, calling reset()");
  7      reset();
  8  }
  9  
 10  void USBHandler::reset() {
 11      DIAG("USB", "USBHandler::reset(): state -> WAITING_FOR_START");
 12      current_state = USBState::WAITING_FOR_START;
 13      start_flag_received = false;
 14      buffer_index = 0;
 15      current_settings.resetToDefaults();
 16  }
 17  
 18  void USBHandler::processUSBData(const uint8_t* data, uint32_t length) {
 19      if (data == nullptr || length == 0) {
 20          DIAG_WARN("USB", "processUSBData: null/empty data");
 21          return;
 22      }
 23      
 24      DIAG("USB", "processUSBData: %lu bytes, state=%d", (unsigned long)length, (int)current_state);
 25      
 26      switch (current_state) {
 27          case USBState::WAITING_FOR_START:
 28              processStartFlag(data, length);
 29              break;
 30              
 31          case USBState::RECEIVING_SETTINGS:
 32              processSettingsData(data, length);
 33              break;
 34              
 35          case USBState::READY_FOR_DATA:
 36              // Ready to receive radar data commands
 37              DIAG("USB", "  READY_FOR_DATA: ignoring %lu bytes", (unsigned long)length);
 38              break;
 39      }
 40  }
 41  
 42  void USBHandler::processStartFlag(const uint8_t* data, uint32_t length) {
 43      // Start flag: bytes [23, 46, 158, 237]
 44      const uint8_t START_FLAG[] = {23, 46, 158, 237};
 45      
 46      // Check if start flag is in the received data
 47      for (uint32_t i = 0; i <= length - 4; i++) {
 48          if (memcmp(data + i, START_FLAG, 4) == 0) {
 49              start_flag_received = true;
 50              current_state = USBState::RECEIVING_SETTINGS;
 51              buffer_index = 0;  // Reset buffer for settings data
 52              DIAG("USB", "START FLAG found at offset %lu, state -> RECEIVING_SETTINGS", (unsigned long)i);
 53              
 54              // If there's more data after the start flag, process it
 55              if (length > i + 4) {
 56                  DIAG("USB", "  %lu trailing bytes after start flag, forwarding to settings parser", (unsigned long)(length - i - 4));
 57                  processSettingsData(data + i + 4, length - i - 4);
 58              }
 59              return;
 60          }
 61      }
 62      DIAG("USB", "  no start flag in %lu bytes", (unsigned long)length);
 63  }
 64  
 65  void USBHandler::processSettingsData(const uint8_t* data, uint32_t length) {
 66      // Add data to buffer
 67      uint32_t bytes_to_copy = (length < (MAX_BUFFER_SIZE - buffer_index)) ? 
 68                               length : (MAX_BUFFER_SIZE - buffer_index);
 69      
 70      memcpy(usb_buffer + buffer_index, data, bytes_to_copy);
 71      buffer_index += bytes_to_copy;
 72      DIAG("USB", "  settings buffer: +%lu bytes, total=%lu/%u", (unsigned long)bytes_to_copy, (unsigned long)buffer_index, MAX_BUFFER_SIZE);
 73      
 74      // Check if we have a complete settings packet (contains "SET" and "END")
 75      if (buffer_index >= 74) {  // Minimum size for valid settings packet
 76          // Look for "SET" at beginning and "END" somewhere in the packet
 77          bool has_set = (memcmp(usb_buffer, "SET", 3) == 0);
 78          bool has_end = false;
 79          
 80          DIAG_BOOL("USB", "  packet starts with SET", has_set);
 81          
 82          for (uint32_t i = 3; i <= buffer_index - 3; i++) {
 83              if (memcmp(usb_buffer + i, "END", 3) == 0) {
 84                  has_end = true;
 85                  DIAG("USB", "  END marker found at offset %lu, packet_len=%lu", (unsigned long)i, (unsigned long)(i + 3));
 86                  
 87                  // Parse the complete packet up to "END"
 88                  if (has_set && current_settings.parseFromUSB(usb_buffer, i + 3)) {
 89                      current_state = USBState::READY_FOR_DATA;
 90                      DIAG("USB", "  Settings parsed OK, state -> READY_FOR_DATA");
 91                  } else {
 92                      DIAG_ERR("USB", "  Settings parse FAILED (has_set=%d)", has_set);
 93                  }
 94                  break;
 95              }
 96          }
 97          
 98          // If we didn't find a valid packet but buffer is full, reset
 99          if (buffer_index >= MAX_BUFFER_SIZE && !has_end) {
100              DIAG_WARN("USB", "  Buffer full (%u) without END marker -- resetting", MAX_BUFFER_SIZE);
101              buffer_index = 0;  // Reset buffer to avoid overflow
102          }
103      }
104  }