1063 lines
44 KiB
C
1063 lines
44 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"
|
|
#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) {
|
|
len += strlen(strPrintfDebug_UDS);
|
|
memcpy(env->hexString, strPrintfDebug_UDS, strlen(strPrintfDebug_UDS));
|
|
} else {
|
|
return env->hexString;
|
|
}
|
|
|
|
}
|
|
|
|
if (tail > 0) {
|
|
PrintfDebug(&data[full * 8], tail);
|
|
|
|
if ((len + strlen(strPrintfDebug_UDS)) < LEN_DEBUG_TP_BUFF) {
|
|
len += strlen(strPrintfDebug_UDS);
|
|
memcpy(env->hexString, strPrintfDebug_UDS, 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;
|
|
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) {
|
|
|
|
|
|
}
|
|
// конец --------------------------- Запись ---------------------------------------------------------
|
|
// конец --------------------------- Запись ---------------------------------------------------------
|
|
// конец --------------------------- Запись ---------------------------------------------------------
|
|
|
|
|
|
|
|
// начало --------------------------- Управление ---------------------------------------------------------
|
|
// начало --------------------------- Управление ---------------------------------------------------------
|
|
// начало --------------------------- Управление ---------------------------------------------------------
|
|
|
|
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(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, ""},
|
|
{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) {
|
|
|
|
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);
|
|
}; |