Prep request handler to receive again if we got wrong mode or PID.

2 files changed, 34 insertions, 3 deletions

diff --git a/src/obd2/obd2.c b/src/obd2/obd2.c
index 88ce74cf..1c3d7bc3 100644
--- a/src/obd2/obd2.c
+++ b/src/obd2/obd2.c
@@ -29,6 +29,13 @@ DiagnosticShims diagnostic_init_shims(LogShim log,
     return shims;
 }
 
+static void setup_receive_handle(DiagnosticRequestHandle* handle) {
+    handle->isotp_receive_handle = isotp_receive(&handle->isotp_shims,
+            handle->request.arbitration_id + ARBITRATION_ID_OFFSET,
+            NULL);
+}
+
+
 DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims,
         DiagnosticRequest* request, DiagnosticResponseReceived callback) {
     DiagnosticRequestHandle handle = {
@@ -72,9 +79,7 @@ DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims,
                 request->payload_length);
     }
 
-    handle.isotp_receive_handle = isotp_receive(&handle.isotp_shims,
-            request->arbitration_id + ARBITRATION_ID_OFFSET,
-            NULL);
+    setup_receive_handle(&handle);
 
     // TODO notes on multi frame:
     // TODO what are the timers for exactly?
@@ -201,6 +206,7 @@ DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims,
                     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);
+                    setup_receive_handle(handle);
                 }
             } else {
                 shims->log("Received an empty response on arb ID 0x%x",
diff --git a/tests/test_core.c b/tests/test_core.c
index 8615ff12..9f979232 100644
--- a/tests/test_core.c
+++ b/tests/test_core.c
@@ -159,6 +159,30 @@ START_TEST (test_wrong_pid_response)
 }
 END_TEST
 
+START_TEST (test_wrong_pid_then_right_completes)
+{
+    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);
+    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);
+
+    can_data[3] = 0x2;
+    diagnostic_receive_can_frame(&SHIMS, &handle, arb_id + 0x8, can_data,
+            sizeof(can_data));
+    fail_unless(last_response_was_received);
+    fail_unless(handle.completed);
+    fail_unless(handle.success);
+    fail_unless(last_response_received.success);
+    ck_assert_int_eq(last_response_received.pid, 0x2);
+}
+END_TEST
+
 START_TEST (test_handle_completed)
 {
     DiagnosticRequest request = {
@@ -229,6 +253,7 @@ Suite* testSuite(void) {
     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_wrong_pid_then_right_completes);
     tcase_add_test(tc_core, test_handle_completed);
     tcase_add_test(tc_core, test_negative_response);