Обновление
This commit is contained in:
cfif
parent
42742bdc11
commit
ab72ce1271
183
CanUds.c
183
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)
|
||||
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, " ");
|
||||
}
|
||||
|
||||
// Коэффициенты 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;
|
||||
char *sendLogCanUdsHex(tCanUds *env, uint8_t *data, size_t size) {
|
||||
memset(env->hexString, 0, sizeof(env->hexString));
|
||||
size_t len = 0;
|
||||
|
||||
if (shift_direction == 0) {
|
||||
// Вращение влево
|
||||
intermediate1 = (seed << shift_count) | (seed >> (32 - shift_count));
|
||||
|
||||
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 {
|
||||
// Вращение вправо
|
||||
intermediate1 = (seed >> shift_count) | (seed << (32 - shift_count));
|
||||
return env->hexString;
|
||||
}
|
||||
|
||||
// Шаг 3: Операция инверсии бит
|
||||
uint32_t intermediate2 = intermediate1;
|
||||
|
||||
switch (inversion_operation) {
|
||||
case 0x00: // Нет инверсии
|
||||
// Ничего не делаем
|
||||
break;
|
||||
case 0x01: // Операция инверсии A
|
||||
intermediate2 = intermediate1 ^ INV_OP_A_MASK;
|
||||
break;
|
||||
case 0x02: // Операция инверсии B
|
||||
intermediate2 = intermediate1 ^ INV_OP_B_MASK;
|
||||
break;
|
||||
case 0x03: // Операция инверсии C
|
||||
intermediate2 = intermediate1 ^ INV_OP_C_MASK;
|
||||
break;
|
||||
default:
|
||||
// Недопустимая операция - оставляем как есть
|
||||
break;
|
||||
}
|
||||
|
||||
// Шаг 4: Финальная битовая операция
|
||||
uint32_t key;
|
||||
if (tail > 0) {
|
||||
PrintfDebug(&data[full * 8], tail);
|
||||
|
||||
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;
|
||||
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 key;
|
||||
}
|
||||
|
||||
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;
|
||||
|
|
|
|||
3
CanUds.h
3
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;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue