2 files changed, 14 insertions, 7 deletions

diff --git a/src/uds/uds.c b/src/uds/uds.c
index 636bbfca..0114384d 100644
--- a/src/uds/uds.c
+++ b/src/uds/uds.c
@@ -53,7 +53,9 @@ static void setup_receive_handle(DiagnosticRequestHandle* handle) {
 static uint16_t autoset_pid_length(uint8_t mode, uint16_t pid,
         uint8_t pid_length) {
     if(pid_length == 0) {
-        if(pid > 0xffff || (mode != 0x3e && mode > 0xa)) {
+        if(mode <= 0xa || mode == 0x3e ) {
+            pid_length = 1;
+        } else if(pid > 0xffff || ((pid & 0xFF00) > 0x0)) {
             pid_length = 2;
         } else {
             pid_length = 1;
@@ -69,7 +71,6 @@ static void send_diagnostic_request(DiagnosticShims* shims,
     if(handle->request.has_pid) {
         handle->request.pid_length = autoset_pid_length(handle->request.mode,
                 handle->request.pid, handle->request.pid_length);
-        handle->request.pid_length = handle->request.pid_length;
         set_bitfield(handle->request.pid, PID_BYTE_INDEX * CHAR_BIT,
                 handle->request.pid_length * CHAR_BIT, payload,
                 sizeof(payload));
@@ -233,6 +234,7 @@ DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims,
 
     DiagnosticResponse response = {
         arbitration_id: arbitration_id,
+        multi_frame: false,
         success: false,
         completed: false
     };
@@ -246,6 +248,7 @@ DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims,
             IsoTpMessage message = isotp_continue_receive(&handle->isotp_shims,
                     &handle->isotp_receive_handles[i], arbitration_id, data,
                     size);
+            response.multi_frame = message.multi_frame;
 
             if(message.completed) {
                 if(message.size > 0) {
diff --git a/src/uds/uds_types.h b/src/uds/uds_types.h
index 68d6b6f5..66bf7437 100644
--- a/src/uds/uds_types.h
+++ b/src/uds/uds_types.h
@@ -9,9 +9,10 @@
 extern "C" {
 #endif
 
-// TODO This isn't true for multi frame messages - we may need to dynamically
-// allocate this in the future
-#define MAX_UDS_PAYLOAD_LENGTH 7
+// TODO This still doesn't have enough space for the largest possible 
+// multiframe response. May need to dynamically allocate in the future.
+#define MAX_UDS_RESPONSE_PAYLOAD_LENGTH 255
+#define MAX_UDS_REQUEST_PAYLOAD_LENGTH 7
 #define MAX_RESPONDING_ECU_COUNT 8
 #define VIN_LENGTH 17
 
@@ -54,7 +55,7 @@ typedef struct {
     bool has_pid;
     uint16_t pid;
     uint8_t pid_length;
-    uint8_t payload[MAX_UDS_PAYLOAD_LENGTH];
+    uint8_t payload[MAX_UDS_REQUEST_PAYLOAD_LENGTH];
     uint8_t payload_length;
     bool no_frame_padding;
     DiagnosticRequestType type;
@@ -92,6 +93,8 @@ typedef enum {
  *      field isn't valid if 'completed' isn't true. If this is 'false', check
  *      the negative_response_code field for the reason.
  * arbitration_id - The arbitration ID the response was received on.
+ * multi_frame - True if this response (whether completed or not) required
+ *      multi-frame CAN support. Can be used for updating time-out functions.
  * mode - The OBD-II mode for the original request.
  * has_pid - If this is a response to a PID request, this will be true and the
  *      'pid' field will be valid.
@@ -106,12 +109,13 @@ typedef enum {
 typedef struct {
     bool completed;
     bool success;
+    bool multi_frame;
     uint32_t arbitration_id;
     uint8_t mode;
     bool has_pid;
     uint16_t pid;
     DiagnosticNegativeResponseCode negative_response_code;
-    uint8_t payload[MAX_UDS_PAYLOAD_LENGTH];
+    uint8_t payload[MAX_UDS_RESPONSE_PAYLOAD_LENGTH];
     uint8_t payload_length;
 } DiagnosticResponse;