Automatically set pid length for outgoing requests if not specified.

3 files changed, 52 insertions, 7 deletions

diff --git a/src/uds/uds.c b/src/uds/uds.c
index 3295aaaf..6a4ce451 100644
--- a/src/uds/uds.c
+++ b/src/uds/uds.c
@@ -49,6 +49,18 @@ static void setup_receive_handle(DiagnosticRequestHandle* handle) {
     }
 }
 
+static uint16_t autoset_pid_length(uint8_t mode, uint16_t pid,
+        uint8_t pid_length) {
+    if(pid_length == 0) {
+        if(pid > 0xffff || mode > 10) {
+            pid_length = 2;
+        } else {
+            pid_length = 1;
+        }
+    }
+    return pid_length;
+}
+
 DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims,
         DiagnosticRequest* request, DiagnosticResponseReceived callback) {
     DiagnosticRequestHandle handle = {
@@ -60,7 +72,10 @@ DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims,
 
     uint8_t payload[MAX_DIAGNOSTIC_PAYLOAD_SIZE] = {0};
     payload[MODE_BYTE_INDEX] = request->mode;
-    if(request->pid_length > 0) {
+    if(request->has_pid) {
+        request->pid_length = autoset_pid_length(request->mode,
+                request->pid, request->pid_length);
+        handle.request.pid_length = request->pid_length;
         set_bitfield(request->pid, PID_BYTE_INDEX * CHAR_BIT,
                 request->pid_length * CHAR_BIT, payload, sizeof(payload));
     }
@@ -120,8 +135,8 @@ DiagnosticRequestHandle diagnostic_request_pid(DiagnosticShims* shims,
     DiagnosticRequest request = {
         arbitration_id: arbitration_id,
         mode: pid_request_type == DIAGNOSTIC_STANDARD_PID ? 0x1 : 0x22,
-        pid: pid,
-        pid_length: pid_request_type == DIAGNOSTIC_STANDARD_PID ? 1 : 2
+        has_pid: true,
+        pid: pid
     };
 
     return diagnostic_request(shims, &request, callback);
@@ -157,7 +172,7 @@ static bool handle_positive_response(DiagnosticRequestHandle* handle,
         // if it matched
         response->mode = handle->request.mode;
         response->has_pid = false;
-        if(handle->request.pid_length > 0 && message->size > 1) {
+        if(handle->request.has_pid && message->size > 1) {
             response->has_pid = true;
             if(handle->request.pid_length == 2) {
                 response->pid = get_bitfield(message->payload, message->size,
@@ -175,7 +190,7 @@ static bool handle_positive_response(DiagnosticRequestHandle* handle,
                     response->payload_length);
         }
 
-        if((handle->request.pid_length == 0 && !response->has_pid)
+        if((!handle->request.has_pid && !response->has_pid)
                 || response->pid == handle->request.pid) {
             response->success = true;
             response->completed = true;
diff --git a/src/uds/uds_types.h b/src/uds/uds_types.h
index f8c7ef08..88c7ce44 100644
--- a/src/uds/uds_types.h
+++ b/src/uds/uds_types.h
@@ -33,9 +33,12 @@ typedef enum {
  *
  * arbitration_id - The arbitration ID to send the request.
  * mode - The OBD-II mode for the request.
- * pid - (optional) The PID to request, if the mode requires one.
+ * has_pid - (optional) If the requests uses a PID, this should be true.
+ * pid - (optional) The PID to request, if the mode requires one. has_pid must
+ *      be true.
  * pid_length - The length of the PID field, either 1 (standard) or 2 bytes
- *      (extended).
+ *      (extended). If 0, it will be set automatically based on the request
+ *      mode.
  * payload - (optional) The payload for the request, if the request requires
  *      one. If payload_length is 0 this field is ignored.
  * payload_length - The length of the payload, or 0 if no payload is used.
@@ -44,6 +47,7 @@ typedef enum {
 typedef struct {
     uint16_t arbitration_id;
     uint8_t mode;
+    bool has_pid;
     uint16_t pid;
     uint8_t pid_length;
     uint8_t payload[MAX_UDS_PAYLOAD_LENGTH];
diff --git a/tests/test_core.c b/tests/test_core.c
index c37656e8..9f118fc2 100644
--- a/tests/test_core.c
+++ b/tests/test_core.c
@@ -122,6 +122,31 @@ START_TEST (test_send_diag_request)
 }
 END_TEST
 
+START_TEST (test_autoset_pid_length)
+{
+    uint16_t arb_id = OBD2_MODE_POWERTRAIN_DIAGNOSTIC_REQUEST;
+    diagnostic_request_pid(&SHIMS, DIAGNOSTIC_STANDARD_PID, arb_id, 0x2,
+            response_received_handler);
+
+    ck_assert_int_eq(last_can_frame_sent_arb_id, arb_id);
+    ck_assert_int_eq(last_can_payload_sent[1], 0x1);
+    ck_assert_int_eq(last_can_payload_sent[2], 0x2);
+    ck_assert_int_eq(last_can_payload_size, 3);
+
+    DiagnosticRequest request = {
+        arbitration_id: 0x100,
+        mode: 0x22,
+        has_pid: true,
+        pid: 2
+    };
+    diagnostic_request(&SHIMS, &request, response_received_handler);
+
+    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, 4);
+}
+END_TEST
+
 START_TEST (test_request_pid_standard)
 {
     uint16_t arb_id = OBD2_MODE_POWERTRAIN_DIAGNOSTIC_REQUEST;
@@ -282,6 +307,7 @@ Suite* testSuite(void) {
     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_autoset_pid_length);
     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);