706 lines
30 KiB
C
706 lines
30 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"
|
|
|
|
#define LOG_SIGN "CAN_UDS"
|
|
#define LOGGER env->logger
|
|
|
|
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")
|
|
}
|
|
|
|
}
|
|
|
|
uint16_t setResponseError(tCanUds *env, eUdsServices service, eUdsResponseError error) {
|
|
env->dataResponse[0] = 0x7F;
|
|
env->dataResponse[1] = service;
|
|
env->dataResponse[2] = error;
|
|
|
|
return 3;
|
|
}
|
|
|
|
// начало --------------------------- ПИНГ ---------------------------------------------------------
|
|
// начало --------------------------- ПИНГ ---------------------------------------------------------
|
|
// начало --------------------------- ПИНГ ---------------------------------------------------------
|
|
|
|
static uint16_t TesterPresent_3E(tCanUds *env) {
|
|
|
|
tUdsServiceCommand *com = (tUdsServiceCommand *) env->data.data;
|
|
|
|
if (com->service < 2) {
|
|
return setResponseError(env, UDS_TesterPresent, UDS_error_incorrectMessageLengthOrInvalidFormat);
|
|
}
|
|
|
|
tTesterPresent testerPresent;
|
|
testerPresent.ServiceId = UDS_TesterPresent | 0b1000000;
|
|
testerPresent.zeroSubFunction = 0;
|
|
|
|
return 2;
|
|
}
|
|
// конец --------------------------- ПИНГ ---------------------------------------------------------
|
|
// конец --------------------------- ПИНГ ---------------------------------------------------------
|
|
// конец --------------------------- ПИНГ ---------------------------------------------------------
|
|
|
|
// начало ----------------------------- Сессия -------------------------------------------------------------
|
|
// начало ----------------------------- Сессия -------------------------------------------------------------
|
|
// начало ----------------------------- Сессия -------------------------------------------------------------
|
|
static uint16_t DiagnosticSessionControl_10(tCanUds *env) {
|
|
tUdsServiceCommand *com = (tUdsServiceCommand *) env->data.data;
|
|
|
|
if (com->service < 2) {
|
|
return setResponseError(env, UDS_DiagnosticSessionControl, UDS_error_incorrectMessageLengthOrInvalidFormat);
|
|
}
|
|
|
|
if ((com->sub_function != UDS_session_defaultSession) &&
|
|
(com->sub_function != UDS_session_programmingSession) &&
|
|
(com->sub_function != UDS_session_extendedDiagnosticSession)) {
|
|
return setResponseError(env, UDS_DiagnosticSessionControl, UDS_error_sub_functionNotSupported);
|
|
}
|
|
|
|
tDiagnosticSessionsType *diagnosticSessionsType = (tDiagnosticSessionsType *) env->dataResponse;
|
|
diagnosticSessionsType->ServiceId = UDS_DiagnosticSessionControl | 0b1000000;
|
|
diagnosticSessionsType->diagnosticSessionType = env->data.data[1];
|
|
|
|
// Нормальный таймаут. Важно: Клиент может отправлять TesterPresent для поддержания сессии до истечения этого таймаута
|
|
diagnosticSessionsType->sessionParameterRecord[0] = 0;
|
|
diagnosticSessionsType->sessionParameterRecord[1] = 0x32;
|
|
|
|
// Расширенный таймаут. Например, ECU может обрабатывать сложный запрос 2 секунды и отправлять NRC 0x78, затем ответ
|
|
diagnosticSessionsType->sessionParameterRecord[2] = 0x01;
|
|
diagnosticSessionsType->sessionParameterRecord[3] = 0xF4;
|
|
|
|
return 4;
|
|
}
|
|
// конец ----------------------------- Сессия --------------------------------------------------------------
|
|
// конец ----------------------------- Сессия --------------------------------------------------------------
|
|
// конец ----------------------------- Сессия --------------------------------------------------------------
|
|
|
|
|
|
// начало --------------------------- Чтение ---------------------------------------------------------
|
|
// начало --------------------------- Чтение ---------------------------------------------------------
|
|
// начало --------------------------- Чтение ---------------------------------------------------------
|
|
static uint16_t ReadDataByIdentifier_22(tCanUds *env) {
|
|
|
|
if (env->data.len < 3) {
|
|
return setResponseError(env, UDS_ReadDataByIdentifier, UDS_error_incorrectMessageLengthOrInvalidFormat);
|
|
}
|
|
|
|
uint8_t dataIdentifier_hi = env->data.data[1];
|
|
uint8_t dataIdentifier_lo = env->data.data[2];
|
|
// uint16_t dataIdentifier = (dataIdentifier_hi << 8) | dataIdentifier_lo;
|
|
|
|
if (dataIdentifier_hi == 0xCF) {
|
|
|
|
if (uds_ReadDataByIdentifier_22_com_CF[dataIdentifier_lo].data != NULL) {
|
|
uint16_t response_size = uds_ReadDataByIdentifier_22_com_CF[dataIdentifier_lo].size;
|
|
|
|
env->dataResponse[0] = UDS_ReadDataByIdentifier | 0b1000000;
|
|
env->dataResponse[1] = dataIdentifier_hi;
|
|
env->dataResponse[2] = dataIdentifier_lo;
|
|
memcpy(&env->dataResponse[3], uds_ReadDataByIdentifier_22_com_CF[dataIdentifier_lo].data, response_size);
|
|
|
|
return response_size;
|
|
}
|
|
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
}
|
|
|
|
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 != enableRxAndTx) &&
|
|
(com->sub_function != 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;
|
|
}
|
|
// конец --------------------------- Молчание -------------------------------------------------------------
|
|
// конец --------------------------- Молчание -------------------------------------------------------------
|
|
// конец --------------------------- Молчание -------------------------------------------------------------
|
|
|
|
const eUds_com uds_com[256] = {
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{DiagnosticSessionControl_10, "DiagnosticSessionControl_10"},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{ECUReset_11, "ECUReset_11"},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{UDS_ClearDiagnosticInformation_14, "UDS_ClearDiagnosticInformation_14"},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{UDS_ReadDTCInformation_19, "UDS_ReadDTCInformation_19"},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{ReadDataByIdentifier_22, "ReadDataByIdentifier_22"},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{CommunicationControl_28, "CommunicationControl_28"},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{WriteDataByIdentifier_2E, "WriteDataByIdentifier_2E"},
|
|
{InputOutputControlByIdentifier_2F, "InputOutputControlByIdentifier_2F"},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{TesterPresent_3E, "TesterPresent_3E"},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""},
|
|
{NULL, ""}
|
|
};
|
|
|
|
|
|
void CanUds(tCanUds *env) {
|
|
|
|
for (;;) {
|
|
osStatus_t status = osMessageQueueGet(env->queue, &env->data, 0, 1000);
|
|
|
|
if (status == osOK) {
|
|
|
|
uint8_t com = env->data.data[0];
|
|
|
|
if (uds_com[com].func != NULL) {
|
|
|
|
uint8_t response_size = uds_com[com].func(env);
|
|
|
|
if (response_size) {
|
|
|
|
CanSerialPortFrameTpTransmit(&env->canSerialPortFrameTp, env->dataResponse,
|
|
response_size, PROTOCOL_CAN_ADR_UDS, WAIT_FRAME_WRITE);
|
|
}
|
|
|
|
} else {
|
|
asm("nop");
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
void CanSerialPortCanUds_Start(tCanUds *env) {
|
|
ThreadBlock_Start(env->T_can_Uds, env, CanUds);
|
|
CanSerialPortFrameTp_Start(&env->canSerialPortFrameTp);
|
|
}
|
|
|
|
void CanUds_Init(
|
|
tCanUds *env,
|
|
tDiagnostic *Diagnostic,
|
|
tSerialPortFrameIO *CanIO,
|
|
tDeviceStorage *deviceStorage,
|
|
tLoggerInterface *logger) {
|
|
|
|
env->logger = logger;
|
|
env->CanIO = CanIO;
|
|
env->deviceStorage = deviceStorage;
|
|
env->Diagnostic = Diagnostic;
|
|
env->filterIdCount = 0;
|
|
|
|
env->queue = osMessageQueueNew(CAN_US_QUEUE_SIZE, sizeof(tCanTP_data), NULL);
|
|
|
|
env->filterReqId[0] = 0;
|
|
env->filterReqId[1] = 0;
|
|
env->filterReqId[2] = 0;
|
|
env->filterReqId[3] = 0;
|
|
env->filterReqId[4] = 0;
|
|
env->filterReqId[5] = 0;
|
|
env->filterReqId[6] = 0;
|
|
|
|
env->filterRespId[0] = 0;
|
|
env->filterRespId[1] = 0;
|
|
env->filterRespId[2] = 0;
|
|
env->filterRespId[3] = 0;
|
|
env->filterRespId[4] = 0;
|
|
env->filterRespId[5] = 0;
|
|
env->filterRespId[6] = 0;
|
|
|
|
|
|
env->filterDirReq[0] = 0;
|
|
env->filterDirReq[1] = 0;
|
|
env->filterDirReq[2] = 0;
|
|
env->filterDirReq[3] = 0;
|
|
env->filterDirReq[4] = 0;
|
|
env->filterDirReq[5] = 1;
|
|
env->filterDirReq[6] = 0;
|
|
|
|
|
|
CanSerialPortFrameTpInit(
|
|
&env->canSerialPortFrameTp,
|
|
|
|
ReceivedCan_func,
|
|
env,
|
|
|
|
ReceivedTP_func,
|
|
env,
|
|
|
|
env->CanIO,
|
|
(tCanTP_data *) &env->canTP_Ext_data,
|
|
sizeof(env->canTP_Ext_data.data),
|
|
env->logger,
|
|
|
|
env->filterIdCount,
|
|
env->filterReqId,
|
|
env->filterRespId,
|
|
env->filterDirReq
|
|
|
|
);
|
|
|
|
InitThreadBlock(env->T_can_Uds, "CanUds", osPriorityNormal);
|
|
}; |