1636 lines
68 KiB
C
1636 lines
68 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"
|
||
#include "fc7xxx_driver_flash.h"
|
||
|
||
#define LOG_SIGN "CAN_UDS"
|
||
#define LOGGER env->logger
|
||
|
||
|
||
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, " ");
|
||
}
|
||
|
||
}
|
||
|
||
// Проверка валидности BCD
|
||
static bool is_valid_bcd(uint8_t value) {
|
||
return ((value & 0x0F) <= 9) && ((value >> 4) <= 9);
|
||
}
|
||
|
||
// Проверка года
|
||
static bool is_valid_year(uint8_t year) {
|
||
return is_valid_bcd(year); // 0x00-0x99
|
||
}
|
||
|
||
// Проверка месяца
|
||
static bool is_valid_month(uint8_t month) {
|
||
if (!is_valid_bcd(month)) return false;
|
||
uint8_t m = ((month >> 4) * 10) + (month & 0x0F);
|
||
return (m >= 1 && m <= 12);
|
||
}
|
||
|
||
// Проверка дня (с учетом месяца и года)
|
||
static bool is_valid_day(uint8_t year, uint8_t month, uint8_t day) {
|
||
if (!is_valid_bcd(day)) return false;
|
||
|
||
uint8_t y = ((year >> 4) * 10) + (year & 0x0F);
|
||
uint8_t m = ((month >> 4) * 10) + (month & 0x0F);
|
||
uint8_t d = ((day >> 4) * 10) + (day & 0x0F);
|
||
|
||
if (d < 1) return false;
|
||
|
||
// Дни в месяцах
|
||
uint8_t days_in_month[] = {31, 28, 31, 30, 31, 30,
|
||
31, 31, 30, 31, 30, 31};
|
||
|
||
// Проверка на високосный год
|
||
if (m == 2) {
|
||
bool is_leap = (y % 4 == 0 && y % 100 != 0) || (y % 400 == 0);
|
||
return d <= (is_leap ? 29 : 28);
|
||
}
|
||
|
||
return d <= days_in_month[m - 1];
|
||
}
|
||
|
||
static bool isSecurityAccessDenied(tCanUds *env, tPermissionSession isPermissionSession) {
|
||
bool result = false;
|
||
|
||
switch (isPermissionSession) {
|
||
|
||
case perm_session_defaultSession: {
|
||
if (env->currentSession == UDS_session_defaultSession)
|
||
result = true;
|
||
break;
|
||
}
|
||
|
||
case perm_session_programmingSession: {
|
||
if (env->currentSession == UDS_session_programmingSession)
|
||
result = true;
|
||
break;
|
||
}
|
||
|
||
case perm_session_extendedDiagnosticSession: {
|
||
if (env->currentSession == UDS_session_extendedDiagnosticSession)
|
||
result = true;
|
||
break;
|
||
}
|
||
|
||
case perm_session_defaultSession_programmingSession: {
|
||
if ((env->currentSession == UDS_session_defaultSession) ||
|
||
(env->currentSession == UDS_session_programmingSession))
|
||
result = true;
|
||
break;
|
||
}
|
||
|
||
case perm_session_defaultSession_extendedDiagnosticSession: {
|
||
if ((env->currentSession == UDS_session_defaultSession) ||
|
||
(env->currentSession == UDS_session_extendedDiagnosticSession))
|
||
result = true;
|
||
break;
|
||
}
|
||
|
||
case perm_session_extendedDiagnosticSession_programmingSession: {
|
||
if ((env->currentSession == UDS_session_extendedDiagnosticSession) ||
|
||
(env->currentSession == UDS_session_programmingSession))
|
||
result = true;
|
||
break;
|
||
}
|
||
|
||
case perm_session_all: {
|
||
result = true;
|
||
break;
|
||
}
|
||
|
||
default:
|
||
break;
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
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);
|
||
|
||
// LoggerFormatInfo(LOGGER, LOG_SIGN, "Add queue (UDS): %d", data[0])
|
||
|
||
if (status != osOK) {
|
||
#if (LOG_UDS == 1)
|
||
LoggerInfoStatic(LOGGER, LOG_SIGN, "Error addCommandQueue")
|
||
#endif
|
||
}
|
||
|
||
}
|
||
|
||
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;
|
||
|
||
env->currentSessionTesterPresentTimeout = SystemGetMs() + timeout_session_S3_Server;
|
||
|
||
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 == UDS_session_programmingSession) &&
|
||
(com->sub_function == UDS_session_extendedDiagnosticSession)) {
|
||
return setResponseError(env, UDS_DiagnosticSessionControl, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
|
||
if (env->currentSession != com->sub_function) {
|
||
|
||
env->currentSession = com->sub_function;
|
||
|
||
setDefaultSecurityAccess(env);
|
||
statusData.Status_Active_Diagnostic_Session.Active_Diagnostic_Session = env->currentSession;
|
||
}
|
||
|
||
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) {
|
||
|
||
|
||
if (!isSecurityAccessDenied(env,
|
||
uds_ReadDataByIdentifier_22_com_CF[dataIdentifier_lo].isPermissionSession)) {
|
||
return setResponseError(env, UDS_ReadDataByIdentifier, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
if ((uds_ReadDataByIdentifier_22_com_CF[dataIdentifier_lo].isSecurity) && (!env->SA.stateSecurityAccess)) {
|
||
return setResponseError(env, UDS_ReadDataByIdentifier, UDS_error_securityAccessDenied);
|
||
}
|
||
|
||
|
||
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) {
|
||
|
||
if (!isSecurityAccessDenied(env,
|
||
uds_ReadDataByIdentifier_22_com_F1[dataIdentifier_lo].isPermissionSession)) {
|
||
return setResponseError(env, UDS_ReadDataByIdentifier, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
if ((uds_ReadDataByIdentifier_22_com_F1[dataIdentifier_lo].isSecurity) && (!env->SA.stateSecurityAccess)) {
|
||
return setResponseError(env, UDS_ReadDataByIdentifier, UDS_error_securityAccessDenied);
|
||
}
|
||
|
||
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) {
|
||
|
||
|
||
if (!isSecurityAccessDenied(env,
|
||
uds_WriteDataByIdentifier_2E_com_CF[dataIdentifier_lo].isPermissionSession)) {
|
||
return setResponseError(env, UDS_WriteDataByIdentifier, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
if ((uds_WriteDataByIdentifier_2E_com_CF[dataIdentifier_lo].isSecurity) && (!env->SA.stateSecurityAccess)) {
|
||
return setResponseError(env, UDS_WriteDataByIdentifier, UDS_error_securityAccessDenied);
|
||
}
|
||
|
||
|
||
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;
|
||
|
||
// CCU_Configuration
|
||
if (dataIdentifier_lo == 0) {
|
||
tStatus_CCU_Configuration *CCU_Configuration = (tStatus_CCU_Configuration *) &env->data.data[3];
|
||
if ((CCU_Configuration->AromaConfiguration > 1) || (CCU_Configuration->AlgorithmConfiguration > 1) ||
|
||
(CCU_Configuration->RearHVACConfiguration > 1)) {
|
||
return setResponseError(env, UDS_WriteDataByIdentifier,
|
||
UDS_error_requestOutOfRange);
|
||
}
|
||
}
|
||
|
||
memcpy(uds_WriteDataByIdentifier_2E_com_CF[dataIdentifier_lo].data, &env->data.data[3], size);
|
||
|
||
SaveToStorageFromStatusData(env->deviceStorage, &statusData.ecu);
|
||
return 3;
|
||
}
|
||
|
||
}
|
||
|
||
if (dataIdentifier_hi == 0xF1) {
|
||
|
||
if (uds_WriteDataByIdentifier_2E_com_F1[dataIdentifier_lo].data != NULL) {
|
||
|
||
if (!isSecurityAccessDenied(env,
|
||
uds_WriteDataByIdentifier_2E_com_F1[dataIdentifier_lo].isPermissionSession)) {
|
||
return setResponseError(env, UDS_WriteDataByIdentifier, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
if ((uds_WriteDataByIdentifier_2E_com_F1[dataIdentifier_lo].isSecurity) && (!env->SA.stateSecurityAccess)) {
|
||
return setResponseError(env, UDS_WriteDataByIdentifier, UDS_error_securityAccessDenied);
|
||
}
|
||
|
||
|
||
uint16_t size = uds_WriteDataByIdentifier_2E_com_F1[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;
|
||
|
||
|
||
// Tester_Fingerprint
|
||
if (dataIdentifier_lo == 0x5A) {
|
||
tFingerprint *fingerprint = (tFingerprint *) &env->data.data[3];
|
||
if (is_valid_day(fingerprint->year, fingerprint->month, fingerprint->day) == false) {
|
||
return setResponseError(env, UDS_WriteDataByIdentifier,
|
||
UDS_error_requestOutOfRange);
|
||
}
|
||
}
|
||
|
||
|
||
memcpy(uds_WriteDataByIdentifier_2E_com_F1[dataIdentifier_lo].data, &env->data.data[3], size);
|
||
|
||
SaveToStorageFromStatusData(env->deviceStorage, &statusData.ecu);
|
||
return 3;
|
||
}
|
||
|
||
}
|
||
|
||
return setResponseError(env, UDS_WriteDataByIdentifier, UDS_error_requestOutOfRange);
|
||
|
||
}
|
||
// конец --------------------------- Запись ---------------------------------------------------------
|
||
// конец --------------------------- Запись ---------------------------------------------------------
|
||
// конец --------------------------- Запись ---------------------------------------------------------
|
||
|
||
|
||
|
||
// начало --------------------------- Управление ---------------------------------------------------------
|
||
// начало --------------------------- Управление ---------------------------------------------------------
|
||
// начало --------------------------- Управление ---------------------------------------------------------
|
||
|
||
static uint16_t InputOutputControlByIdentifier_2F(tCanUds *env) {
|
||
|
||
if (env->data.len < 4) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier,
|
||
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 ((env->data.data[3] != UDS_io_returnControlToECU) &&
|
||
(env->data.data[3] != UDS_io_shortTermAdjustment)) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier, UDS_error_sub_functionNotSupported);
|
||
}
|
||
|
||
if (dataIdentifier_hi == 0xCF) {
|
||
|
||
if (uds_IoDataByIdentifier_2F_com_CF[dataIdentifier_lo].data != NULL) {
|
||
|
||
if (!isSecurityAccessDenied(env, uds_IoDataByIdentifier_2F_com_CF[dataIdentifier_lo].isPermissionSession)) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
if ((uds_IoDataByIdentifier_2F_com_CF[dataIdentifier_lo].isSecurity) && (!env->SA.stateSecurityAccess)) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier, UDS_error_securityAccessDenied);
|
||
}
|
||
|
||
uint16_t size = uds_IoDataByIdentifier_2F_com_CF[dataIdentifier_lo].size;
|
||
|
||
if ((env->data.data[3] == UDS_io_shortTermAdjustment) && (env->data.len != size + 4)) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier,
|
||
UDS_error_incorrectMessageLengthOrInvalidFormat);
|
||
}
|
||
|
||
env->dataResponse[0] = UDS_InputOutputControlByIdentifier | 0b1000000;
|
||
env->dataResponse[1] = dataIdentifier_hi;
|
||
env->dataResponse[2] = dataIdentifier_lo;
|
||
env->dataResponse[3] = env->data.data[3];
|
||
|
||
if (env->data.data[3] == UDS_io_shortTermAdjustment) {
|
||
|
||
// Motor_Def...MotorSealingValve (percent)
|
||
if ((dataIdentifier_lo >= 0x60) && (dataIdentifier_lo <= 0x78)) {
|
||
uint16_t percent = (env->data.data[4] << 8) | env->data.data[5];
|
||
|
||
if (percent > 1000) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier, UDS_error_requestOutOfRange);
|
||
}
|
||
|
||
memcpy(uds_IoDataByIdentifier_2F_com_CF[dataIdentifier_lo].data, &env->data.data[4], size);
|
||
return size + 4;
|
||
} else {
|
||
|
||
switch (dataIdentifier_lo) {
|
||
|
||
// Doors Actuator
|
||
case 0x50: {
|
||
for (uint8_t i = 0; i < 24; i += 2) {
|
||
uint16_t percent = (env->data.data[4 + i] << 8) | env->data.data[5 + 1 + i];
|
||
if (percent > 1000) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier,
|
||
UDS_error_requestOutOfRange);
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
|
||
// IO
|
||
case 0x51: {
|
||
for (uint8_t i = 0; i < 8; ++i) {
|
||
uint8_t on_off = env->data.data[4 + i];
|
||
if (on_off > 1) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier,
|
||
UDS_error_requestOutOfRange);
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
|
||
// PWM
|
||
case 0x52: {
|
||
|
||
tStatus_Pwm *pwm = (tStatus_Pwm *) &env->data.data[4];
|
||
if ((pwm->Pwm_Front > 1000) || (pwm->Pwm_Rear > 1000)) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier,
|
||
UDS_error_requestOutOfRange);
|
||
}
|
||
|
||
break;
|
||
}
|
||
|
||
// eComp
|
||
case 0x53: {
|
||
|
||
tStatus_eComp *eComp = (tStatus_eComp *) &env->data.data[4];
|
||
if ((eComp->eComp_Statue_Request > 1) || (eComp->LowTempValve > 1)) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier,
|
||
UDS_error_requestOutOfRange);
|
||
}
|
||
|
||
break;
|
||
}
|
||
|
||
// External
|
||
case 0x54: {
|
||
|
||
tStatus_External *external = (tStatus_External *) &env->data.data[4];
|
||
if (external->CoolingFan > 1000) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier,
|
||
UDS_error_requestOutOfRange);
|
||
}
|
||
|
||
break;
|
||
}
|
||
|
||
// EMS
|
||
case 0x55: {
|
||
|
||
tStatus_EMS *ems = (tStatus_EMS *) &env->data.data[4];
|
||
if (ems->EMS_Chiller_Req > 1) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier,
|
||
UDS_error_requestOutOfRange);
|
||
}
|
||
|
||
break;
|
||
}
|
||
|
||
// Ionizer_Aroma
|
||
case 0x56: {
|
||
|
||
tStatus_Ionizer_Aroma *ionizer_Aroma = (tStatus_Ionizer_Aroma *) &env->data.data[4];
|
||
if ((ionizer_Aroma->Ionizer > 1) || (ionizer_Aroma->AromatizationIntensivity > 3)) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier,
|
||
UDS_error_requestOutOfRange);
|
||
}
|
||
|
||
break;
|
||
}
|
||
|
||
// Duct Select
|
||
case 0x79: {
|
||
|
||
tStatus_Duct_Select *duct_Select = (tStatus_Duct_Select *) &env->data.data[4];
|
||
if ((duct_Select->Duct_FL > 1) || (duct_Select->Duct_FR > 1) ||
|
||
(duct_Select->Duct_RL > 1) || (duct_Select->Duct_RR > 1)) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier,
|
||
UDS_error_requestOutOfRange);
|
||
}
|
||
|
||
break;
|
||
}
|
||
|
||
}
|
||
|
||
}
|
||
|
||
memcpy(uds_IoDataByIdentifier_2F_com_CF[dataIdentifier_lo].data, &env->data.data[4], size);
|
||
|
||
return size + 4;
|
||
}
|
||
|
||
|
||
return 4;
|
||
}
|
||
|
||
}
|
||
/*
|
||
if (dataIdentifier_hi == 0xF1) {
|
||
|
||
if (uds_IoDataByIdentifier_2F_com_F1[dataIdentifier_lo].data != NULL) {
|
||
|
||
|
||
if (!isSecurityAccessDenied(env, uds_IoDataByIdentifier_2F_com_F1[dataIdentifier_lo].isPermissionSession)) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
if ((uds_IoDataByIdentifier_2F_com_F1[dataIdentifier_lo].isSecurity) && (!env->SA.stateSecurityAccess)) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier, UDS_error_securityAccessDenied);
|
||
}
|
||
|
||
uint16_t size = uds_IoDataByIdentifier_2F_com_F1[dataIdentifier_lo].size;
|
||
|
||
if (env->data.len != size + 4) {
|
||
return setResponseError(env, UDS_InputOutputControlByIdentifier,
|
||
UDS_error_incorrectMessageLengthOrInvalidFormat);
|
||
}
|
||
|
||
env->dataResponse[0] = UDS_InputOutputControlByIdentifier | 0b1000000;
|
||
env->dataResponse[1] = dataIdentifier_hi;
|
||
env->dataResponse[2] = dataIdentifier_lo;
|
||
env->dataResponse[3] = env->data.data[3];
|
||
//memcpy(uds_IoDataByIdentifier_2F_com_F1[dataIdentifier_lo].data, &env->data.data[3], size);
|
||
|
||
return 4;
|
||
}
|
||
|
||
}
|
||
*/
|
||
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) &&
|
||
(com->sub_function != UDS_reset_rapidReset)) {
|
||
return setResponseError(env, UDS_ECUResetService, UDS_error_sub_functionNotSupported);
|
||
}
|
||
|
||
if ((env->currentSession == UDS_session_programmingSession) &&
|
||
((com->sub_function == UDS_reset_softReset) || (com->sub_function == UDS_reset_rapidReset))) {
|
||
return setResponseError(env, UDS_ECUResetService, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
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);
|
||
}
|
||
|
||
if (env->currentSession == UDS_session_programmingSession) {
|
||
return setResponseError(env, UDS_ClearDiagnosticInformation, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
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);
|
||
}
|
||
|
||
if (env->currentSession == UDS_session_programmingSession) {
|
||
return setResponseError(env, UDS_ReadDTCInformation, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
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);
|
||
}
|
||
|
||
|
||
if ((env->currentSession == UDS_session_defaultSession) ||
|
||
(env->currentSession == UDS_session_programmingSession)) {
|
||
return setResponseError(env, UDS_Communication_Control, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
|
||
if (com->sub_function == UDS_sub_enableRxAndTx) {
|
||
set_CCU_Mute(env->canSpamTransmitter, false);
|
||
} else {
|
||
set_CCU_Mute(env->canSpamTransmitter, true);
|
||
}
|
||
|
||
env->dataResponse[0] = UDS_Communication_Control | 0b1000000;
|
||
env->dataResponse[1] = env->data.data[1] & 0b01111111; // sub-function
|
||
|
||
return 2;
|
||
}
|
||
// конец --------------------------- Молчание -------------------------------------------------------------
|
||
// конец --------------------------- Молчание -------------------------------------------------------------
|
||
// конец --------------------------- Молчание -------------------------------------------------------------
|
||
|
||
|
||
|
||
// начало --------------------------- Заморозка изменения DTC -------------------------------------------------------------
|
||
// начало --------------------------- Заморозка изменения DTC -------------------------------------------------------------
|
||
// начало --------------------------- Заморозка изменения DTC -------------------------------------------------------------
|
||
|
||
static uint16_t ControlDTCSetting_85(tCanUds *env) {
|
||
|
||
tUdsServiceCommand *com = (tUdsServiceCommand *) env->data.data;
|
||
|
||
if (env->data.len < 2) {
|
||
return setResponseError(env, UDS_ControlDTCSetting,
|
||
UDS_error_incorrectMessageLengthOrInvalidFormat);
|
||
}
|
||
|
||
if ((com->sub_function != UDS_sub_dtc_on) &&
|
||
(com->sub_function != UDS_sub_dtc_off)) {
|
||
return setResponseError(env, UDS_ControlDTCSetting, UDS_error_sub_functionNotSupported);
|
||
}
|
||
|
||
if ((env->currentSession == UDS_session_defaultSession) ||
|
||
(env->currentSession == UDS_session_programmingSession)) {
|
||
return setResponseError(env, UDS_ControlDTCSetting, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
if (com->sub_function == UDS_sub_dtc_on) {
|
||
setNoBitsDTC(env->Diagnostic, false);
|
||
} else {
|
||
setNoBitsDTC(env->Diagnostic, true); // off - заморожены
|
||
}
|
||
|
||
env->dataResponse[0] = UDS_ControlDTCSetting | 0b1000000;
|
||
env->dataResponse[1] = env->data.data[1] & 0b01111111; // sub-function
|
||
|
||
return 2;
|
||
}
|
||
// конец --------------------------- Заморозка изменения DTC -------------------------------------------------------------
|
||
// конец --------------------------- Заморозка изменения DTC -------------------------------------------------------------
|
||
// конец --------------------------- Заморозка изменения DTC -------------------------------------------------------------
|
||
|
||
|
||
// начало --------------------------- Функции -------------------------------------------------------------
|
||
// начало --------------------------- Функции -------------------------------------------------------------
|
||
// начало --------------------------- Функции -------------------------------------------------------------
|
||
|
||
static uint16_t vUDS_check_Programming_Preconditions(tCanUds *env, eUdsRoutineControlType udsRoutineControlType) {
|
||
|
||
eUdsRoutineStartStopRoutineResult udsStartStopRoutineResult = UDS_routine_RoutineStartStopSuccess;
|
||
eUdsRequestRoutineResult udsRequestRoutineResult = UDS_routine_RoutineFinishSuccess;
|
||
|
||
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_VIN_learn(tCanUds *env, eUdsRoutineControlType udsRoutineControlType) {
|
||
|
||
eUdsRoutineStartStopRoutineResult udsStartStopRoutineResult = UDS_routine_RoutineStartStopSuccess;
|
||
eUdsRequestRoutineResult udsRequestRoutineResult = UDS_routine_RoutineFinishSuccess;
|
||
|
||
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_RoutineFinishSuccess;
|
||
|
||
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;
|
||
}
|
||
|
||
uint8_t zzz[2048];
|
||
|
||
static uint16_t vUDS_routine_Erase_Memory(tCanUds *env, eUdsRoutineControlType udsRoutineControlType) {
|
||
|
||
eUdsRoutineStartStopRoutineResult udsStartStopRoutineResult = UDS_routine_RoutineStartStopSuccess;
|
||
eUdsRequestRoutineResult udsRequestRoutineResult = UDS_routine_RoutineFinishSuccess;
|
||
|
||
if ((udsRoutineControlType == UDS_routine_StartRoutine) || (udsRoutineControlType == UDS_routine_StopRoutine)) {
|
||
|
||
bool result = env->clear_flash_func(0x01100000, 512 * 1024);
|
||
|
||
|
||
// bool result = FLASHDRIVER_FlashEraseBlock(FLASH_BLOCK_SELECT1);
|
||
|
||
uint32_t adr = 0x01100000;
|
||
for (uint32_t i = 0; i < 256; ++i) {
|
||
env->write_flash_func(adr, zzz, 2048);
|
||
adr += 2048;
|
||
}
|
||
|
||
if (result) {
|
||
env->dataResponse[4] = UDS_routine_RoutineStartStopSuccess;
|
||
} else {
|
||
env->dataResponse[4] = UDS_routine_RoutineStartStopFailure;
|
||
}
|
||
}
|
||
|
||
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_RoutineFinishSuccess;
|
||
|
||
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_eComp_Initialize(tCanUds *env, eUdsRoutineControlType udsRoutineControlType) {
|
||
|
||
eUdsRoutineStartStopRoutineResult udsStartStopRoutineResult = UDS_routine_RoutineStartStopSuccess;
|
||
eUdsRequestRoutineResult udsRequestRoutineResult = UDS_routine_RoutineFinishSuccess;
|
||
|
||
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[2];
|
||
uint8_t routineIdentifier_lo = env->data.data[3];
|
||
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] = env->data.data[1]; // eUdsRoutineControlType
|
||
env->dataResponse[2] = routineIdentifier_hi;
|
||
env->dataResponse[3] = routineIdentifier_lo;
|
||
|
||
switch (routineIdentifier) {
|
||
case Check_Programming_Preconditions: {
|
||
|
||
if ((env->currentSession == UDS_session_defaultSession) ||
|
||
(env->currentSession == UDS_session_programmingSession)) {
|
||
return setResponseError(env, UDS_RoutineControl, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
if (!env->SA.stateSecurityAccess) {
|
||
return setResponseError(env, UDS_RoutineControl, UDS_error_securityAccessDenied);
|
||
}
|
||
|
||
return vUDS_check_Programming_Preconditions(env, udsRoutineControlType);
|
||
}
|
||
case UDS_routine_VIN_learn: {
|
||
|
||
if ((env->currentSession == UDS_session_defaultSession) ||
|
||
(env->currentSession == UDS_session_programmingSession)) {
|
||
return setResponseError(env, UDS_RoutineControl, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
if (!env->SA.stateSecurityAccess) {
|
||
return setResponseError(env, UDS_RoutineControl, UDS_error_securityAccessDenied);
|
||
}
|
||
|
||
return vUDS_routine_VIN_learn(env, udsRoutineControlType);
|
||
}
|
||
case UDS_routine_Compare_Checksum: {
|
||
|
||
if ((env->currentSession == UDS_session_defaultSession) ||
|
||
(env->currentSession == UDS_session_extendedDiagnosticSession)) {
|
||
return setResponseError(env, UDS_RoutineControl, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
if (!env->SA.stateSecurityAccess) {
|
||
return setResponseError(env, UDS_RoutineControl, UDS_error_securityAccessDenied);
|
||
}
|
||
|
||
return vUDS_routine_Compare_Checksum(env, udsRoutineControlType);
|
||
}
|
||
case UDS_routine_Erase_Memory: {
|
||
|
||
if ((env->currentSession == UDS_session_defaultSession) ||
|
||
(env->currentSession == UDS_session_extendedDiagnosticSession)) {
|
||
return setResponseError(env, UDS_RoutineControl, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
if (!env->SA.stateSecurityAccess) {
|
||
return setResponseError(env, UDS_RoutineControl, UDS_error_securityAccessDenied);
|
||
}
|
||
|
||
return vUDS_routine_Erase_Memory(env, udsRoutineControlType);
|
||
}
|
||
case UDS_routine_Check_Programming_Dependancies: {
|
||
|
||
if (env->currentSession == UDS_session_extendedDiagnosticSession) {
|
||
return setResponseError(env, UDS_RoutineControl, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
if (!env->SA.stateSecurityAccess) {
|
||
return setResponseError(env, UDS_RoutineControl, UDS_error_securityAccessDenied);
|
||
}
|
||
|
||
return vUDS_routine_Check_Programming_Dependancies(env, udsRoutineControlType);
|
||
}
|
||
case UDS_routine_eComp_Initialize: {
|
||
|
||
if ((env->currentSession == UDS_session_defaultSession) ||
|
||
(env->currentSession == UDS_session_programmingSession)) {
|
||
return setResponseError(env, UDS_RoutineControl, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
if (!env->SA.stateSecurityAccess) {
|
||
return setResponseError(env, UDS_RoutineControl, UDS_error_securityAccessDenied);
|
||
}
|
||
|
||
return vUDS_routine_eComp_Initialize(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->currentSession == UDS_session_defaultSession) {
|
||
return setResponseError(env, UDS_SecurityAccess, UDS_error_conditionsNotCorrect);
|
||
}
|
||
|
||
// Отправляется, если таймер задержки активен и получен запрос
|
||
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 {
|
||
|
||
env->randomSecuritySeed = getRandom32(env->adcTask0);
|
||
|
||
if (env->randomSecuritySeed == 0) {
|
||
env->randomSecuritySeed = 0x11223344;
|
||
}
|
||
|
||
env->dataResponse[2] = env->randomSecuritySeed >> 24;
|
||
env->dataResponse[3] = env->randomSecuritySeed >> 16;
|
||
env->dataResponse[4] = env->randomSecuritySeed >> 8;
|
||
env->dataResponse[5] = env->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(env->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, ""},
|
||
{ControlDTCSetting_85, "ControlDTCSetting_85"},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{NULL, ""},
|
||
{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);
|
||
|
||
// Контроль TesterPresent
|
||
if ((env->currentSessionTesterPresentTimeout > 0) &&
|
||
(env->currentSessionTesterPresentTimeout < SystemGetMs())) {
|
||
env->currentSessionTesterPresentTimeout = 0;
|
||
|
||
#if (LOG_UDS == 1)
|
||
LoggerFormatInfo(LOGGER, LOG_SIGN, "Resetting the session to default: %d", UDS_session_defaultSession)
|
||
#endif
|
||
setDefaultSecurityAccess(env);
|
||
set_CCU_Mute(env->canSpamTransmitter, false);
|
||
setNoBitsDTC(env->Diagnostic, false);
|
||
env->currentSession = UDS_session_defaultSession;
|
||
statusData.Status_Active_Diagnostic_Session.Active_Diagnostic_Session = env->currentSession;
|
||
|
||
}
|
||
|
||
if (status == osOK) {
|
||
|
||
uint8_t com = env->data.data[0];
|
||
|
||
if (uds_com[com].func != NULL) {
|
||
|
||
#if (LOG_UDS == 1)
|
||
sendLogCanUdsHex(env, env->data.data, env->data.len);
|
||
LoggerFormatInfo(LOGGER, LOG_SIGN, "> %s [%d] %s", uds_com[com].desc, env->data.len, env->hexString)
|
||
#endif
|
||
uint8_t response_size = uds_com[com].func(env);
|
||
|
||
if (response_size) {
|
||
#if (LOG_UDS == 1)
|
||
sendLogCanUdsHex(env, env->dataResponse, response_size);
|
||
LoggerFormatInfo(LOGGER, LOG_SIGN, "< %s [%d] %s", uds_com[com].desc, response_size, env->hexString)
|
||
#endif
|
||
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,
|
||
tCanSpamTransmitter *canSpamTransmitter,
|
||
tAdcTask *adcTask0,
|
||
uds_clear_flash_func *clear_flash_func,
|
||
uds_write_flash_func *write_flash_func,
|
||
tLoggerInterface *logger) {
|
||
|
||
env->logger = logger;
|
||
env->CanIO = CanIO;
|
||
env->deviceStorage = deviceStorage;
|
||
env->Diagnostic = Diagnostic;
|
||
env->adcTask0 = adcTask0;
|
||
env->filterIdCount = 2;
|
||
env->canSpamTransmitter = canSpamTransmitter;
|
||
env->clear_flash_func = clear_flash_func;
|
||
env->write_flash_func = write_flash_func;
|
||
env->queue = osMessageQueueNew(CAN_US_QUEUE_SIZE, sizeof(tCanTP_data), NULL);
|
||
|
||
setDefaultStatus();
|
||
|
||
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] = Diag_From_CCU_CANID;
|
||
env->filterRespId[1] = Diag_From_CCU_CANID;
|
||
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] = 0;
|
||
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;
|
||
statusData.Status_Active_Diagnostic_Session.Active_Diagnostic_Session = UDS_session_defaultSession;
|
||
|
||
InitThreadBlock(env->T_can_Uds, "CanUds", osPriorityNormal);
|
||
}; |