Accept functional broadcast responses to a range of arb IDs.

4 files changed, 99 insertions, 80 deletions

diff --git a/src/obd2/obd2.c b/src/obd2/obd2.c
index 25b00e7c..513c7d71 100644
--- a/src/obd2/obd2.c
+++ b/src/obd2/obd2.c
@@ -30,9 +30,20 @@ DiagnosticShims diagnostic_init_shims(LogShim log,
 }
 
 static void setup_receive_handle(DiagnosticRequestHandle* handle) {
-    handle->isotp_receive_handle = isotp_receive(&handle->isotp_shims,
-            handle->request.arbitration_id + ARBITRATION_ID_OFFSET,
-            NULL);
+    if(handle->request.arbitration_id == OBD2_FUNCTIONAL_BROADCAST_ID) {
+        for(uint16_t response_id = 0;
+                response_id < OBD2_FUNCTIONAL_RESPONSE_COUNT; ++response_id) {
+            handle->isotp_receive_handles[response_id] = isotp_receive(
+                    &handle->isotp_shims, OBD2_FUNCTIONAL_RESPONSE_START + response_id,
+                    NULL);
+        }
+        handle->isotp_receive_handle_count = OBD2_FUNCTIONAL_RESPONSE_COUNT;
+    } else {
+        handle->isotp_receive_handle_count = 1;
+        handle->isotp_receive_handles[0] = isotp_receive(&handle->isotp_shims,
+                handle->request.arbitration_id + ARBITRATION_ID_OFFSET,
+                NULL);
+    }
 }
 
 
@@ -185,43 +196,46 @@ DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims,
     if(!handle->isotp_send_handle.completed) {
         isotp_continue_send(&handle->isotp_shims,
                 &handle->isotp_send_handle, arbitration_id, data, size);
-    } else if(!handle->isotp_receive_handle.completed) {
-        IsoTpMessage message = isotp_continue_receive(&handle->isotp_shims,
-                &handle->isotp_receive_handle, arbitration_id, data, size);
-
-        if(message.completed) {
-            if(message.size > 0) {
-                response.mode = message.payload[0];
-                if(handle_negative_response(&message, &response, shims)) {
-                    shims->log("Received a negative response to mode %d on arb ID 0x%x",
-                            response.mode, response.arbitration_id);
-                    handle->success = true;
-                    handle->completed = true;
-                } else if(handle_positive_response(handle, &message, &response,
-                            shims)) {
-                    shims->log("Received a positive mode %d response on arb ID 0x%x",
-                            response.mode, response.arbitration_id);
-                    handle->success = true;
-                    handle->completed = true;
+    } else {
+        for(uint8_t i = 0; i < handle->isotp_receive_handle_count; ++i) {
+            IsoTpMessage message = isotp_continue_receive(&handle->isotp_shims,
+                    &handle->isotp_receive_handles[i], arbitration_id, data, size);
+
+            // TODO as of now we're completing the handle as soon as one
+            // broadcast response is received....need to hang on for 100ms
+            if(message.completed) {
+                if(message.size > 0) {
+                    response.mode = message.payload[0];
+                    if(handle_negative_response(&message, &response, shims)) {
+                        shims->log("Received a negative response to mode %d on arb ID 0x%x",
+                                response.mode, response.arbitration_id);
+                        handle->success = true;
+                        handle->completed = true;
+                    } else if(handle_positive_response(handle, &message, &response,
+                                shims)) {
+                        shims->log("Received a positive mode %d response on arb ID 0x%x",
+                                response.mode, response.arbitration_id);
+                        handle->success = true;
+                        handle->completed = true;
+                    } else {
+                        shims->log("Response was for a mode 0x%x request (pid 0x%x), not our mode 0x%x request (pid 0x%x)",
+                                MAX(0, response.mode - MODE_RESPONSE_OFFSET),
+                                response.pid, handle->request.mode,
+                                handle->request.pid);
+                        // TODO just leave handles open until the user decides
+                        // to be done with it - keep a count of valid responses
+                        // received.
+                    }
                 } else {
-                    shims->log("Response was for a mode 0x%x request (pid 0x%x), not our mode 0x%x request (pid 0x%x)",
-                            MAX(0, response.mode - MODE_RESPONSE_OFFSET),
-                            response.pid, handle->request.mode,
-                            handle->request.pid);
-                    setup_receive_handle(handle);
+                    shims->log("Received an empty response on arb ID 0x%x",
+                            response.arbitration_id);
                 }
-            } else {
-                shims->log("Received an empty response on arb ID 0x%x",
-                        response.arbitration_id);
-            }
 
-            if(handle->completed && handle->callback != NULL) {
-                handle->callback(&response);
+                if(handle->completed && handle->callback != NULL) {
+                    handle->callback(&response);
+                }
             }
         }
-    } else {
-        shims->log("Mode %d request to arb ID 0x%x is already completed",
-                handle->request.mode, handle->request.arbitration_id);
     }
     return response;
 }
diff --git a/src/obd2/obd2.h b/src/obd2/obd2.h
index b301138c..bcd6e76b 100644
--- a/src/obd2/obd2.h
+++ b/src/obd2/obd2.h
@@ -6,6 +6,8 @@
 #include <stdbool.h>
 
 #define OBD2_FUNCTIONAL_BROADCAST_ID 0x7df
+#define OBD2_FUNCTIONAL_RESPONSE_START 0x7e8
+#define OBD2_FUNCTIONAL_RESPONSE_COUNT 8
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/src/obd2/obd2_types.h b/src/obd2/obd2_types.h
index 5260fa8e..0699fc4b 100644
--- a/src/obd2/obd2_types.h
+++ b/src/obd2/obd2_types.h
@@ -12,6 +12,7 @@ extern "C" {
 // TODO This isn't true for multi frame messages - we may need to dynamically
 // allocate this in the future
 #define MAX_OBD2_PAYLOAD_LENGTH 7
+#define MAX_RESPONDING_ECU_COUNT 8
 #define VIN_LENGTH 17
 
 /* Private: The four main types of diagnositc requests that determine how the
@@ -149,7 +150,8 @@ typedef struct {
     // Private
     IsoTpShims isotp_shims;
     IsoTpSendHandle isotp_send_handle;
-    IsoTpReceiveHandle isotp_receive_handle;
+    IsoTpReceiveHandle isotp_receive_handles[MAX_RESPONDING_ECU_COUNT];
+    uint8_t isotp_receive_handle_count;
     DiagnosticResponseReceived callback;
     // DiagnosticMilStatusReceived mil_status_callback;
     // DiagnosticVinReceived vin_callback;
diff --git a/tests/test_core.c b/tests/test_core.c
index be4d1412..182f268b 100644
--- a/tests/test_core.c
+++ b/tests/test_core.c
@@ -20,7 +20,7 @@ void response_received_handler(const DiagnosticResponse* response) {
 START_TEST (test_receive_wrong_arb_id)
 {
     DiagnosticRequest request = {
-        arbitration_id: OBD2_FUNCTIONAL_BROADCAST_ID,
+        arbitration_id: 0x100,
         mode: OBD2_MODE_POWERTRAIN_DIAGNOSTIC_REQUEST
     };
     DiagnosticRequestHandle handle = diagnostic_request(&SHIMS, &request,
@@ -37,7 +37,7 @@ END_TEST
 START_TEST (test_send_diag_request_with_payload)
 {
     DiagnosticRequest request = {
-        arbitration_id: OBD2_FUNCTIONAL_BROADCAST_ID,
+        arbitration_id: 0x100,
         mode: OBD2_MODE_POWERTRAIN_DIAGNOSTIC_REQUEST,
         payload: {0x12, 0x34},
         payload_length: 2
@@ -56,7 +56,7 @@ START_TEST (test_send_diag_request_with_payload)
 }
 END_TEST
 
-START_TEST (test_send_diag_request)
+START_TEST (test_send_functional_request)
 {
     DiagnosticRequest request = {
         arbitration_id: OBD2_FUNCTIONAL_BROADCAST_ID,
@@ -72,6 +72,41 @@ START_TEST (test_send_diag_request)
 
     fail_if(last_response_was_received);
     const uint8_t can_data[] = {0x2, request.mode + 0x40, 0x23};
+    for(uint16_t filter = OBD2_FUNCTIONAL_RESPONSE_START; filter <
+            OBD2_FUNCTIONAL_RESPONSE_START + OBD2_FUNCTIONAL_RESPONSE_COUNT;
+            filter++) {
+        DiagnosticResponse response = diagnostic_receive_can_frame(&SHIMS, &handle,
+                filter, can_data, sizeof(can_data));
+        fail_unless(response.success);
+        fail_unless(response.completed);
+        fail_unless(handle.completed);
+        ck_assert(last_response_received.success);
+        ck_assert_int_eq(last_response_received.arbitration_id,
+                filter);
+        ck_assert_int_eq(last_response_received.mode, request.mode);
+        ck_assert_int_eq(last_response_received.pid, 0);
+        ck_assert_int_eq(last_response_received.payload_length, 1);
+        ck_assert_int_eq(last_response_received.payload[0], can_data[2]);
+    }
+}
+END_TEST
+
+START_TEST (test_send_diag_request)
+{
+    DiagnosticRequest request = {
+        arbitration_id: 0x100,
+        mode: OBD2_MODE_EMISSIONS_DTC_REQUEST
+    };
+    DiagnosticRequestHandle handle = diagnostic_request(&SHIMS, &request,
+            response_received_handler);
+
+    fail_if(handle.completed);
+    ck_assert_int_eq(last_can_frame_sent_arb_id, request.arbitration_id);
+    ck_assert_int_eq(last_can_payload_sent[1], request.mode);
+    ck_assert_int_eq(last_can_payload_size, 2);
+
+    fail_if(last_response_was_received);
+    const uint8_t can_data[] = {0x2, request.mode + 0x40, 0x23};
     DiagnosticResponse response = diagnostic_receive_can_frame(&SHIMS, &handle,
             request.arbitration_id + 0x8, can_data, sizeof(can_data));
     fail_unless(response.success);
@@ -110,7 +145,7 @@ END_TEST
 
 START_TEST (test_request_pid_enhanced)
 {
-    uint16_t arb_id = OBD2_FUNCTIONAL_BROADCAST_ID;
+    uint16_t arb_id = 0x100;
     DiagnosticRequestHandle handle = diagnostic_request_pid(&SHIMS,
             DIAGNOSTIC_ENHANCED_PID, arb_id, 0x2, response_received_handler);
 
@@ -131,7 +166,7 @@ END_TEST
 
 START_TEST (test_wrong_mode_response)
 {
-    uint16_t arb_id = OBD2_FUNCTIONAL_BROADCAST_ID;
+    uint16_t arb_id = 0x100;
     DiagnosticRequestHandle handle = diagnostic_request_pid(&SHIMS,
             DIAGNOSTIC_ENHANCED_PID, arb_id, 0x2, response_received_handler);
 
@@ -146,7 +181,7 @@ END_TEST
 
 START_TEST (test_missing_pid)
 {
-    uint16_t arb_id = OBD2_FUNCTIONAL_BROADCAST_ID;
+    uint16_t arb_id = 0x100;
     DiagnosticRequestHandle handle = diagnostic_request_pid(&SHIMS,
             DIAGNOSTIC_ENHANCED_PID, arb_id, 0x2, response_received_handler);
 
@@ -161,7 +196,7 @@ END_TEST
 
 START_TEST (test_wrong_pid_response)
 {
-    uint16_t arb_id = OBD2_FUNCTIONAL_BROADCAST_ID;
+    uint16_t arb_id = 0x100;
     DiagnosticRequestHandle handle = diagnostic_request_pid(&SHIMS,
             DIAGNOSTIC_ENHANCED_PID, arb_id, 0x2, response_received_handler);
 
@@ -176,7 +211,7 @@ END_TEST
 
 START_TEST (test_wrong_pid_then_right_completes)
 {
-    uint16_t arb_id = OBD2_FUNCTIONAL_BROADCAST_ID;
+    uint16_t arb_id = 0x100;
     DiagnosticRequestHandle handle = diagnostic_request_pid(&SHIMS,
             DIAGNOSTIC_ENHANCED_PID, arb_id, 0x2, response_received_handler);
 
@@ -198,44 +233,10 @@ START_TEST (test_wrong_pid_then_right_completes)
 }
 END_TEST
 
-START_TEST (test_handle_completed)
-{
-    DiagnosticRequest request = {
-        arbitration_id: OBD2_FUNCTIONAL_BROADCAST_ID,
-        mode: OBD2_MODE_POWERTRAIN_DIAGNOSTIC_REQUEST
-    };
-    DiagnosticRequestHandle handle = diagnostic_request(&SHIMS, &request,
-            response_received_handler);
-
-    fail_if(handle.completed);
-
-    const uint8_t can_data[] = {0x2, request.mode + 0x40, 0x23};
-    DiagnosticResponse response = diagnostic_receive_can_frame(&SHIMS, &handle,
-            request.arbitration_id + 0x8, can_data, sizeof(can_data));
-    fail_unless(response.success);
-    fail_unless(response.completed);
-    fail_unless(handle.completed);
-
-    response = diagnostic_receive_can_frame(&SHIMS, &handle,
-            request.arbitration_id + 0x8, can_data, sizeof(can_data));
-    fail_if(response.success);
-    fail_if(response.completed);
-    fail_unless(handle.completed);
-
-    ck_assert(last_response_received.success);
-    ck_assert_int_eq(last_response_received.arbitration_id,
-            request.arbitration_id + 0x8);
-    ck_assert_int_eq(last_response_received.mode, request.mode);
-    ck_assert_int_eq(last_response_received.pid, 0);
-    ck_assert_int_eq(last_response_received.payload_length, 1);
-    ck_assert_int_eq(last_response_received.payload[0], can_data[2]);
-}
-END_TEST
-
 START_TEST (test_negative_response)
 {
     DiagnosticRequest request = {
-        arbitration_id: OBD2_FUNCTIONAL_BROADCAST_ID,
+        arbitration_id: 0x100,
         mode: OBD2_MODE_POWERTRAIN_DIAGNOSTIC_REQUEST
     };
     DiagnosticRequestHandle handle = diagnostic_request(&SHIMS, &request,
@@ -262,6 +263,7 @@ Suite* testSuite(void) {
     TCase *tc_core = tcase_create("core");
     tcase_add_checked_fixture(tc_core, setup, NULL);
     tcase_add_test(tc_core, test_send_diag_request);
+    tcase_add_test(tc_core, test_send_functional_request);
     tcase_add_test(tc_core, test_send_diag_request_with_payload);
     tcase_add_test(tc_core, test_receive_wrong_arb_id);
     tcase_add_test(tc_core, test_request_pid_standard);
@@ -270,7 +272,6 @@ Suite* testSuite(void) {
     tcase_add_test(tc_core, test_wrong_pid_response);
     tcase_add_test(tc_core, test_missing_pid);
     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);
 
     // TODO these are future work: