diff --git a/CanUds.c b/CanUds.c index e395216..bf40961 100644 --- a/CanUds.c +++ b/CanUds.c @@ -9,110 +9,97 @@ #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 = 0x11223344; +uint32_t randomSecuritySeed = 0x853A08FB; -// Определение операций инверсии бит -#define INV_OP_A_MASK ((1UL << 6) | (1UL << 15) | (1UL << 17) | (1UL << 11) | \ - (1UL << 9) | (1UL << 16) | (1UL << 7) | (1UL << 10) | \ - (1UL << 13) | (1UL << 4)) +extern uint32_t generate_key(uint32_t seed); -#define INV_OP_B_MASK ((1UL << 14) | (1UL << 16) | (1UL << 11) | (1UL << 9) | \ - (1UL << 15) | (1UL << 7) | (1UL << 10) | (1UL << 13) | \ - (1UL << 4) | (1UL << 1)) -#define INV_OP_C_MASK ((1UL << 1) | (1UL << 7) | (1UL << 10) | (1UL << 13) | \ - (1UL << 4) | (1UL << 14) | (1UL << 16) | (1UL << 11) | \ - (1UL << 9) | (1UL << 15)) +static char strPrintfDebug_UDS[80]; -/** - * @brief Генерация ключа по сиду согласно алгоритму CCU - * @param seed Входное 32-битное значение сида - * @return 32-битный ключ - */ -static uint32_t generate_key_from_seed(uint32_t seed) { - // Шаг 1: Извлечение коэффициентов из сида - // Коэффициенты извлекаются из определенных битовых позиций (0 - LSB) +static void PrintfDebug(uint8_t *data, uint8_t dlc) { - // Коэффициент 1 (бит 20): направление сдвига (0=влево, 1=вправо) - uint8_t shift_direction = (seed >> 20) & 0x01; + switch (dlc) { - // Коэффициенты 2-5 (биты 7,11,5,8): количество сдвигов (4-битное число) - uint8_t shift_count = 0; - shift_count |= ((seed >> 7) & 0x01) << 3; // coefficient2 (MSB) - shift_count |= ((seed >> 11) & 0x01) << 2; // coefficient3 - shift_count |= ((seed >> 5) & 0x01) << 1; // coefficient4 - shift_count |= ((seed >> 8) & 0x01) << 0; // coefficient5 (LSB) - - // Коэффициенты 6-7 (биты 22,1): выбор операции инверсии (2 бита) - uint8_t inversion_operation = 0; - inversion_operation |= ((seed >> 22) & 0x01) << 1; // coefficient6 (MSB) - inversion_operation |= ((seed >> 1) & 0x01) << 0; // coefficient7 (LSB) - - // Коэффициенты 8-9 (биты 21,6): выбор битовой операции (2 бита) - uint8_t bitwise_operation = 0; - bitwise_operation |= ((seed >> 21) & 0x01) << 1; // coefficient8 (MSB) - bitwise_operation |= ((seed >> 6) & 0x01) << 0; // coefficient9 (LSB) - - // Шаг 2: Операция вращения (циклический сдвиг) - uint32_t intermediate1; - - if (shift_direction == 0) { - // Вращение влево - intermediate1 = (seed << shift_count) | (seed >> (32 - shift_count)); - } else { - // Вращение вправо - intermediate1 = (seed >> shift_count) | (seed << (32 - shift_count)); - } - - // Шаг 3: Операция инверсии бит - uint32_t intermediate2 = intermediate1; - - switch (inversion_operation) { - case 0x00: // Нет инверсии - // Ничего не делаем + case 0: + sprintf(strPrintfDebug_UDS, " "); break; - case 0x01: // Операция инверсии A - intermediate2 = intermediate1 ^ INV_OP_A_MASK; + case 1: + sprintf(strPrintfDebug_UDS, "%02X", data[0]); break; - case 0x02: // Операция инверсии B - intermediate2 = intermediate1 ^ INV_OP_B_MASK; + case 2: + sprintf(strPrintfDebug_UDS, "%02X:%02X", data[0], data[1]); break; - case 0x03: // Операция инверсии C - intermediate2 = intermediate1 ^ INV_OP_C_MASK; + 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: - // Недопустимая операция - оставляем как есть - break; + sprintf(strPrintfDebug_UDS, " "); } - // Шаг 4: Финальная битовая операция - uint32_t key; +} + + +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; + } - switch (bitwise_operation) { - case 0x00: // Key = intermediate2 - key = intermediate2; - break; - case 0x01: // Key = seed AND intermediate2 - key = seed & intermediate2; - break; - case 0x02: // Key = seed XOR intermediate2 - key = seed ^ intermediate2; - break; - case 0x03: // Key = seed OR intermediate2 - key = seed | intermediate2; - break; - default: - // Недопустимая операция - возвращаем intermediate2 - key = intermediate2; - break; } - return key; + 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) { @@ -156,7 +143,7 @@ static uint16_t TesterPresent_3E(tCanUds *env) { tUdsServiceCommand *com = (tUdsServiceCommand *) env->data.data; - if (com->service < 2) { + if (env->data.len < 2) { return setResponseError(env, UDS_TesterPresent, UDS_error_incorrectMessageLengthOrInvalidFormat); } @@ -176,7 +163,7 @@ static uint16_t TesterPresent_3E(tCanUds *env) { static uint16_t DiagnosticSessionControl_10(tCanUds *env) { tUdsServiceCommand *com = (tUdsServiceCommand *) env->data.data; - if (com->service < 2) { + if (env->data.len < 2) { return setResponseError(env, UDS_DiagnosticSessionControl, UDS_error_incorrectMessageLengthOrInvalidFormat); } @@ -203,7 +190,7 @@ static uint16_t DiagnosticSessionControl_10(tCanUds *env) { diagnosticSessionsType->sessionParameterRecord[2] = 0x01; diagnosticSessionsType->sessionParameterRecord[3] = 0xF4; - return 4; + return 6; } // конец ----------------------------- Сессия -------------------------------------------------------------- // конец ----------------------------- Сессия -------------------------------------------------------------- @@ -676,7 +663,7 @@ static uint16_t SecurityAccess_27(tCanUds *env) { } // Отправляется, если активирован таймер задержки из-за превышения максимального числа неудачных попыток доступа - if (env->SA.counter_max_attempts_default > MAX_ATTEMPTS_DEFAULT) { + 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); } @@ -684,7 +671,7 @@ static uint16_t SecurityAccess_27(tCanUds *env) { uint32_t securitySeedFromServer = (env->data.data[2] << 24) | (env->data.data[3] << 16) | (env->data.data[4] << 8) | env->data.data[5]; - uint32_t securitySeedMy = generate_key_from_seed(randomSecuritySeed); + uint32_t securitySeedMy = generate_key(randomSecuritySeed); if (securitySeedMy != securitySeedFromServer) { ++env->SA.counter_max_attempts_default; @@ -716,13 +703,13 @@ const eUds_com uds_com[256] = { {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, ""}, @@ -976,10 +963,16 @@ void CanUds(tCanUds *env) { 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); } @@ -1013,12 +1006,12 @@ void CanUds_Init( env->CanIO = CanIO; env->deviceStorage = deviceStorage; env->Diagnostic = Diagnostic; - env->filterIdCount = 0; + env->filterIdCount = 2; env->queue = osMessageQueueNew(CAN_US_QUEUE_SIZE, sizeof(tCanTP_data), NULL); - env->filterReqId[0] = 0; - env->filterReqId[1] = 0; + 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; diff --git a/CanUds.h b/CanUds.h index 630ef36..390628b 100644 --- a/CanUds.h +++ b/CanUds.h @@ -14,6 +14,8 @@ #define MAX_ATTEMPTS_DEFAULT 3 #define BLOCK_TIME_DEFAULT 60000 +#define LEN_DEBUG_UDS_BUFF 128 + // Время (логарифмическая шкала) // │ // ├── StMin (0.1-127 мс) ← Между КАДРАМИ @@ -136,6 +138,7 @@ typedef struct { uint32_t filterRespId[16]; uint8_t filterDirReq[16]; + char hexString[LEN_DEBUG_UDS_BUFF]; eUdsSession currentSession;