1070 lines
46 KiB
C
1070 lines
46 KiB
C
//
|
|
// 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"
|
|
|
|
#define LOG_SIGN "CAN_UDS"
|
|
#define LOGGER env->logger
|
|
|
|
uint32_t randomSecuritySeed = 0x11223344;
|
|
|
|
|
|
// Определение операций инверсии бит
|
|
#define INV_OP_A_MASK ((1UL << 6) | (1UL << 15) | (1UL << 17) | (1UL << 11) | \
|
|
(1UL << 9) | (1UL << 16) | (1UL << 7) | (1UL << 10) | \
|
|
(1UL << 13) | (1UL << 4))
|
|
|
|
#define INV_OP_B_MASK ((1UL << 14) | (1UL << 16) | (1UL << 11) | (1UL << 9) | \
|
|
(1UL << 15) | (1UL << 7) | (1UL << 10) | (1UL << 13) | \
|
|
(1UL << 4) | (1UL << 1))
|
|
|
|
#define INV_OP_C_MASK ((1UL << 1) | (1UL << 7) | (1UL << 10) | (1UL << 13) | \
|
|
(1UL << 4) | (1UL << 14) | (1UL << 16) | (1UL << 11) | \
|
|
(1UL << 9) | (1UL << 15))
|
|
|
|
/**
|
|
* @brief Генерация ключа по сиду согласно алгоритму CCU
|
|
* @param seed Входное 32-битное значение сида
|
|
* @return 32-битный ключ
|
|
*/
|
|
static uint32_t generate_key_from_seed(uint32_t seed) {
|
|
// Шаг 1: Извлечение коэффициентов из сида
|
|
// Коэффициенты извлекаются из определенных битовых позиций (0 - LSB)
|
|
|
|
// Коэффициент 1 (бит 20): направление сдвига (0=влево, 1=вправо)
|
|
uint8_t shift_direction = (seed >> 20) & 0x01;
|
|
|
|
// Коэффициенты 2-5 (биты 7,11,5,8): количество сдвигов (4-битное число)
|
|
uint8_t shift_count = 0;
|
|
shift_count |= ((seed >> 7) & 0x01) << 3; // coefficient2 (MSB)
|
|
shift_count |= ((seed >> 11) & 0x01) << 2; // coefficient3
|
|
shift_count |= ((seed >> 5) & 0x01) << 1; // coefficient4
|
|
shift_count |= ((seed >> 8) & 0x01) << 0; // coefficient5 (LSB)
|
|
|
|
// Коэффициенты 6-7 (биты 22,1): выбор операции инверсии (2 бита)
|
|
uint8_t inversion_operation = 0;
|
|
inversion_operation |= ((seed >> 22) & 0x01) << 1; // coefficient6 (MSB)
|
|
inversion_operation |= ((seed >> 1) & 0x01) << 0; // coefficient7 (LSB)
|
|
|
|
// Коэффициенты 8-9 (биты 21,6): выбор битовой операции (2 бита)
|
|
uint8_t bitwise_operation = 0;
|
|
bitwise_operation |= ((seed >> 21) & 0x01) << 1; // coefficient8 (MSB)
|
|
bitwise_operation |= ((seed >> 6) & 0x01) << 0; // coefficient9 (LSB)
|
|
|
|
// Шаг 2: Операция вращения (циклический сдвиг)
|
|
uint32_t intermediate1;
|
|
|
|
if (shift_direction == 0) {
|
|
// Вращение влево
|
|
intermediate1 = (seed << shift_count) | (seed >> (32 - shift_count));
|
|
} else {
|
|
// Вращение вправо
|
|
intermediate1 = (seed >> shift_count) | (seed << (32 - shift_count));
|
|
}
|
|
|
|
// Шаг 3: Операция инверсии бит
|
|
uint32_t intermediate2 = intermediate1;
|
|
|
|
switch (inversion_operation) {
|
|
case 0x00: // Нет инверсии
|
|
// Ничего не делаем
|
|
break;
|
|
case 0x01: // Операция инверсии A
|
|
intermediate2 = intermediate1 ^ INV_OP_A_MASK;
|
|
break;
|
|
case 0x02: // Операция инверсии B
|
|
intermediate2 = intermediate1 ^ INV_OP_B_MASK;
|
|
break;
|
|
case 0x03: // Операция инверсии C
|
|
intermediate2 = intermediate1 ^ INV_OP_C_MASK;
|
|
break;
|
|
default:
|
|
// Недопустимая операция - оставляем как есть
|
|
break;
|
|
}
|
|
|
|
// Шаг 4: Финальная битовая операция
|
|
uint32_t key;
|
|
|
|
switch (bitwise_operation) {
|
|
case 0x00: // Key = intermediate2
|
|
key = intermediate2;
|
|
break;
|
|
case 0x01: // Key = seed AND intermediate2
|
|
key = seed & intermediate2;
|
|
break;
|
|
case 0x02: // Key = seed XOR intermediate2
|
|
key = seed ^ intermediate2;
|
|
break;
|
|
case 0x03: // Key = seed OR intermediate2
|
|
key = seed | intermediate2;
|
|
break;
|
|
default:
|
|
// Недопустимая операция - возвращаем intermediate2
|
|
key = intermediate2;
|
|
break;
|
|
}
|
|
|
|
return key;
|
|
}
|
|
|
|
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 (com->service < 2) {
|
|
return setResponseError(env, UDS_TesterPresent, UDS_error_incorrectMessageLengthOrInvalidFormat);
|
|
}
|
|
|
|
tTesterPresent testerPresent;
|
|
testerPresent.ServiceId = UDS_TesterPresent | 0b1000000;
|
|
testerPresent.zeroSubFunction = 0;
|
|
|
|
return 2;
|
|
}
|
|
// конец --------------------------- ПИНГ ---------------------------------------------------------
|
|
// конец --------------------------- ПИНГ ---------------------------------------------------------
|
|
// конец --------------------------- ПИНГ ---------------------------------------------------------
|
|
|
|
// начало ----------------------------- Сессия -------------------------------------------------------------
|
|
// начало ----------------------------- Сессия -------------------------------------------------------------
|
|
// начало ----------------------------- Сессия -------------------------------------------------------------
|
|
static uint16_t DiagnosticSessionControl_10(tCanUds *env) {
|
|
tUdsServiceCommand *com = (tUdsServiceCommand *) env->data.data;
|
|
|
|
if (com->service < 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 4;
|
|
}
|
|
// конец ----------------------------- Сессия --------------------------------------------------------------
|
|
// конец ----------------------------- Сессия --------------------------------------------------------------
|
|
// конец ----------------------------- Сессия --------------------------------------------------------------
|
|
|
|
|
|
// начало --------------------------- Чтение ---------------------------------------------------------
|
|
// начало --------------------------- Чтение ---------------------------------------------------------
|
|
// начало --------------------------- Чтение ---------------------------------------------------------
|
|
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) {
|
|
|
|
|
|
}
|
|
// конец --------------------------- Запись ---------------------------------------------------------
|
|
// конец --------------------------- Запись ---------------------------------------------------------
|
|
// конец --------------------------- Запись ---------------------------------------------------------
|
|
|
|
|
|
|
|
// начало --------------------------- Управление ---------------------------------------------------------
|
|
// начало --------------------------- Управление ---------------------------------------------------------
|
|
// начало --------------------------- Управление ---------------------------------------------------------
|
|
|
|
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);
|
|
}
|
|
|
|
setDiagnosticData(env->Diagnostic, DIAGNOSTIC_SET_UDS_ClearDiagnosticInformation_14);
|
|
|
|
env->dataResponse[0] = UDS_ClearDiagnosticInformation | 0b1000000;
|
|
|
|
return 1;
|
|
}
|
|
// конец --------------------------- Очистка ошибок ---------------------------------------------------------
|
|
// конец --------------------------- Очистка ошибок ---------------------------------------------------------
|
|
// конец --------------------------- Очистка ошибок ---------------------------------------------------------
|
|
|
|
// начало --------------------------- Чтение ошибок ---------------------------------------------------------
|
|
// начало --------------------------- Чтение ошибок ---------------------------------------------------------
|
|
// начало --------------------------- Чтение ошибок ---------------------------------------------------------
|
|
|
|
void SetUdsDTC_error(tCanUds *env, uint8_t mask, uint16_t *sizeResponse) {
|
|
|
|
for (uint8_t i = 0; i < COUNT_DTC_CODE_ERROR; ++i) {
|
|
|
|
if (dtc_state_error[i] & mask) {
|
|
env->dataResponse[*sizeResponse] = dtc_codes[i].DTCHighByte;
|
|
env->dataResponse[*sizeResponse + 1] = dtc_codes[i].DTCMiddleByte;
|
|
env->dataResponse[*sizeResponse + 2] = dtc_codes[i].DTCLowByte;
|
|
env->dataResponse[*sizeResponse + 3] = dtc_state_error[i] & mask;
|
|
*sizeResponse += 4;
|
|
}
|
|
}
|
|
}
|
|
|
|
uint8_t GetCountUdsDTC_error(uint8_t mask) {
|
|
|
|
uint8_t count = 0;
|
|
|
|
for (uint8_t i = 0; i < COUNT_DTC_CODE_ERROR; ++i) {
|
|
|
|
if (dtc_state_error[i] & mask) {
|
|
++count;
|
|
}
|
|
}
|
|
|
|
return count;
|
|
}
|
|
|
|
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;
|
|
// младший байт количества DTC
|
|
env->dataResponse[5] = GetCountUdsDTC_error(env->data.data[2]);
|
|
|
|
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;
|
|
|
|
SetUdsDTC_error(env, env->data.data[2], &response_size);
|
|
}
|
|
|
|
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_from_seed(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, ""},
|
|
{DiagnosticSessionControl_10, "DiagnosticSessionControl_10"},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{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, ""},
|
|
{ReadDataByIdentifier_22, "ReadDataByIdentifier_22"},
|
|
{NULL, ""},
|
|
{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) {
|
|
|
|
uint8_t response_size = uds_com[com].func(env);
|
|
|
|
if (response_size) {
|
|
|
|
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 = 0;
|
|
|
|
env->queue = osMessageQueueNew(CAN_US_QUEUE_SIZE, sizeof(tCanTP_data), NULL);
|
|
|
|
env->filterReqId[0] = 0;
|
|
env->filterReqId[1] = 0;
|
|
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);
|
|
}; |