From 4bb2542529ddd9492bfd0921403274a332547155 Mon Sep 17 00:00:00 2001 From: cfif Date: Tue, 27 Jan 2026 13:55:33 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9E=D0=B1=D0=BD=D0=BE=D0=B2=D0=BB=D0=B5?= =?UTF-8?q?=D0=BD=D0=B8=D0=B5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CanUds.c | 281 +++++++++++++++++++++++++++++++++++++++++++++++++++++-- CanUds.h | 57 ++++++++++- 2 files changed, 328 insertions(+), 10 deletions(-) diff --git a/CanUds.c b/CanUds.c index ff4a7c9..e7a23d4 100644 --- a/CanUds.c +++ b/CanUds.c @@ -8,10 +8,32 @@ #include "TesterPresent_3e.h" #include "DiagnosticSessionControl_10.h" #include "StatusError.h" +#include "SystemDelayInterface.h" #define LOG_SIGN "CAN_UDS" #define LOGGER env->logger +uint32_t randomSecuritySeed = 0x11223344; + + +static unsigned long SeedToKeyUAZ_ERA_NAMI(unsigned char rnd, unsigned long dwSeedInfo) { +#define Mask 0x52F387A6UL + unsigned char i; + unsigned long key = dwSeedInfo; + + rnd += 35; + if (rnd < 35) rnd = 255; + + for (i = 0; i < rnd; i++) { + if (key & 0x80000000UL) + key = (key << 1) ^ Mask; + else + key <<= 1; + } + + return key; +} + bool ReceivedCan_func(void *arg, can_rx_message_type *canFrame) { tCanUds *env = arg; @@ -30,6 +52,13 @@ void ReceivedTP_func(void *arg, tCanTP_data *data) { } +static void setDefaultSecurityAccess(tCanUds *env) { + env->SA.requestSequenceRequestSeed = false; + env->SA.stateSecurityAccess = false; + env->SA.counter_max_attempts_default = 0; + env->SA.time_counter_max_attempts_default = 0; +} + uint16_t setResponseError(tCanUds *env, eUdsServices service, eUdsResponseError error) { env->dataResponse[0] = 0x7F; env->dataResponse[1] = service; @@ -76,6 +105,11 @@ static uint16_t DiagnosticSessionControl_10(tCanUds *env) { return setResponseError(env, UDS_DiagnosticSessionControl, UDS_error_sub_functionNotSupported); } + if (env->currentSession != com->sub_function) { + env->currentSession = com->sub_function; + setDefaultSecurityAccess(env); + } + tDiagnosticSessionsType *diagnosticSessionsType = (tDiagnosticSessionsType *) env->dataResponse; diagnosticSessionsType->ServiceId = UDS_DiagnosticSessionControl | 0b1000000; diagnosticSessionsType->diagnosticSessionType = env->data.data[1]; @@ -118,7 +152,7 @@ static uint16_t ReadDataByIdentifier_22(tCanUds *env) { env->dataResponse[2] = dataIdentifier_lo; memcpy(&env->dataResponse[3], uds_ReadDataByIdentifier_22_com_CF[dataIdentifier_lo].data, response_size); - return response_size; + return response_size + 3; } } @@ -133,7 +167,7 @@ static uint16_t ReadDataByIdentifier_22(tCanUds *env) { env->dataResponse[2] = dataIdentifier_lo; memcpy(&env->dataResponse[3], uds_ReadDataByIdentifier_22_com_F1[dataIdentifier_lo].data, response_size); - return response_size; + return response_size + 3; } } @@ -328,8 +362,8 @@ static uint16_t CommunicationControl_28(tCanUds *env) { UDS_error_incorrectMessageLengthOrInvalidFormat); } - if ((com->sub_function != enableRxAndTx) && - (com->sub_function != enableRxAndDisableTx)) { + if ((com->sub_function != UDS_sub_enableRxAndTx) && + (com->sub_function != UDS_sub_enableRxAndDisableTx)) { return setResponseError(env, UDS_Communication_Control, UDS_error_sub_functionNotSupported); } @@ -343,6 +377,239 @@ static uint16_t CommunicationControl_28(tCanUds *env) { // конец --------------------------- Молчание ------------------------------------------------------------- // конец --------------------------- Молчание ------------------------------------------------------------- +// начало --------------------------- Функции ------------------------------------------------------------- +// начало --------------------------- Функции ------------------------------------------------------------- +// начало --------------------------- Функции ------------------------------------------------------------- + +static uint16_t vUDS_routine_VIN_learn(tCanUds *env, eUdsRoutineControlType udsRoutineControlType) { + + eUdsRoutineStartStopRoutineResult udsStartStopRoutineResult = UDS_routine_RoutineStartStopSuccess; + eUdsRequestRoutineResult udsRequestRoutineResult = UDS_routine_RoutineStop; + + if ((udsRoutineControlType == UDS_routine_StartRoutine) || (udsRoutineControlType == UDS_routine_StopRoutine)) { + env->dataResponse[4] = udsStartStopRoutineResult; + } + + if (udsRoutineControlType == UDS_routine_RequestRoutineResults) { + env->dataResponse[4] = udsRequestRoutineResult; + } + + return 4 + 1; +} + +static uint16_t vUDS_routine_Deflector_learn(tCanUds *env, eUdsRoutineControlType udsRoutineControlType) { + + eUdsRoutineStartStopRoutineResult udsStartStopRoutineResult = UDS_routine_RoutineStartStopSuccess; + eUdsRequestRoutineResult udsRequestRoutineResult = UDS_routine_RoutineStop; + + if ((udsRoutineControlType == UDS_routine_StartRoutine) || (udsRoutineControlType == UDS_routine_StopRoutine)) { + env->dataResponse[4] = udsStartStopRoutineResult; + } + + if (udsRoutineControlType == UDS_routine_RequestRoutineResults) { + env->dataResponse[4] = udsRequestRoutineResult; + } + + return 4 + 1; +} + +static uint16_t vUDS_routine_Compare_Checksum(tCanUds *env, eUdsRoutineControlType udsRoutineControlType) { + + eUdsRoutineStartStopRoutineResult udsStartStopRoutineResult = UDS_routine_RoutineStartStopSuccess; + eUdsRequestRoutineResult udsRequestRoutineResult = UDS_routine_RoutineStop; + + if ((udsRoutineControlType == UDS_routine_StartRoutine) || (udsRoutineControlType == UDS_routine_StopRoutine)) { + env->dataResponse[4] = udsStartStopRoutineResult; + } + + if (udsRoutineControlType == UDS_routine_RequestRoutineResults) { + env->dataResponse[4] = udsRequestRoutineResult; + } + + return 4 + 1; +} + +static uint16_t +vUDS_routine_Check_Programming_Preconditions(tCanUds *env, eUdsRoutineControlType udsRoutineControlType) { + + eUdsRoutineStartStopRoutineResult udsStartStopRoutineResult = UDS_routine_RoutineStartStopSuccess; + eUdsRequestRoutineResult udsRequestRoutineResult = UDS_routine_RoutineStop; + + if ((udsRoutineControlType == UDS_routine_StartRoutine) || (udsRoutineControlType == UDS_routine_StopRoutine)) { + env->dataResponse[4] = udsStartStopRoutineResult; + } + + if (udsRoutineControlType == UDS_routine_RequestRoutineResults) { + env->dataResponse[4] = udsRequestRoutineResult; + } + + return 4 + 1; +} + +static uint16_t vUDS_routine_Erase_Memory(tCanUds *env, eUdsRoutineControlType udsRoutineControlType) { + + eUdsRoutineStartStopRoutineResult udsStartStopRoutineResult = UDS_routine_RoutineStartStopSuccess; + eUdsRequestRoutineResult udsRequestRoutineResult = UDS_routine_RoutineStop; + + if ((udsRoutineControlType == UDS_routine_StartRoutine) || (udsRoutineControlType == UDS_routine_StopRoutine)) { + env->dataResponse[4] = udsStartStopRoutineResult; + } + + if (udsRoutineControlType == UDS_routine_RequestRoutineResults) { + env->dataResponse[4] = udsRequestRoutineResult; + } + + return 4 + 1; +} + +static uint16_t +vUDS_routine_Check_Programming_Dependancies(tCanUds *env, eUdsRoutineControlType udsRoutineControlType) { + + eUdsRoutineStartStopRoutineResult udsStartStopRoutineResult = UDS_routine_RoutineStartStopSuccess; + eUdsRequestRoutineResult udsRequestRoutineResult = UDS_routine_RoutineStop; + + if ((udsRoutineControlType == UDS_routine_StartRoutine) || (udsRoutineControlType == UDS_routine_StopRoutine)) { + env->dataResponse[4] = udsStartStopRoutineResult; + } + + if (udsRoutineControlType == UDS_routine_RequestRoutineResults) { + env->dataResponse[4] = udsRequestRoutineResult; + } + + return 4 + 1; +} + +static uint16_t Routine_Control_31(tCanUds *env) { + + tUdsServiceCommand *com = (tUdsServiceCommand *) env->data.data; + + if (env->data.len < 4) { + return setResponseError(env, UDS_Communication_Control, + UDS_error_incorrectMessageLengthOrInvalidFormat); + } + + eUdsRoutineControlType udsRoutineControlType = env->data.data[1]; + uint8_t routineIdentifier_hi = env->data.data[1]; + uint8_t routineIdentifier_lo = env->data.data[2]; + uint16_t routineIdentifier = (routineIdentifier_hi << 8) | routineIdentifier_lo; + + if ((udsRoutineControlType != UDS_routine_StartRoutine) && (udsRoutineControlType != UDS_routine_StopRoutine) && + (udsRoutineControlType != UDS_routine_RequestRoutineResults)) { + return setResponseError(env, UDS_RoutineControl, UDS_error_sub_functionNotSupported); + } + + env->dataResponse[0] = UDS_RoutineControl | 0b1000000; + env->dataResponse[1] = routineIdentifier_hi; + env->dataResponse[2] = routineIdentifier_lo; + env->dataResponse[3] = env->data.data[3]; + + switch (routineIdentifier) { + case UDS_routine_VIN_learn: + return vUDS_routine_VIN_learn(env, udsRoutineControlType); + case UDS_routine_Deflector_learn: + return vUDS_routine_Deflector_learn(env, udsRoutineControlType); + case UDS_routine_Compare_Checksum: + return vUDS_routine_Compare_Checksum(env, udsRoutineControlType); + case UDS_routine_Check_Programming_Preconditions: + return vUDS_routine_Check_Programming_Preconditions(env, udsRoutineControlType); + case UDS_routine_Erase_Memory: + return vUDS_routine_Erase_Memory(env, udsRoutineControlType); + case UDS_routine_Check_Programming_Dependancies: + return vUDS_routine_Check_Programming_Dependancies(env, udsRoutineControlType); + default: + return setResponseError(env, UDS_RoutineControl, UDS_error_requestOutOfRange); + } + +} +// конец --------------------------- Функции ------------------------------------------------------------- +// конец --------------------------- Функции ------------------------------------------------------------- +// конец --------------------------- Функции ------------------------------------------------------------- + + +// начало --------------------------- Security Access ------------------------------------------------------------- +// начало --------------------------- Security Access ------------------------------------------------------------- +// начало --------------------------- Security Access ------------------------------------------------------------- + +static uint16_t SecurityAccess_27(tCanUds *env) { + + tUdsServiceCommand *com = (tUdsServiceCommand *) env->data.data; + + if (env->data.len < 2) { + return setResponseError(env, UDS_SecurityAccess, + UDS_error_incorrectMessageLengthOrInvalidFormat); + } + + if ((com->sub_function != UDS_sub_requestSeed) && (com->sub_function != UDS_sub_sendKey)) { + return setResponseError(env, UDS_SecurityAccess, UDS_error_sub_functionNotSupported); + } + + // Отправляется, если таймер задержки активен и получен запрос + if ((env->SA.time_counter_max_attempts_default > 0) && + (SystemGetMs() - env->SA.time_counter_max_attempts_default) < BLOCK_TIME_DEFAULT) { + return setResponseError(env, UDS_SecurityAccess, UDS_error_requiredTimeDelayNotExpired); + } + + if (com->sub_function == UDS_sub_requestSeed) { + + // Получен запрос + env->SA.requestSequenceRequestSeed = true; + + env->dataResponse[0] = UDS_SecurityAccess | 0b1000000; + env->dataResponse[1] = env->data.data[1] & 0b01111111; // sub-function + + env->dataResponse[2] = randomSecuritySeed >> 24; + env->dataResponse[3] = randomSecuritySeed >> 16; + env->dataResponse[4] = randomSecuritySeed >> 8; + env->dataResponse[5] = randomSecuritySeed; + + return 6; + } + + if (com->sub_function == UDS_sub_sendKey) { + + if (env->data.len < 6) { + return setResponseError(env, UDS_SecurityAccess, UDS_error_incorrectMessageLengthOrInvalidFormat); + } + + // Ранее не получен запрос UDS_sub_requestSeed + if (env->SA.requestSequenceRequestSeed == false) { + return setResponseError(env, UDS_SecurityAccess, UDS_error_requestSequenceError); + } + + if ((env->SA.time_counter_max_attempts_default > 0) && + (SystemGetMs() - env->SA.time_counter_max_attempts_default) > BLOCK_TIME_DEFAULT) { + env->SA.time_counter_max_attempts_default = 0; + env->SA.counter_max_attempts_default = 0; + } + + // Отправляется, если активирован таймер задержки из-за превышения максимального числа неудачных попыток доступа + if (env->SA.counter_max_attempts_default > MAX_ATTEMPTS_DEFAULT) { + env->SA.time_counter_max_attempts_default = SystemGetMs(); + return setResponseError(env, UDS_SecurityAccess, UDS_error_exceededNumberOfAttempts); + } + + uint32_t securitySeedFromServer = + (env->data.data[2] << 24) | (env->data.data[3] << 16) | (env->data.data[4] << 8) | env->data.data[5]; + + uint32_t securitySeedMy = SeedToKeyUAZ_ERA_NAMI(0, randomSecuritySeed); + + if (securitySeedMy != securitySeedFromServer) { + ++env->SA.counter_max_attempts_default; + return setResponseError(env, UDS_SecurityAccess, UDS_error_invalidKey); + } + + env->SA.stateSecurityAccess = true; + + env->dataResponse[0] = UDS_SecurityAccess | 0b1000000; + env->dataResponse[1] = env->data.data[1] & 0b01111111; // sub-function + + return 2; + } +} +// конец --------------------------- Security Access ------------------------------------------------------------- +// конец --------------------------- Security Access ------------------------------------------------------------- +// конец --------------------------- Security Access ------------------------------------------------------------- + const eUds_com uds_com[256] = { {NULL, ""}, {NULL, ""}, @@ -383,7 +650,7 @@ const eUds_com uds_com[256] = { {NULL, ""}, {NULL, ""}, {NULL, ""}, - {NULL, ""}, + {SecurityAccess_27, "SecurityAccess_27"}, {CommunicationControl_28, "CommunicationControl_28"}, {NULL, ""}, {NULL, ""}, @@ -393,7 +660,7 @@ const eUds_com uds_com[256] = { {WriteDataByIdentifier_2E, "WriteDataByIdentifier_2E"}, {InputOutputControlByIdentifier_2F, "InputOutputControlByIdentifier_2F"}, {NULL, ""}, - {NULL, ""}, + {Routine_Control_31, "Routine_Control_31"}, {NULL, ""}, {NULL, ""}, {NULL, ""}, @@ -702,5 +969,7 @@ void CanUds_Init( ); + env->currentSession = UDS_session_defaultSession; + InitThreadBlock(env->T_can_Uds, "CanUds", osPriorityNormal); }; \ No newline at end of file diff --git a/CanUds.h b/CanUds.h index 544f4e9..630ef36 100644 --- a/CanUds.h +++ b/CanUds.h @@ -11,6 +11,9 @@ #define CAN_US_QUEUE_SIZE 3 +#define MAX_ATTEMPTS_DEFAULT 3 +#define BLOCK_TIME_DEFAULT 60000 + // Время (логарифмическая шкала) // │ // ├── StMin (0.1-127 мс) ← Между КАДРАМИ @@ -34,7 +37,7 @@ typedef enum { UDS_ECUResetService = 0x11, UDS_InputOutputControlByIdentifier = 0x2F, UDS_SecurityAccess = 0x27, - UDS_Communication_Control= 0x28 + UDS_Communication_Control = 0x28 } eUdsServices; typedef enum { @@ -42,8 +45,12 @@ typedef enum { UDS_error_responseTooLong = 0x14, UDS_error_requestOutOfRange = 0x31, UDS_error_securityAccessDenied = 0x33, + UDS_error_invalidKey= 0x35, UDS_error_sub_functionNotSupported = 0x12, - UDS_error_conditionsNotCorrect = 0x22 + UDS_error_conditionsNotCorrect = 0x22, + UDS_error_requestSequenceError = 0x24, + UDS_error_exceededNumberOfAttempts = 0x36, + UDS_error_requiredTimeDelayNotExpired = 0x37 } eUdsResponseError; @@ -69,10 +76,42 @@ typedef enum { } eUdsReset; typedef enum { - enableRxAndTx = 0x0, - enableRxAndDisableTx = 0x1 + UDS_sub_enableRxAndTx = 0x0, + UDS_sub_enableRxAndDisableTx = 0x1 } eUdsCommunicationControl; +typedef enum { + UDS_sub_requestSeed = 0x1, + UDS_sub_sendKey = 0x2 +} eUdsSecurityAccess; + +typedef enum { + UDS_routine_StartRoutine = 1, + UDS_routine_StopRoutine = 2, + UDS_routine_RequestRoutineResults = 3 +} eUdsRoutineControlType; + +typedef enum { + UDS_routine_VIN_learn = 0x1300, + UDS_routine_Deflector_learn = 0xCF00, + UDS_routine_Compare_Checksum = 0x0202, + UDS_routine_Check_Programming_Preconditions = 00203, + UDS_routine_Erase_Memory = 0xFF00, + UDS_routine_Check_Programming_Dependancies = 0xFF01 +} eUdsRoutineModes; + +typedef enum { + UDS_routine_RoutineStartStopSuccess = 0, + UDS_routine_RoutineStartStopFailure = 0xff +} eUdsRoutineStartStopRoutineResult; + +typedef enum { + UDS_routine_RoutineStop = 0, + UDS_routine_RoutineRun = 1, + UDS_routine_RoutineFinishError = 2, + UDS_routine_RoutineFinishSuccess = 3 +} eUdsRequestRoutineResult; + typedef struct { eUdsServices service; uint8_t sub_function; @@ -97,6 +136,16 @@ typedef struct { uint32_t filterRespId[16]; uint8_t filterDirReq[16]; + + eUdsSession currentSession; + + struct { + bool requestSequenceRequestSeed; + bool stateSecurityAccess; + uint32_t counter_max_attempts_default; + uint32_t time_counter_max_attempts_default; + } SA; + tStaticThreadBlock(512) T_can_Uds; } tCanUds;