Don't complete requests if mode or PID didn't match.

2 files changed, 31 insertions, 20 deletions

diff --git a/src/obd2/obd2.c b/src/obd2/obd2.c
index 73e13b2f..88ce74cf 100644
--- a/src/obd2/obd2.c
+++ b/src/obd2/obd2.c
@@ -137,15 +137,16 @@ static bool handle_positive_response(DiagnosticRequestHandle* handle,
         // hide the "response" version of the mode from the user
         // if it matched
         response->mode = handle->request.mode;
+        bool has_pid = false;
         if(handle->request.pid_length > 0 && message->size > 1) {
+            has_pid = true;
             if(handle->request.pid_length == 2) {
                 response->pid = get_bitfield(message->payload, message->size,
                         PID_BYTE_INDEX * CHAR_BIT, sizeof(uint16_t) * CHAR_BIT);
             } else {
                 response->pid = message->payload[PID_BYTE_INDEX];
             }
-            // TODO we're not currently throwing an error or anything if the PID
-            // doesn't match - it may be OK to leave that up to the user.
+
         }
 
         uint8_t payload_index = 1 + handle->request.pid_length;
@@ -154,8 +155,13 @@ static bool handle_positive_response(DiagnosticRequestHandle* handle,
             memcpy(response->payload, &message->payload[payload_index],
                     response->payload_length);
         }
-        response->success = true;
-        response->completed = true;
+
+        if(!has_pid || response->pid == handle->request.pid) {
+            response->success = true;
+            response->completed = true;
+        } else {
+            response_was_positive = false;
+        }
     }
     return response_was_positive;
 }
@@ -192,22 +198,14 @@ DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims,
                     handle->success = true;
                     handle->completed = true;
                 } else {
-                    shims->log("Response was for a mode 0x%x request, not our mode 0x%x request",
-                            response.mode - MODE_RESPONSE_OFFSET,
-                            handle->request.mode);
+                    shims->log("Response was for a mode 0x%x request (pid 0x%x), not our mode 0x%x request (pid 0x%x)",
+                            response.mode - MODE_RESPONSE_OFFSET, response.pid,
+                            handle->request.mode, handle->request.pid);
                 }
             } else {
                 shims->log("Received an empty response on arb ID 0x%x",
                         response.arbitration_id);
             }
-            // TODO For now even if we got an empty repsonse or something for
-            // the wrong mode, we're marking this as completed - I'm not sure
-            // those other cases could or will ever happen in practice.
-            // Alternatively, we could re-init handle->isotp_receive_handle if
-            // the current one completed without a valid response to this
-            // diagnostic request.
-            response.completed = true;
-            handle->completed = true;
 
             if(handle->completed && handle->callback != NULL) {
                 handle->callback(&response);
diff --git a/tests/test_core.c b/tests/test_core.c
index b115c1c8..8615ff12 100644
--- a/tests/test_core.c
+++ b/tests/test_core.c
@@ -139,11 +139,23 @@ START_TEST (test_wrong_mode_response)
     const uint8_t can_data[] = {0x4, 0x1 + 0x40, 0x0, 0x2, 0x45};
     diagnostic_receive_can_frame(&SHIMS, &handle, arb_id + 0x8, can_data,
             sizeof(can_data));
-    // TODO change this if we even re-request a message receipt on a mode or PID
-    // mismatch
-    fail_unless(last_response_was_received);
-    fail_unless(handle.completed);
-    fail_if(last_response_received.success);
+    fail_if(last_response_was_received);
+    fail_if(handle.completed);
+}
+END_TEST
+
+START_TEST (test_wrong_pid_response)
+{
+    uint16_t arb_id = OBD2_FUNCTIONAL_BROADCAST_ID;
+    DiagnosticRequestHandle handle = diagnostic_request_pid(&SHIMS,
+            DIAGNOSTIC_ENHANCED_PID, arb_id, 0x2, response_received_handler);
+
+    fail_if(last_response_was_received);
+    const uint8_t can_data[] = {0x4, 0x22 + 0x40, 0x0, 0x3, 0x45};
+    diagnostic_receive_can_frame(&SHIMS, &handle, arb_id + 0x8, can_data,
+            sizeof(can_data));
+    fail_if(last_response_was_received);
+    fail_if(handle.completed);
 }
 END_TEST
 
@@ -216,6 +228,7 @@ Suite* testSuite(void) {
     tcase_add_test(tc_core, test_request_pid_standard);
     tcase_add_test(tc_core, test_request_pid_enhanced);
     tcase_add_test(tc_core, test_wrong_mode_response);
+    tcase_add_test(tc_core, test_wrong_pid_response);
     tcase_add_test(tc_core, test_handle_completed);
     tcase_add_test(tc_core, test_negative_response);