Skip to content

Commit 4b4100b

Browse files
driftregiondriftregion
authored
Merge pull request #88 from frickly-systems/tim/feat/io-control
add 0x2F InputOutputControlByIdentifier
2 parents 902c172 + 604720a commit 4b4100b

File tree

6 files changed

+231
-8
lines changed

6 files changed

+231
-8
lines changed

‎iso14229.c‎

Lines changed: 31 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1296,6 +1296,34 @@ static UDSErr_t Handle_0x2E_WriteDataByIdentifier(UDSServer_t *srv, UDSReq_t *r)
12961296
return UDS_PositiveResponse;
12971297
}
12981298

1299+
static UDSErr_t Handle_0x2F_IOControlByIdentifier(UDSServer_t *srv, UDSReq_t *r) {
1300+
if (r->recv_len < UDS_0X2F_REQ_MIN_LEN) {
1301+
return NegativeResponse(r, UDS_NRC_IncorrectMessageLengthOrInvalidFormat);
1302+
}
1303+
1304+
r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_IO_CONTROL_BY_IDENTIFIER);
1305+
r->send_buf[1] = r->recv_buf[1];
1306+
r->send_buf[2] = r->recv_buf[2];
1307+
r->send_buf[3] = r->recv_buf[3];
1308+
r->send_len = UDS_0X2F_RESP_BASE_LEN;
1309+
1310+
UDSIOCtrlArgs_t args = {
1311+
.dataId = (uint16_t)(r->recv_buf[1] << 8) | (uint16_t)r->recv_buf[2],
1312+
.ioCtrlParam = r->recv_buf[3],
1313+
.ctrlStateAndMask = &r->recv_buf[UDS_0X2F_REQ_MIN_LEN],
1314+
.ctrlStateAndMaskLen = r->recv_len - UDS_0X2F_REQ_MIN_LEN,
1315+
.copy = safe_copy,
1316+
};
1317+
1318+
UDSErr_t err = EmitEvent(srv, UDS_EVT_IOControl, &args);
1319+
1320+
if (err != UDS_PositiveResponse) {
1321+
return NegativeResponse(r, err);
1322+
}
1323+
1324+
return UDS_PositiveResponse;
1325+
}
1326+
12991327
static UDSErr_t Handle_0x31_RoutineControl(UDSServer_t *srv, UDSReq_t *r) {
13001328
UDSErr_t err = UDS_PositiveResponse;
13011329
if (r->recv_len < UDS_0X31_REQ_MIN_LEN) {
@@ -1766,8 +1794,8 @@ static UDSService getServiceForSID(uint8_t sid) {
17661794
return NULL;
17671795
case kSID_WRITE_DATA_BY_IDENTIFIER:
17681796
return Handle_0x2E_WriteDataByIdentifier;
1769-
case kSID_INPUT_CONTROL_BY_IDENTIFIER:
1770-
return NULL;
1797+
case kSID_IO_CONTROL_BY_IDENTIFIER:
1798+
return Handle_0x2F_IOControlByIdentifier;
17711799
case kSID_ROUTINE_CONTROL:
17721800
return Handle_0x31_RoutineControl;
17731801
case kSID_REQUEST_DOWNLOAD:
@@ -1863,7 +1891,7 @@ static UDSErr_t evaluateServiceResponse(UDSServer_t *srv, UDSReq_t *r) {
18631891
case kSID_READ_SCALING_DATA_BY_IDENTIFIER:
18641892
case kSID_READ_PERIODIC_DATA_BY_IDENTIFIER:
18651893
case kSID_DYNAMICALLY_DEFINE_DATA_IDENTIFIER:
1866-
case kSID_INPUT_CONTROL_BY_IDENTIFIER:
1894+
case kSID_IO_CONTROL_BY_IDENTIFIER:
18671895
case kSID_WRITE_MEMORY_BY_ADDRESS:
18681896
case kSID_ACCESS_TIMING_PARAMETER:
18691897
case kSID_SECURED_DATA_TRANSMISSION:

‎iso14229.h‎

Lines changed: 13 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -309,6 +309,7 @@ typedef enum UDSEvent {
309309
UDS_EVT_SecAccessValidateKey, // UDSSecAccessValidateKeyArgs_t *
310310
UDS_EVT_WriteDataByIdent, // UDSWDBIArgs_t *
311311
UDS_EVT_WriteMemByAddr, // UDSWriteMemByAddrArgs_t *
312+
UDS_EVT_IOControl, // UDSIOCtrlArgs_t*
312313
UDS_EVT_RoutineCtrl, // UDSRoutineCtrlArgs_t*
313314
UDS_EVT_RequestDownload, // UDSRequestDownloadArgs_t*
314315
UDS_EVT_RequestUpload, // UDSRequestUploadArgs_t *
@@ -507,6 +508,8 @@ typedef enum {
507508
#define UDS_0X2E_REQ_BASE_LEN 3U
508509
#define UDS_0X2E_REQ_MIN_LEN 4U
509510
#define UDS_0X2E_RESP_LEN 3U
511+
#define UDS_0X2F_REQ_MIN_LEN 4U
512+
#define UDS_0X2F_RESP_BASE_LEN 4U
510513
#define UDS_0X31_REQ_MIN_LEN 4U
511514
#define UDS_0X31_RESP_MIN_LEN 4U
512515
#define UDS_0X34_REQ_BASE_LEN 3U
@@ -540,7 +543,7 @@ enum UDSDiagnosticServiceId {
540543
kSID_READ_PERIODIC_DATA_BY_IDENTIFIER = 0x2A,
541544
kSID_DYNAMICALLY_DEFINE_DATA_IDENTIFIER = 0x2C,
542545
kSID_WRITE_DATA_BY_IDENTIFIER = 0x2E,
543-
kSID_INPUT_CONTROL_BY_IDENTIFIER = 0x2F,
546+
kSID_IO_CONTROL_BY_IDENTIFIER = 0x2F,
544547
kSID_ROUTINE_CONTROL = 0x31,
545548
kSID_REQUEST_DOWNLOAD = 0x34,
546549
kSID_REQUEST_UPLOAD = 0x35,
@@ -935,6 +938,15 @@ typedef struct {
935938
const uint8_t *const data; /*! pointer to data */
936939
} UDSWriteMemByAddrArgs_t;
937940

941+
typedef struct {
942+
const uint16_t dataId; /*! Data Identifier */
943+
const uint8_t ioCtrlParam; /*! inputOutputControlParameter */
944+
const void *const ctrlStateAndMask; /*! controlState bytes and controlMask (optional) */
945+
const size_t ctrlStateAndMaskLen; /*! number of bytes in `ctrlStateAndMask` */
946+
uint8_t (*copy)(UDSServer_t *srv, const void *src,
947+
uint16_t count); /*! function for copying data */
948+
} UDSIOCtrlArgs_t;
949+
938950
typedef struct {
939951
const uint8_t ctrlType; /*! routineControlType */
940952
const uint16_t id; /*! routineIdentifier */

‎src/server.c‎

Lines changed: 31 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -432,6 +432,34 @@ static UDSErr_t Handle_0x2E_WriteDataByIdentifier(UDSServer_t *srv, UDSReq_t *r)
432432
return UDS_PositiveResponse;
433433
}
434434

435+
static UDSErr_t Handle_0x2F_IOControlByIdentifier(UDSServer_t *srv, UDSReq_t *r) {
436+
if (r->recv_len < UDS_0X2F_REQ_MIN_LEN) {
437+
return NegativeResponse(r, UDS_NRC_IncorrectMessageLengthOrInvalidFormat);
438+
}
439+
440+
r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_IO_CONTROL_BY_IDENTIFIER);
441+
r->send_buf[1] = r->recv_buf[1];
442+
r->send_buf[2] = r->recv_buf[2];
443+
r->send_buf[3] = r->recv_buf[3];
444+
r->send_len = UDS_0X2F_RESP_BASE_LEN;
445+
446+
UDSIOCtrlArgs_t args = {
447+
.dataId = (uint16_t)(r->recv_buf[1] << 8) | (uint16_t)r->recv_buf[2],
448+
.ioCtrlParam = r->recv_buf[3],
449+
.ctrlStateAndMask = &r->recv_buf[UDS_0X2F_REQ_MIN_LEN],
450+
.ctrlStateAndMaskLen = r->recv_len - UDS_0X2F_REQ_MIN_LEN,
451+
.copy = safe_copy,
452+
};
453+
454+
UDSErr_t err = EmitEvent(srv, UDS_EVT_IOControl, &args);
455+
456+
if (err != UDS_PositiveResponse) {
457+
return NegativeResponse(r, err);
458+
}
459+
460+
return UDS_PositiveResponse;
461+
}
462+
435463
static UDSErr_t Handle_0x31_RoutineControl(UDSServer_t *srv, UDSReq_t *r) {
436464
UDSErr_t err = UDS_PositiveResponse;
437465
if (r->recv_len < UDS_0X31_REQ_MIN_LEN) {
@@ -902,8 +930,8 @@ static UDSService getServiceForSID(uint8_t sid) {
902930
return NULL;
903931
case kSID_WRITE_DATA_BY_IDENTIFIER:
904932
return Handle_0x2E_WriteDataByIdentifier;
905-
case kSID_INPUT_CONTROL_BY_IDENTIFIER:
906-
return NULL;
933+
case kSID_IO_CONTROL_BY_IDENTIFIER:
934+
return Handle_0x2F_IOControlByIdentifier;
907935
case kSID_ROUTINE_CONTROL:
908936
return Handle_0x31_RoutineControl;
909937
case kSID_REQUEST_DOWNLOAD:
@@ -999,7 +1027,7 @@ static UDSErr_t evaluateServiceResponse(UDSServer_t *srv, UDSReq_t *r) {
9991027
case kSID_READ_SCALING_DATA_BY_IDENTIFIER:
10001028
case kSID_READ_PERIODIC_DATA_BY_IDENTIFIER:
10011029
case kSID_DYNAMICALLY_DEFINE_DATA_IDENTIFIER:
1002-
case kSID_INPUT_CONTROL_BY_IDENTIFIER:
1030+
case kSID_IO_CONTROL_BY_IDENTIFIER:
10031031
case kSID_WRITE_MEMORY_BY_ADDRESS:
10041032
case kSID_ACCESS_TIMING_PARAMETER:
10051033
case kSID_SECURED_DATA_TRANSMISSION:

‎src/server.h‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -135,6 +135,15 @@ typedef struct {
135135
const uint8_t *const data; /*! pointer to data */
136136
} UDSWriteMemByAddrArgs_t;
137137

138+
typedef struct {
139+
const uint16_t dataId; /*! Data Identifier */
140+
const uint8_t ioCtrlParam; /*! inputOutputControlParameter */
141+
const void *const ctrlStateAndMask; /*! controlState bytes and controlMask (optional) */
142+
const size_t ctrlStateAndMaskLen; /*! number of bytes in `ctrlStateAndMask` */
143+
uint8_t (*copy)(UDSServer_t *srv, const void *src,
144+
uint16_t count); /*! function for copying data */
145+
} UDSIOCtrlArgs_t;
146+
138147
typedef struct {
139148
const uint8_t ctrlType; /*! routineControlType */
140149
const uint16_t id; /*! routineIdentifier */

‎src/uds.h‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@ typedef enum UDSEvent {
1515
UDS_EVT_SecAccessValidateKey, // UDSSecAccessValidateKeyArgs_t *
1616
UDS_EVT_WriteDataByIdent, // UDSWDBIArgs_t *
1717
UDS_EVT_WriteMemByAddr, // UDSWriteMemByAddrArgs_t *
18+
UDS_EVT_IOControl, // UDSIOCtrlArgs_t*
1819
UDS_EVT_RoutineCtrl, // UDSRoutineCtrlArgs_t*
1920
UDS_EVT_RequestDownload, // UDSRequestDownloadArgs_t*
2021
UDS_EVT_RequestUpload, // UDSRequestUploadArgs_t *
@@ -213,6 +214,8 @@ typedef enum {
213214
#define UDS_0X2E_REQ_BASE_LEN 3U
214215
#define UDS_0X2E_REQ_MIN_LEN 4U
215216
#define UDS_0X2E_RESP_LEN 3U
217+
#define UDS_0X2F_REQ_MIN_LEN 4U
218+
#define UDS_0X2F_RESP_BASE_LEN 4U
216219
#define UDS_0X31_REQ_MIN_LEN 4U
217220
#define UDS_0X31_RESP_MIN_LEN 4U
218221
#define UDS_0X34_REQ_BASE_LEN 3U
@@ -246,7 +249,7 @@ enum UDSDiagnosticServiceId {
246249
kSID_READ_PERIODIC_DATA_BY_IDENTIFIER = 0x2A,
247250
kSID_DYNAMICALLY_DEFINE_DATA_IDENTIFIER = 0x2C,
248251
kSID_WRITE_DATA_BY_IDENTIFIER = 0x2E,
249-
kSID_INPUT_CONTROL_BY_IDENTIFIER = 0x2F,
252+
kSID_IO_CONTROL_BY_IDENTIFIER = 0x2F,
250253
kSID_ROUTINE_CONTROL = 0x31,
251254
kSID_REQUEST_DOWNLOAD = 0x34,
252255
kSID_REQUEST_UPLOAD = 0x35,

‎test/test_server.c‎

Lines changed: 143 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -697,6 +697,146 @@ void test_0x27_brute_force_prevention_2(void **state) {
697697
TEST_MEMORY_EQUAL(buf, DENIED, sizeof(DENIED));
698698
}
699699

700+
int fn_test_0x2F(UDSServer_t *srv, UDSEvent_t ev, void *arg) {
701+
UDSIOCtrlArgs_t *args = arg;
702+
703+
TEST_INT_EQUAL(ev, UDS_EVT_IOControl);
704+
TEST_INT_EQUAL(args->dataId, 0x9B00);
705+
if (args->ioCtrlParam == 0x00) {
706+
TEST_INT_EQUAL(args->ctrlStateAndMaskLen, 0x00);
707+
const uint8_t response_data[] = {0x3A};
708+
return args->copy(srv, response_data, sizeof(response_data));
709+
710+
} else if (args->ioCtrlParam == 0x02) {
711+
// Provoke a negative response
712+
return UDS_NRC_SecurityAccessDenied;
713+
714+
} else if (args->ioCtrlParam == 0x03) {
715+
TEST_INT_EQUAL(args->ctrlStateAndMaskLen, 0x01);
716+
const uint8_t expected_data[] = {0x3C};
717+
TEST_MEMORY_EQUAL(args->ctrlStateAndMask, expected_data, args->ctrlStateAndMaskLen);
718+
const uint8_t response_data[] = {0x0C};
719+
return args->copy(srv, response_data, sizeof(response_data));
720+
}
721+
722+
return UDS_NRC_RequestOutOfRange;
723+
}
724+
725+
// ISO14229-1 2020 13.2.5.2 Example #1 - "Air Inlet Door Position" shortTermAdjustment
726+
// This test just simulates the 0x2F function request/responses of the example.
727+
void test_0x2F_example(void **state) {
728+
Env_t *e = *state;
729+
uint8_t buf[20] = {0};
730+
731+
e->server->fn = fn_test_0x2F;
732+
e->server->fn_data = NULL;
733+
734+
/* Request per ISO14229-1 2020 Table 406 */
735+
const uint8_t REQ1[] = {
736+
0x2F, /* SID */
737+
0x9B, /* DataIdentifier [High Byte] */
738+
0x00, /* DataIdentifier [Low Byte] */
739+
0x03, /* ControlOptionRecord [inputOutputControlParamter] */
740+
0x3C, /* ControlOptionRecord [State#1] */
741+
};
742+
743+
UDSTpSend(e->client_tp, REQ1, sizeof(REQ1), NULL);
744+
745+
/* Response per ISO14229-1 2020 Table 407 */
746+
const uint8_t EXPECTED_RESP1[] = {
747+
0x6F, /* Response SID */
748+
0x9B, /* DataIdentifier [High Byte] */
749+
0x00, /* DataIdentifier [Low Byte] */
750+
0x03, /* ControlOptionRecord [inputOutputControlParamter] */
751+
0x0C, /* ControlOptionRecord [State#1] */
752+
};
753+
754+
/* the client transport should receive a positive response within client_p2 ms */
755+
EXPECT_WITHIN_MS(e, UDSTpRecv(e->client_tp, buf, sizeof(buf), NULL) > 0,
756+
UDS_CLIENT_DEFAULT_P2_MS);
757+
TEST_MEMORY_EQUAL(buf, EXPECTED_RESP1, sizeof(EXPECTED_RESP1));
758+
759+
/* Request per ISO14229-1 2020 Table 410 */
760+
const uint8_t REQ2[] = {
761+
0x2F, /* SID */
762+
0x9B, /* DataIdentifier [High Byte] */
763+
0x00, /* DataIdentifier [Low Byte] */
764+
0x00, /* ControlOptionRecord [inputOutputControlParamter] */
765+
};
766+
767+
UDSTpSend(e->client_tp, REQ2, sizeof(REQ2), NULL);
768+
769+
/* Response per ISO14229-1 2020 Table 411 */
770+
const uint8_t EXPECTED_RESP2[] = {
771+
0x6F, /* Response SID */
772+
0x9B, /* DataIdentifier [High Byte] */
773+
0x00, /* DataIdentifier [Low Byte] */
774+
0x00, /* ControlOptionRecord [inputOutputControlParamter] */
775+
0x3A, /* ControlOptionRecord [State#1] */
776+
};
777+
778+
/* the client transport should receive a positive response within client_p2 ms */
779+
EXPECT_WITHIN_MS(e, UDSTpRecv(e->client_tp, buf, sizeof(buf), NULL) > 0,
780+
UDS_CLIENT_DEFAULT_P2_MS);
781+
TEST_MEMORY_EQUAL(buf, EXPECTED_RESP2, sizeof(EXPECTED_RESP2));
782+
}
783+
784+
void test_0x2F_incorrect_request_length(void **state) {
785+
Env_t *e = *state;
786+
uint8_t buf[20] = {0};
787+
788+
e->server->fn = fn_test_0x2F;
789+
e->server->fn_data = NULL;
790+
791+
const uint8_t REQ[] = {
792+
0x2F, /* SID */
793+
0x9B, /* DataIdentifier [High Byte] */
794+
0x00, /* DataIdentifier [Low Byte] */
795+
/* MISSING required ControlOptionRecord [inputOutputControlParamter] */
796+
};
797+
798+
UDSTpSend(e->client_tp, REQ, sizeof(REQ), NULL);
799+
800+
const uint8_t EXPECTED_RESP[] = {
801+
0x7F, /* Response SID */
802+
0x2F, /* Original Request SID */
803+
0x13, /* NRC: IncorrectMessageLengthOrInvalidFormat */
804+
};
805+
806+
/* the client transport should receive a response within client_p2 ms */
807+
EXPECT_WITHIN_MS(e, UDSTpRecv(e->client_tp, buf, sizeof(buf), NULL) > 0,
808+
UDS_CLIENT_DEFAULT_P2_MS);
809+
TEST_MEMORY_EQUAL(buf, EXPECTED_RESP, sizeof(EXPECTED_RESP));
810+
}
811+
812+
void test_0x2F_negative_response(void **state) {
813+
Env_t *e = *state;
814+
uint8_t buf[20] = {0};
815+
816+
e->server->fn = fn_test_0x2F;
817+
e->server->fn_data = NULL;
818+
819+
const uint8_t REQ[] = {
820+
0x2F, /* SID */
821+
0x9B, /* DataIdentifier [High Byte] */
822+
0x00, /* DataIdentifier [Low Byte] */
823+
0x02, /* ControlOptionRecord [inputOutputControlParamter] */
824+
};
825+
826+
UDSTpSend(e->client_tp, REQ, sizeof(REQ), NULL);
827+
828+
const uint8_t EXPECTED_RESP[] = {
829+
0x7F, /* Response SID */
830+
0x2F, /* Original Request SID */
831+
0x33, /* NRC: SecurityAccessDenied */
832+
};
833+
834+
/* the client transport should receive a response within client_p2 ms */
835+
EXPECT_WITHIN_MS(e, UDSTpRecv(e->client_tp, buf, sizeof(buf), NULL) > 0,
836+
UDS_CLIENT_DEFAULT_P2_MS);
837+
TEST_MEMORY_EQUAL(buf, EXPECTED_RESP, sizeof(EXPECTED_RESP));
838+
}
839+
700840
int fn_test_0x31_RCRRP(UDSServer_t *srv, UDSEvent_t ev, void *arg) {
701841
return *(int *)(srv->fn_data);
702842
}
@@ -980,6 +1120,9 @@ int main(int ac, char **av) {
9801120
cmocka_unit_test_setup_teardown(test_0x27_unlock, Setup, Teardown),
9811121
cmocka_unit_test_setup_teardown(test_0x27_brute_force_prevention_1, Setup, Teardown),
9821122
cmocka_unit_test_setup_teardown(test_0x27_brute_force_prevention_2, Setup, Teardown),
1123+
cmocka_unit_test_setup_teardown(test_0x2F_example, Setup, Teardown),
1124+
cmocka_unit_test_setup_teardown(test_0x2F_incorrect_request_length, Setup, Teardown),
1125+
cmocka_unit_test_setup_teardown(test_0x2F_negative_response, Setup, Teardown),
9831126
cmocka_unit_test_setup_teardown(test_0x31_RCRRP, Setup, Teardown),
9841127
cmocka_unit_test_setup_teardown(test_0x34_no_handler, Setup, Teardown),
9851128
cmocka_unit_test_setup_teardown(test_0x34, Setup, Teardown),

0 commit comments

Comments
 (0)