Test basic diag request and response!

4 files changed, 55 insertions, 38 deletions

diff --git a/deps/isotp-c b/deps/isotp-c
-Subproject e3637d97ecaef1768d3f9ef40cb0204a0e668ff
+Subproject 3b25a0491ce9ef9b55c903c6c7f0929bc2910d1
diff --git a/src/obd2/obd2.c b/src/obd2/obd2.c
index d8ef190c..1dcb827e 100644
--- a/src/obd2/obd2.c
+++ b/src/obd2/obd2.c
@@ -1,4 +1,5 @@
 #include <obd2/obd2.h>
+#include <arpa/inet.h>
 
 #define MODE_RESPONSE_OFFSET 0x40
 #define NEGATIVE_RESPONSE_MODE 0x7f
@@ -12,9 +13,9 @@ DiagnosticShims diagnostic_init_shims(LogShim log,
         SendCanMessageShim send_can_message,
         SetTimerShim set_timer) {
     DiagnosticShims shims = {
+        log: log,
         send_can_message: send_can_message,
-        set_timer: set_timer,
-        log: log
+        set_timer: set_timer
     };
     return shims;
 }
@@ -32,11 +33,11 @@ DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims,
     uint8_t payload[MAX_DIAGNOSTIC_PAYLOAD_SIZE];
     payload[MODE_BYTE_INDEX] = request->mode;
     if(request->pid_length > 0) {
-        copy_bytes_right_aligned(request->pid, sizeof(request->pid),
+        copy_bytes_right_aligned(&request->pid, sizeof(request->pid),
                 PID_BYTE_INDEX, request->pid_length, payload, sizeof(payload));
     }
     if(request->payload_length > 0) {
-        memcpy(payload[PID_BYTE_INDEX + request->pid_length],
+        memcpy(&payload[PID_BYTE_INDEX + request->pid_length],
                 request->payload, request->payload_length);
     }
 
@@ -46,15 +47,11 @@ DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims,
     handle.isotp_send_handle = isotp_send(&handle.isotp_shims,
             request->arbitration_id, payload,
             1 + request->payload_length + request->pid_length,
-            // TODO is this ok to pass null here?
             NULL);
 
     handle.isotp_receive_handle = isotp_receive(&handle.isotp_shims,
             // TODO need to either always add 0x8 or let the user specify
             request->arbitration_id + 0x8,
-            // TODO this callback is mostly useful for debugging stuff as it
-            // doesn't have the internal state we need to complete the
-            // diagnositc request - can we pass NULL or will that 'splode?
             NULL);
 
     // when a can frame is received and passes to the diagnostic handle
@@ -117,20 +114,21 @@ DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims,
 }
 
 DiagnosticRequestHandle diagnostic_request_pid(DiagnosticShims* shims,
-        DiagnosticPidRequestType pid_request_type, uint16_t pid,
-        DiagnosticResponseReceived callback) {
+        DiagnosticPidRequestType pid_request_type, uint16_t arbitration_id,
+        uint16_t pid, DiagnosticResponseReceived callback) {
     DiagnosticRequest request = {
+        arbitration_id: arbitration_id,
         mode: pid_request_type == DIAGNOSTIC_STANDARD_PID ? 0x1 : 0x22,
-        pid: pid
+        pid: pid,
+        pid_length: pid_request_type == DIAGNOSTIC_STANDARD_PID ? 1 : 2
     };
 
     return diagnostic_request(shims, &request, callback);
 }
 
 DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims,
-        DiagnosticRequestHandle* handle,
-        const uint16_t arbitration_id, const uint8_t data[],
-        const uint8_t size) {
+        DiagnosticRequestHandle* handle, const uint16_t arbitration_id,
+        const uint8_t data[], const uint8_t size) {
 
     DiagnosticResponse response = {
         arbitration_id: arbitration_id,
@@ -169,12 +167,15 @@ DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims,
                         // if it matched
                         response.mode = handle->request.mode;
                         if(handle->request.pid_length > 0 && message.size > 1) {
-                            copy_bytes_right_aligned(handle->request.pid, sizeof(handle->request.pid),
-                                    PID_BYTE_INDEX, handle->request.pid_length, response.pid,
-                                    sizeof(response.pid));
+                            if(handle->request.pid_length == 2) {
+                                response.pid = *(uint16_t*)&message.payload[PID_BYTE_INDEX];
+                                response.pid = ntohs(response.pid);
+                            } else {
+                                response.pid = message.payload[PID_BYTE_INDEX];
+                            }
                         }
 
-                        uint8_t payload_index =  1 + handle->request.pid_length;
+                        uint8_t payload_index = 1 + handle->request.pid_length;
                         response.payload_length = message.size - payload_index;
                         if(response.payload_length > 0) {
                             memcpy(response.payload, &message.payload[payload_index],
@@ -182,7 +183,7 @@ DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims,
                         }
                         response.success = true;
                     } else {
-                        shims->log("Response was for a mode %d request, not our mode %d request",
+                        shims->log("Response was for a mode 0x%x request, not our mode 0x%x request",
                                 response.mode - MODE_RESPONSE_OFFSET,
                                 handle->request.mode);
                     }
diff --git a/src/obd2/obd2.h b/src/obd2/obd2.h
index 75894835..2bb6c77f 100644
--- a/src/obd2/obd2.h
+++ b/src/obd2/obd2.h
@@ -133,9 +133,9 @@ typedef enum {
 } DiagnosticPidRequestType;
 
 typedef struct {
-    SetTimerShim set_timer;
-    SendCanMessageShim send_can_message;
     LogShim log;
+    SendCanMessageShim send_can_message;
+    SetTimerShim set_timer;
 } DiagnosticShims;
 
 DiagnosticShims diagnostic_init_shims(LogShim log,
@@ -147,8 +147,8 @@ DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims,
 
 // decide mode 0x1 / 0x22 based on pid type
 DiagnosticRequestHandle diagnostic_request_pid(DiagnosticShims* shims,
-        DiagnosticPidRequestType pid_request_type, uint16_t pid,
-        DiagnosticResponseReceived callback);
+        DiagnosticPidRequestType pid_request_type, uint16_t arbitration_id,
+        uint16_t pid, DiagnosticResponseReceived callback);
 
 DiagnosticRequestHandle diagnostic_request_malfunction_indicator_status(
         DiagnosticShims* shims,
diff --git a/tests/test_core.c b/tests/test_core.c
index 2730cd73..4b247340 100644
--- a/tests/test_core.c
+++ b/tests/test_core.c
@@ -62,18 +62,19 @@ END_TEST
 
 START_TEST (test_request_pid_standard)
 {
+    // TODO need a constant for the 7df broadcast functional request
+    uint16_t arb_id = 0x7df;
     DiagnosticRequestHandle handle = diagnostic_request_pid(&SHIMS,
-            DIAGNOSTIC_STANDARD_PID, 0x2, response_received_handler);
+            DIAGNOSTIC_STANDARD_PID, arb_id, 0x2, response_received_handler);
 
     fail_if(last_response_was_received);
     const uint8_t can_data[] = {0x3, 0x1 + 0x40, 0x2, 0x45};
-    // TODO need a constant for the 7df broadcast functional request
-    diagnostic_receive_can_frame(&SHIMS, &handle, 0x7df + 0x8,
+    diagnostic_receive_can_frame(&SHIMS, &handle, arb_id + 0x8,
             can_data, sizeof(can_data));
     fail_unless(last_response_was_received);
     ck_assert(last_response_received.success);
     ck_assert_int_eq(last_response_received.arbitration_id,
-            0x7df + 0x8);
+            arb_id + 0x8);
     ck_assert_int_eq(last_response_received.mode, 0x1);
     ck_assert_int_eq(last_response_received.pid, 0x2);
     ck_assert_int_eq(last_response_received.payload_length, 1);
@@ -83,19 +84,18 @@ END_TEST
 
 START_TEST (test_request_pid_enhanced)
 {
+    uint16_t arb_id = 0x7df;
     DiagnosticRequestHandle handle = diagnostic_request_pid(&SHIMS,
-            DIAGNOSTIC_ENHANCED_PID, 0x2, response_received_handler);
+            DIAGNOSTIC_ENHANCED_PID, arb_id, 0x2, response_received_handler);
 
     fail_if(last_response_was_received);
-    const uint8_t can_data[] = {0x4, 0x1 + 0x40, 0x0, 0x2, 0x45};
-    // TODO need a constant for the 7df broadcast functional request
-    diagnostic_receive_can_frame(&SHIMS, &handle, 0x7df + 0x8, can_data,
+    const uint8_t can_data[] = {0x4, 0x22 + 0x40, 0x0, 0x2, 0x45};
+    diagnostic_receive_can_frame(&SHIMS, &handle, arb_id + 0x8, can_data,
             sizeof(can_data));
     fail_unless(last_response_was_received);
     ck_assert(last_response_received.success);
     ck_assert_int_eq(last_response_received.arbitration_id,
-            0x7df + 0x8);
-    // TODO should we set it back to the original mode, or leave as mode + 0x40?
+            arb_id + 0x8);
     ck_assert_int_eq(last_response_received.mode, 0x22);
     ck_assert_int_eq(last_response_received.pid, 0x2);
     ck_assert_int_eq(last_response_received.payload_length, 1);
@@ -103,6 +103,21 @@ START_TEST (test_request_pid_enhanced)
 }
 END_TEST
 
+START_TEST (test_wrong_mode_response)
+{
+    uint16_t arb_id = 0x7df;
+    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, 0x1 + 0x40, 0x0, 0x2, 0x45};
+    diagnostic_receive_can_frame(&SHIMS, &handle, arb_id + 0x8, can_data,
+            sizeof(can_data));
+    fail_unless(last_response_was_received);
+    fail_if(last_response_received.success);
+}
+END_TEST
+
 Suite* testSuite(void) {
     Suite* s = suite_create("obd2");
     TCase *tc_core = tcase_create("core");
@@ -111,6 +126,7 @@ Suite* testSuite(void) {
     tcase_add_test(tc_core, test_receive_wrong_arb_id);
     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);
 
     // TODO these are future work:
     // TODO test request MIL