// // Created by cfif on 20.01.2026. // #include "CanUds.h" #include "CanPorts.h" #include "StatusData.h" #include "memory.h" #include "TesterPresent_3e.h" #include "DiagnosticSessionControl_10.h" #include "StatusError.h" #include "SystemDelayInterface.h" #include "candb.h" #include "stdio.h" #define LOG_SIGN "CAN_UDS" #define LOGGER env->logger uint32_t randomSecuritySeed = 0x853A08FB; extern uint32_t generate_key(uint32_t seed); static char strPrintfDebug_UDS[80]; static void PrintfDebug(uint8_t *data, uint8_t dlc) { switch (dlc) { case 0: sprintf(strPrintfDebug_UDS, " "); break; case 1: sprintf(strPrintfDebug_UDS, "%02X", data[0]); break; case 2: sprintf(strPrintfDebug_UDS, "%02X:%02X", data[0], data[1]); break; case 3: sprintf(strPrintfDebug_UDS, "%02X:%02X:%02X", data[0], data[1], data[2]); break; case 4: sprintf(strPrintfDebug_UDS, "%02X:%02X:%02X:%02X", data[0], data[1], data[2], data[3]); break; case 5: sprintf(strPrintfDebug_UDS, "%02X:%02X:%02X:%02X:%02X", data[0], data[1], data[2], data[3], data[4]); break; case 6: sprintf(strPrintfDebug_UDS, "%02X:%02X:%02X:%02X:%02X:%02X", data[0], data[1], data[2], data[3], data[4], data[5]); break; case 7: sprintf(strPrintfDebug_UDS, "%02X:%02X:%02X:%02X:%02X:%02X:%02X", data[0], data[1], data[2], data[3], data[4], data[5], data[6]); break; case 8: sprintf(strPrintfDebug_UDS, "%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X ", data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7]); break; default: sprintf(strPrintfDebug_UDS, " "); } } char *sendLogCanUdsHex(tCanUds *env, uint8_t *data, size_t size) { memset(env->hexString, 0, sizeof(env->hexString)); size_t len = 0; uint8_t full = size / 8; uint8_t tail = size % 8; for (uint8_t i = 0; i < full; ++i) { PrintfDebug(&data[i * 8], 8); if ((len + strlen(strPrintfDebug_UDS)) < LEN_DEBUG_TP_BUFF) { memcpy(&env->hexString[len], strPrintfDebug_UDS, strlen(strPrintfDebug_UDS)); len += strlen(strPrintfDebug_UDS); } else { return env->hexString; } } if (tail > 0) { PrintfDebug(&data[full * 8], tail); if ((len + strlen(strPrintfDebug_UDS)) < LEN_DEBUG_TP_BUFF) { memcpy(&env->hexString[len], strPrintfDebug_UDS, strlen(strPrintfDebug_UDS)); len += strlen(strPrintfDebug_UDS); } else { return env->hexString; } } return env->hexString; } bool ReceivedCan_func(void *arg, can_rx_message_type *canFrame) { tCanUds *env = arg; return false; } void ReceivedTP_func(void *arg, tCanTP_data *data) { tCanUds *env = arg; osStatus_t status = osMessageQueuePut(env->queue, data, 0, 0U); if (status != osOK) { LoggerInfoStatic(LOGGER, LOG_SIGN, "Ошибка добавления в очередь addCommandQueue") } } 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; env->dataResponse[2] = error; return 3; } // начало --------------------------- ПИНГ --------------------------------------------------------- // начало --------------------------- ПИНГ --------------------------------------------------------- // начало --------------------------- ПИНГ --------------------------------------------------------- static uint16_t TesterPresent_3E(tCanUds *env) { tUdsServiceCommand *com = (tUdsServiceCommand *) env->data.data; if (env->data.len < 2) { return setResponseError(env, UDS_TesterPresent, UDS_error_incorrectMessageLengthOrInvalidFormat); } tTesterPresent *testerPresent = (tTesterPresent *) env->dataResponse; testerPresent->ServiceId = UDS_TesterPresent | 0b1000000; testerPresent->zeroSubFunction = 0; return 2; } // конец --------------------------- ПИНГ --------------------------------------------------------- // конец --------------------------- ПИНГ --------------------------------------------------------- // конец --------------------------- ПИНГ --------------------------------------------------------- // начало ----------------------------- Сессия ------------------------------------------------------------- // начало ----------------------------- Сессия ------------------------------------------------------------- // начало ----------------------------- Сессия ------------------------------------------------------------- static uint16_t DiagnosticSessionControl_10(tCanUds *env) { tUdsServiceCommand *com = (tUdsServiceCommand *) env->data.data; if (env->data.len < 2) { return setResponseError(env, UDS_DiagnosticSessionControl, UDS_error_incorrectMessageLengthOrInvalidFormat); } if ((com->sub_function != UDS_session_defaultSession) && (com->sub_function != UDS_session_programmingSession) && (com->sub_function != UDS_session_extendedDiagnosticSession)) { 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]; // Нормальный таймаут. Важно: Клиент может отправлять TesterPresent для поддержания сессии до истечения этого таймаута diagnosticSessionsType->sessionParameterRecord[0] = 0; diagnosticSessionsType->sessionParameterRecord[1] = 0x32; // Расширенный таймаут. Например, ECU может обрабатывать сложный запрос 2 секунды и отправлять NRC 0x78, затем ответ diagnosticSessionsType->sessionParameterRecord[2] = 0x01; diagnosticSessionsType->sessionParameterRecord[3] = 0xF4; return 6; } // конец ----------------------------- Сессия -------------------------------------------------------------- // конец ----------------------------- Сессия -------------------------------------------------------------- // конец ----------------------------- Сессия -------------------------------------------------------------- // начало --------------------------- Чтение --------------------------------------------------------- // начало --------------------------- Чтение --------------------------------------------------------- // начало --------------------------- Чтение --------------------------------------------------------- static uint16_t ReadDataByIdentifier_22(tCanUds *env) { if (env->data.len < 3) { return setResponseError(env, UDS_ReadDataByIdentifier, UDS_error_incorrectMessageLengthOrInvalidFormat); } uint8_t dataIdentifier_hi = env->data.data[1]; uint8_t dataIdentifier_lo = env->data.data[2]; // uint16_t dataIdentifier = (dataIdentifier_hi << 8) | dataIdentifier_lo; if (dataIdentifier_hi == 0xCF) { if (uds_ReadDataByIdentifier_22_com_CF[dataIdentifier_lo].data != NULL) { uint16_t response_size = uds_ReadDataByIdentifier_22_com_CF[dataIdentifier_lo].size; env->dataResponse[0] = UDS_ReadDataByIdentifier | 0b1000000; env->dataResponse[1] = dataIdentifier_hi; env->dataResponse[2] = dataIdentifier_lo; memcpy(&env->dataResponse[3], uds_ReadDataByIdentifier_22_com_CF[dataIdentifier_lo].data, response_size); return response_size + 3; } } if (dataIdentifier_hi == 0xF1) { if (uds_ReadDataByIdentifier_22_com_F1[dataIdentifier_lo].data != NULL) { uint16_t response_size = uds_ReadDataByIdentifier_22_com_F1[dataIdentifier_lo].size; env->dataResponse[0] = UDS_ReadDataByIdentifier | 0b1000000; env->dataResponse[1] = dataIdentifier_hi; env->dataResponse[2] = dataIdentifier_lo; memcpy(&env->dataResponse[3], uds_ReadDataByIdentifier_22_com_F1[dataIdentifier_lo].data, response_size); return response_size + 3; } } return setResponseError(env, UDS_ReadDataByIdentifier, UDS_error_requestOutOfRange); } // конец --------------------------- Чтение --------------------------------------------------------- // конец --------------------------- Чтение --------------------------------------------------------- // конец --------------------------- Чтение --------------------------------------------------------- // начало --------------------------- Запись --------------------------------------------------------- // начало --------------------------- Запись --------------------------------------------------------- // начало --------------------------- Запись --------------------------------------------------------- static uint16_t WriteDataByIdentifier_2E(tCanUds *env) { if (env->data.len < 3) { return setResponseError(env, UDS_WriteDataByIdentifier, UDS_error_incorrectMessageLengthOrInvalidFormat); } uint8_t dataIdentifier_hi = env->data.data[1]; uint8_t dataIdentifier_lo = env->data.data[2]; // uint16_t dataIdentifier = (dataIdentifier_hi << 8) | dataIdentifier_lo; if (dataIdentifier_hi == 0xCF) { if (uds_WriteDataByIdentifier_2E_com_CF[dataIdentifier_lo].data != NULL) { uint16_t size = uds_WriteDataByIdentifier_2E_com_CF[dataIdentifier_lo].size; if (env->data.len != size + 3) { return setResponseError(env, UDS_WriteDataByIdentifier, UDS_error_incorrectMessageLengthOrInvalidFormat); } env->dataResponse[0] = UDS_WriteDataByIdentifier | 0b1000000; env->dataResponse[1] = dataIdentifier_hi; env->dataResponse[2] = dataIdentifier_lo; memcpy(uds_WriteDataByIdentifier_2E_com_CF[dataIdentifier_lo].data, &env->data.data[3], size); return 3; } } if (dataIdentifier_hi == 0xF1) { if (uds_WriteDataByIdentifier_2E_com_F1[dataIdentifier_lo].data != NULL) { uint16_t size = uds_WriteDataByIdentifier_2E_com_F1[dataIdentifier_lo].size; env->dataResponse[0] = UDS_WriteDataByIdentifier | 0b1000000; env->dataResponse[1] = dataIdentifier_hi; env->dataResponse[2] = dataIdentifier_lo; memcpy(uds_WriteDataByIdentifier_2E_com_F1[dataIdentifier_lo].data, &env->data.data[3], size); return 3; } } return setResponseError(env, UDS_WriteDataByIdentifier, UDS_error_requestOutOfRange); } // конец --------------------------- Запись --------------------------------------------------------- // конец --------------------------- Запись --------------------------------------------------------- // конец --------------------------- Запись --------------------------------------------------------- // начало --------------------------- Управление --------------------------------------------------------- // начало --------------------------- Управление --------------------------------------------------------- // начало --------------------------- Управление --------------------------------------------------------- static uint16_t InputOutputControlByIdentifier_2F(tCanUds *env) { if (env->data.len < 5) { return setResponseError(env, UDS_InputOutputControlByIdentifier, UDS_error_incorrectMessageLengthOrInvalidFormat); } if ((env->data.data[3] != UDS_io_returnControlToECU) && (env->data.data[3] != UDS_io_shortTermAdjustment)) { return setResponseError(env, UDS_InputOutputControlByIdentifier, UDS_error_conditionsNotCorrect); } uint8_t dataIdentifier_hi = env->data.data[1]; uint8_t dataIdentifier_lo = env->data.data[2]; return setResponseError(env, UDS_InputOutputControlByIdentifier, UDS_error_requestOutOfRange); } // конец --------------------------- Управление --------------------------------------------------------- // конец --------------------------- Управление --------------------------------------------------------- // конец --------------------------- Управление --------------------------------------------------------- // начало --------------------------- Перезагрузка --------------------------------------------------------- // начало --------------------------- Перезагрузка --------------------------------------------------------- // начало --------------------------- Перезагрузка --------------------------------------------------------- static uint16_t ECUReset_11(tCanUds *env) { tUdsServiceCommand *com = (tUdsServiceCommand *) env->data.data; if (env->data.len < 2) { return setResponseError(env, UDS_ECUResetService, UDS_error_incorrectMessageLengthOrInvalidFormat); } if ((com->sub_function != UDS_reset_hardReset) && (com->sub_function != UDS_reset_softReset)) { return setResponseError(env, UDS_ECUResetService, UDS_error_sub_functionNotSupported); } env->dataResponse[0] = UDS_ECUResetService | 0b1000000;; env->dataResponse[1] = env->data.data[1] & 0b01111111; // sub-function return 2; } // конец --------------------------- Перезагрузка --------------------------------------------------------- // конец --------------------------- Перезагрузка --------------------------------------------------------- // конец --------------------------- Перезагрузка --------------------------------------------------------- // начало --------------------------- Очистка ошибок --------------------------------------------------------- // начало --------------------------- Очистка ошибок --------------------------------------------------------- // начало --------------------------- Очистка ошибок --------------------------------------------------------- static uint16_t UDS_ClearDiagnosticInformation_14(tCanUds *env) { if (env->data.len < 4) { return setResponseError(env, UDS_ClearDiagnosticInformation, UDS_error_incorrectMessageLengthOrInvalidFormat); } SetGetDiagnosticData(env->Diagnostic, DIAGNOSTIC_UDS_ClearDiagnosticInformation_14, NULL); env->dataResponse[0] = UDS_ClearDiagnosticInformation | 0b1000000; return 1; } // конец --------------------------- Очистка ошибок --------------------------------------------------------- // конец --------------------------- Очистка ошибок --------------------------------------------------------- // конец --------------------------- Очистка ошибок --------------------------------------------------------- // начало --------------------------- Чтение ошибок --------------------------------------------------------- // начало --------------------------- Чтение ошибок --------------------------------------------------------- // начало --------------------------- Чтение ошибок --------------------------------------------------------- static uint16_t UDS_ReadDTCInformation_19(tCanUds *env) { tUdsServiceCommand *com = (tUdsServiceCommand *) env->data.data; if (env->data.len < 3) { return setResponseError(env, UDS_ReadDTCInformation, UDS_error_incorrectMessageLengthOrInvalidFormat); } if ((com->sub_function != UDS_dtc_reportNumberOfDTCByStatusMask) && (com->sub_function != UDS_dtc_reportDTCByStatusMask)) { return setResponseError(env, UDS_ReadDTCInformation, UDS_error_sub_functionNotSupported); } uint16_t response_size = 0; env->dataResponse[0] = UDS_ReadDTCInformation | 0b1000000; if (com->sub_function == UDS_dtc_reportNumberOfDTCByStatusMask) { env->dataResponse[1] = env->data.data[1] & 0b01111111; // sub-function // доступные биты статусов env->dataResponse[2] = 0xFF; // формат кодирования DTC env->dataResponse[3] = 1; // старший байт количества DTC env->dataResponse[4] = 0; tDiagnosticDTC diagnosticDTC = {env->data.data[2], NULL}; // младший байт количества DTC env->dataResponse[5] = SetGetDiagnosticData(env->Diagnostic, DIAGNOSTIC_UDS_ReadDTCInformation_19_1, &diagnosticDTC); response_size = 6; } if (com->sub_function == UDS_dtc_reportDTCByStatusMask) { env->dataResponse[1] = env->data.data[1] & 0b01111111; // sub-function // доступные биты статусов env->dataResponse[2] = 0xFF; response_size = 3; tDiagnosticDTC diagnosticDTC = {env->data.data[2], &env->dataResponse[response_size]}; response_size += SetGetDiagnosticData(env->Diagnostic, DIAGNOSTIC_UDS_ReadDTCInformation_19_2, &diagnosticDTC); } return response_size; } // конец --------------------------- Чтение ошибок --------------------------------------------------------- // конец --------------------------- Чтение ошибок --------------------------------------------------------- // конец --------------------------- Чтение ошибок --------------------------------------------------------- // начало --------------------------- Молчание ------------------------------------------------------------- // начало --------------------------- Молчание ------------------------------------------------------------- // начало --------------------------- Молчание ------------------------------------------------------------- static uint16_t CommunicationControl_28(tCanUds *env) { tUdsServiceCommand *com = (tUdsServiceCommand *) env->data.data; if (env->data.len < 3) { return setResponseError(env, UDS_Communication_Control, UDS_error_incorrectMessageLengthOrInvalidFormat); } if ((com->sub_function != UDS_sub_enableRxAndTx) && (com->sub_function != UDS_sub_enableRxAndDisableTx)) { return setResponseError(env, UDS_Communication_Control, UDS_error_sub_functionNotSupported); } env->dataResponse[0] = UDS_Communication_Control | 0b1000000; env->dataResponse[1] = env->data.data[1] & 0b01111111; // sub-function return 2; } // конец --------------------------- Молчание ------------------------------------------------------------- // конец --------------------------- Молчание ------------------------------------------------------------- // конец --------------------------- Молчание ------------------------------------------------------------- // начало --------------------------- Функции ------------------------------------------------------------- // начало --------------------------- Функции ------------------------------------------------------------- // начало --------------------------- Функции ------------------------------------------------------------- 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 // Доступ уже получен if (env->SA.stateSecurityAccess) { env->dataResponse[2] = 0; env->dataResponse[3] = 0; env->dataResponse[4] = 0; env->dataResponse[5] = 0; } else { if (randomSecuritySeed == 0) { randomSecuritySeed = 0x11223344; } 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_requestOutOfRange); } // Ранее не получен запрос 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 = generate_key(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; } return setResponseError(env, UDS_SecurityAccess, UDS_error_invalidKey); } // конец --------------------------- Security Access ------------------------------------------------------------- // конец --------------------------- Security Access ------------------------------------------------------------- // конец --------------------------- Security Access ------------------------------------------------------------- const eUds_com uds_com[256] = { {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {DiagnosticSessionControl_10, "DiagnosticSessionControl_10"}, {ECUReset_11, "ECUReset_11"}, {NULL, ""}, {NULL, ""}, {UDS_ClearDiagnosticInformation_14, "UDS_ClearDiagnosticInformation_14"}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {UDS_ReadDTCInformation_19, "UDS_ReadDTCInformation_19"}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {ReadDataByIdentifier_22, "ReadDataByIdentifier_22"}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {SecurityAccess_27, "SecurityAccess_27"}, {CommunicationControl_28, "CommunicationControl_28"}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {WriteDataByIdentifier_2E, "WriteDataByIdentifier_2E"}, {InputOutputControlByIdentifier_2F, "InputOutputControlByIdentifier_2F"}, {NULL, ""}, {Routine_Control_31, "Routine_Control_31"}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {TesterPresent_3E, "TesterPresent_3E"}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""}, {NULL, ""} }; void CanUds(tCanUds *env) { for (;;) { osStatus_t status = osMessageQueueGet(env->queue, &env->data, 0, 1000); if (status == osOK) { uint8_t com = env->data.data[0]; if (uds_com[com].func != NULL) { sendLogCanUdsHex(env, env->data.data, env->data.len); LoggerFormatInfo(LOGGER, LOG_SIGN, "> %s [%d] %s", uds_com[com].desc, env->data.len, env->hexString) uint8_t response_size = uds_com[com].func(env); if (response_size) { sendLogCanUdsHex(env, env->dataResponse, response_size); LoggerFormatInfo(LOGGER, LOG_SIGN, "< %s [%d] %s", uds_com[com].desc, response_size, env->hexString) CanSerialPortFrameTpTransmit(&env->canSerialPortFrameTp, env->dataResponse, response_size, PROTOCOL_CAN_ADR_UDS, WAIT_FRAME_WRITE); } } else { asm("nop"); } } } } void CanSerialPortCanUds_Start(tCanUds *env) { ThreadBlock_Start(env->T_can_Uds, env, CanUds); CanSerialPortFrameTp_Start(&env->canSerialPortFrameTp); } void CanUds_Init( tCanUds *env, tDiagnostic *Diagnostic, tSerialPortFrameIO *CanIO, tDeviceStorage *deviceStorage, tLoggerInterface *logger) { env->logger = logger; env->CanIO = CanIO; env->deviceStorage = deviceStorage; env->Diagnostic = Diagnostic; env->filterIdCount = 2; env->queue = osMessageQueueNew(CAN_US_QUEUE_SIZE, sizeof(tCanTP_data), NULL); env->filterReqId[0] = Diag_To_CCU_CANID; env->filterReqId[1] = Diag_Functional_CANID; env->filterReqId[2] = 0; env->filterReqId[3] = 0; env->filterReqId[4] = 0; env->filterReqId[5] = 0; env->filterReqId[6] = 0; env->filterRespId[0] = 0; env->filterRespId[1] = 0; env->filterRespId[2] = 0; env->filterRespId[3] = 0; env->filterRespId[4] = 0; env->filterRespId[5] = 0; env->filterRespId[6] = 0; env->filterDirReq[0] = 0; env->filterDirReq[1] = 0; env->filterDirReq[2] = 0; env->filterDirReq[3] = 0; env->filterDirReq[4] = 0; env->filterDirReq[5] = 1; env->filterDirReq[6] = 0; CanSerialPortFrameTpInit( &env->canSerialPortFrameTp, ReceivedCan_func, env, ReceivedTP_func, env, env->CanIO, (tCanTP_data *) &env->canTP_Ext_data, sizeof(env->canTP_Ext_data.data), env->logger, env->filterIdCount, env->filterReqId, env->filterRespId, env->filterDirReq ); env->currentSession = UDS_session_defaultSession; InitThreadBlock(env->T_can_Uds, "CanUds", osPriorityNormal); };