Draft implemenation of receiving and parsing single fram diag messages.

6 files changed, 99 insertions, 43 deletions

diff --git a/README.mkd b/README.mkd
index 28aee721..bd074e99 100644
--- a/README.mkd
+++ b/README.mkd
@@ -1,6 +1,8 @@
 OBD-II Support Library in C
 =============================
 
+TODO diagram out a request, response and error response
+
 TODO isotp needs to accept responses on an ID other that the request's arb id -
 or maybe we have 2 isotp instances, for rx and tx.
 
diff --git a/deps/isotp-c b/deps/isotp-c
-Subproject fe20a273bb3979d9e806d828486633249d073ed
+Subproject e3637d97ecaef1768d3f9ef40cb0204a0e668ff
diff --git a/src/obd2/obd2.c b/src/obd2/obd2.c
index 7188af0d..d8ef190c 100644
--- a/src/obd2/obd2.c
+++ b/src/obd2/obd2.c
@@ -1,8 +1,12 @@
 #include <obd2/obd2.h>
 
+#define MODE_RESPONSE_OFFSET 0x40
+#define NEGATIVE_RESPONSE_MODE 0x7f
 #define MAX_DIAGNOSTIC_PAYLOAD_SIZE 6
 #define MODE_BYTE_INDEX 0
 #define PID_BYTE_INDEX 1
+#define NEGATIVE_RESPONSE_MODE_INDEX 1
+#define NEGATIVE_RESPONSE_NRC_INDEX 2
 
 DiagnosticShims diagnostic_init_shims(LogShim log,
         SendCanMessageShim send_can_message,
@@ -18,11 +22,13 @@ DiagnosticShims diagnostic_init_shims(LogShim log,
 DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims,
         DiagnosticRequest* request, DiagnosticResponseReceived callback) {
     DiagnosticRequestHandle handle = {
-        type: DIAGNOSTIC_REQUEST_TYPE_PID,
+        // TODO can we copy the request?
+        request: *request,
         callback: callback,
         success: false,
         completed: false
     };
+
     uint8_t payload[MAX_DIAGNOSTIC_PAYLOAD_SIZE];
     payload[MODE_BYTE_INDEX] = request->mode;
     if(request->pid_length > 0) {
@@ -125,22 +131,80 @@ DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims,
         DiagnosticRequestHandle* handle,
         const uint16_t arbitration_id, const uint8_t data[],
         const uint8_t size) {
-    if(!handle->isotp_send_handle.completed) {
-    } else if(!handle->isotp_receive_handle.completed) {
-    } else {
-        shims->log("Handle is already completed");
-    }
-    // TODO determine which isotp handler to pass it to based on our state
-    IsoTpMessage message = isotp_receive_can_frame(&handle->isotp_shims,
-            &handle->isotp_send_handle, arbitration_id, data, size);
 
     DiagnosticResponse response = {
+        arbitration_id: arbitration_id,
         success: false,
         completed: false
     };
 
-    if(message.completed) {
+    if(!handle->isotp_send_handle.completed) {
+        // TODO when completing a send, this returns...a Message? we have to
+        // check when the isotp_send_handle is completed, and if it is, start
+        isotp_receive_can_frame(&handle->isotp_shims,
+                &handle->isotp_send_handle, arbitration_id, data, size);
+    } else if(!handle->isotp_receive_handle.completed) {
+        IsoTpMessage message = isotp_receive_can_frame(&handle->isotp_shims,
+                &handle->isotp_receive_handle, arbitration_id, data, size);
+
+        if(message.completed) {
+            if(message.size > 0) {
+                response.mode = message.payload[0];
+                if(response.mode == NEGATIVE_RESPONSE_MODE) {
+                    if(message.size > NEGATIVE_RESPONSE_MODE_INDEX) {
+                        // TODO we're setting the mode to the originating
+                        // request for the error, so the user can confirm - i
+                        // think this is OK since we're storing the failure
+                        // status elsewhere, but think about it.
+                        response.mode = message.payload[NEGATIVE_RESPONSE_MODE_INDEX];
+                    }
+
+                    if(message.size > NEGATIVE_RESPONSE_NRC_INDEX) {
+                        response.negative_response_code = message.payload[NEGATIVE_RESPONSE_NRC_INDEX];
+                    }
+                    response.success = false;
+                } else {
+                    if(response.mode == handle->request.mode + MODE_RESPONSE_OFFSET) {
+                        // hide the "response" version of the mode from the user
+                        // 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));
+                        }
+
+                        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],
+                                    response.payload_length);
+                        }
+                        response.success = true;
+                    } else {
+                        shims->log("Response was for a mode %d request, not our mode %d request",
+                                response.mode - MODE_RESPONSE_OFFSET,
+                                handle->request.mode);
+                    }
+                }
+            }
+
+            response.completed = true;
+            // TODO what does it mean for the handle to be successful, vs. the
+            // request to be successful? if we get a NRC, is that a successful
+            // request?
+            handle->success = true;
+            handle->completed = true;
+
+            if(handle->callback != NULL) {
+                handle->callback(&response);
+            }
+        }
+
+    } else {
+        shims->log("Handle is already completed");
     }
+    return response;
 }
 
 // TODO everything below here is for future work...not critical for now.
diff --git a/src/obd2/obd2.h b/src/obd2/obd2.h
index 727b816f..75894835 100644
--- a/src/obd2/obd2.h
+++ b/src/obd2/obd2.h
@@ -9,10 +9,20 @@
 extern "C" {
 #endif
 
+// 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 VIN_LENGTH 17
 
+typedef enum {
+    DIAGNOSTIC_REQUEST_TYPE_PID,
+    DIAGNOSTIC_REQUEST_TYPE_DTC,
+    DIAGNOSTIC_REQUEST_TYPE_MIL_STATUS,
+    DIAGNOSTIC_REQUEST_TYPE_VIN
+} DiagnosticRequestType;
+
 typedef struct {
+    DiagnosticRequestType type;
     uint16_t arbitration_id;
     uint8_t mode;
     uint16_t pid;
@@ -69,13 +79,8 @@ typedef struct {
     uint8_t mode;
     bool success;
     bool completed;
-    // if mode is one with a PID, read the correct numbers of PID bytes (1 or 2)
-    // into this field, then store the remainder of the payload in the payload
-    // field
     uint16_t pid;
     DiagnosticNegativeResponseCode negative_response_code;
-    // if response mode is a negative response, read first byte of payload into
-    // NRC and store remainder of payload in payload field
     uint8_t payload[MAX_OBD2_PAYLOAD_LENGTH];
     uint8_t payload_length;
 } DiagnosticResponse;
@@ -109,25 +114,14 @@ typedef struct {
     float max_value;
 } DiagnosticParameter;
 
-typedef enum {
-    DIAGNOSTIC_REQUEST_TYPE_PID,
-    DIAGNOSTIC_REQUEST_TYPE_DTC,
-    DIAGNOSTIC_REQUEST_TYPE_MIL_STATUS,
-    DIAGNOSTIC_REQUEST_TYPE_VIN
-} DiagnosticRequestType;
-
 typedef struct {
+    DiagnosticRequest request;
     bool success;
     bool completed;
+
     IsoTpShims isotp_shims;
     IsoTpHandle isotp_send_handle;
     IsoTpHandle isotp_receive_handle;
-    // TODO the Handle may need to keep the original request, otherwise we can't
-    // compare an incoming CAN message to see if it matches the service / PID!
-    // TODO i'm not sure this type/callback in here is too useful - see the
-    // comments in obd2.c:diagnostic_request
-
-    DiagnosticRequestType type;
     DiagnosticResponseReceived callback;
     DiagnosticMilStatusReceived mil_status_callback;
     DiagnosticVinReceived vin_callback;
diff --git a/tests/common.c b/tests/common.c
index e35ef529..99b2f7c4 100644
--- a/tests/common.c
+++ b/tests/common.c
@@ -35,12 +35,6 @@ void mock_send_can(const uint16_t arbitration_id, const uint8_t* data,
 void mock_set_timer(uint16_t time_ms, void (*callback)) {
 }
 
-void response_received_handler(const DiagnosticResponse* response) {
-    last_response_was_received = true;
-    // TODO not sure if we can copy the struct like this
-    last_response_received = *response;
-}
-
 void setup() {
     SHIMS = diagnostic_init_shims(debug, mock_send_can, mock_set_timer);
     memset(last_can_payload_sent, 0, sizeof(last_can_payload_sent));
diff --git a/tests/test_core.c b/tests/test_core.c
index a466e8e0..2730cd73 100644
--- a/tests/test_core.c
+++ b/tests/test_core.c
@@ -8,7 +8,12 @@ extern void setup();
 extern bool last_response_was_received;
 extern DiagnosticResponse last_response_received;
 extern DiagnosticShims SHIMS;
-extern DiagnosticResponseReceived response_received_handler;
+
+void response_received_handler(const DiagnosticResponse* response) {
+    last_response_was_received = true;
+    // TODO not sure if we can copy the struct like this
+    last_response_received = *response;
+}
 
 START_TEST (test_receive_wrong_arb_id)
 {
@@ -45,11 +50,9 @@ START_TEST (test_send_diag_request)
     fail_unless(response.success);
     fail_unless(response.completed);
     fail_unless(handle.completed);
-    fail_unless(last_response_was_received);
     ck_assert(last_response_received.success);
     ck_assert_int_eq(last_response_received.arbitration_id,
             request.arbitration_id + 0x8);
-    // TODO should we set it back to the original mode, or leave as mode + 0x40?
     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);
@@ -71,7 +74,6 @@ START_TEST (test_request_pid_standard)
     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?
     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);