diff options
| -rw-r--r-- | src/uds/uds.c | 25 | ||||
| -rw-r--r-- | src/uds/uds_types.h | 8 | ||||
| -rw-r--r-- | tests/test_core.c | 26 |
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); |