mirror of
https://gitee.com/Vancouver2017/luban-lite.git
synced 2025-12-17 17:48:55 +00:00
312 lines
11 KiB
C
312 lines
11 KiB
C
/*
|
||
*
|
||
* SPDX-License-Identifier: Apache-2.0
|
||
*
|
||
*/
|
||
|
||
#include "uds_def.h"
|
||
//Negative Response
|
||
#define IS_NEGATIVE_RESPONSE(frame_buf) (frame_buf[0] == 0x7F)
|
||
//Positive Response
|
||
#define IS_POSITIVE_RESPONSE(frame_buf, service_id) (frame_buf[1] == (0x40 + (service_id)))
|
||
//eg：0x50 03
|
||
#define IS_RESPONSE_WITH_SUBFUNC(frame_buf, service_id, subfunc) \
|
||
(frame_buf[1] == (0x40 + (service_id)) && frame_buf[2] == (subfunc))
|
||
|
||
//Check if it is a negative response for a specific service
|
||
#define IS_NEGATIVE_RESPONSE_FOR_SERVICE(frame_buf, service_id) \
|
||
(IS_NEGATIVE_RESPONSE(frame_buf) && frame_buf[1] == (service_id))
|
||
|
||
#define UDS_SERVICE_DIAG_SESSION_CTRL 0x10
|
||
#define UDS_SERVICE_ECU_RESET 0x11
|
||
#define UDS_SERVICE_READ_DATA 0x22
|
||
#define UDS_SERVICE_SECURITY_ACCESS 0x27
|
||
#define UDS_SERVICE_COMMUNICATION_CTRL 0x28
|
||
#define UDS_SERVICE_ROUTINE_CTRL 0x31
|
||
#define UDS_SERVICE_REQUEST_DOWNLOAD 0x34
|
||
#define UDS_SERVICE_REQUEST_UPLOAD 0x35
|
||
#define UDS_SERVICE_TRANSFER_DATA 0x36
|
||
#define UDS_SERVICE_REQUEST_TRANSFER_EXIT 0x37
|
||
#define UDS_SERVICE_WRITE_DATA 0x3E
|
||
#define UDS_SERVICE_CTRL_DTC 0x85
|
||
|
||
static rt_timer_t response_timer;
|
||
|
||
void response_timeout(void *param)
|
||
{
|
||
rt_kprintf("\n[UDS] ERROR: Response timeout!\n");
|
||
}
|
||
|
||
void init_uds_timers()
|
||
{
|
||
response_timer =
|
||
rt_timer_create("uds_tout", response_timeout, RT_NULL, 2000, RT_TIMER_FLAG_ONE_SHOT);
|
||
}
|
||
|
||
static uint8_t g_key[4] = { 0 };
|
||
typedef struct {
|
||
uint8_t data[8];
|
||
uint8_t len;
|
||
uint8_t next_state;
|
||
} UdsRequest;
|
||
|
||
static const UdsRequest uds_requests[] = {
|
||
[UDS_STATE_ENTER_EXT_SESSION] = {
|
||
.data = {0x03, 0x22, 0xF1, 0x86},
|
||
.len = 4,
|
||
.next_state = UDS_STATE_READ_ECU_INFO,
|
||
},
|
||
[UDS_STATE_READ_ECU_INFO] = {
|
||
.data = {0x02, 0x10, 0x02},
|
||
.len = 3,
|
||
.next_state = UDS_STATE_ENTER_PROG_SESSION,
|
||
},
|
||
[UDS_STATE_ENTER_PROG_SESSION] = {
|
||
.data = {0x02, 0x27, 0x01},
|
||
.len = 3,
|
||
.next_state = UDS_STATE_SAFE_ACCESS,
|
||
},
|
||
[UDS_STATE_REQUEST_UPLOAD_EXIT] = {
|
||
.data = {0x01, 0x37},
|
||
.len = 2,
|
||
.next_state = UDS_STATE_APP_CHECK,
|
||
}
|
||
};
|
||
|
||
#define UDS_STATE_COUNT (UDS_STATE_ENTER_DEFAULT_SESSION + 1)
|
||
void send_uds_service(void)
|
||
{
|
||
uint8_t uds_request[8] = { 0 };
|
||
uint8_t uds_request_len = 0;
|
||
|
||
if (g_current_state >= UDS_STATE_COUNT) {
|
||
rt_kprintf("[UDS] Invalid state: %d\n", g_current_state);
|
||
return;
|
||
}
|
||
|
||
const UdsRequest *req = &uds_requests[g_current_state];
|
||
|
||
if (g_current_state == UDS_STATE_SAFE_ACCESS) {
|
||
uds_request[0] = 0x06;
|
||
uds_request[1] = 0x27;
|
||
uds_request[2] = 0x02;
|
||
if (g_key[0] || g_key[1] || g_key[2] || g_key[3]) {
|
||
uds_request[3] = g_key[0];
|
||
uds_request[4] = g_key[1];
|
||
uds_request[5] = g_key[2];
|
||
uds_request[6] = g_key[3];
|
||
uds_request_len = 7;
|
||
g_current_state = UDS_STATE_REQUEST_UPLOAD_WAIT;
|
||
} else {
|
||
rt_kprintf("[UDS] Error: Key not initialized\n");
|
||
return;
|
||
}
|
||
} else {
|
||
if (req->len > 0 && req->len <= 8) {
|
||
memcpy(uds_request, req->data, req->len);
|
||
uds_request_len = req->len;
|
||
g_current_state = req->next_state;
|
||
} else {
|
||
rt_kprintf("[UDS] Invalid request length: %d\n", req->len);
|
||
return;
|
||
}
|
||
}
|
||
|
||
if (uds_request_len > 0) {
|
||
uds_send_frame(UDS_REQUEST_ID, uds_request, uds_request_len, 1);
|
||
} else {
|
||
rt_kprintf("[UDS] Error: Attempting to send empty frame\n");
|
||
}
|
||
}
|
||
|
||
int uds_calculate_key_direct(const uint8_t *frame_buf, uint8_t *key_buf)
|
||
{
|
||
if (frame_buf == NULL || key_buf == NULL) {
|
||
return -1;
|
||
}
|
||
uint8_t frame_len = frame_buf[0];
|
||
if (frame_len < 6) {
|
||
return -1;
|
||
}
|
||
if (frame_buf[1] != 0x67 || frame_buf[2] != 0x01) {
|
||
return -1;
|
||
}
|
||
|
||
uint32_t seed = (uint32_t)frame_buf[3] << 24 | (uint32_t)frame_buf[4] << 16 |
|
||
(uint32_t)frame_buf[5] << 8 | (uint32_t)frame_buf[6];
|
||
uint32_t calculated_key = ~seed;
|
||
key_buf[0] = (uint8_t)(calculated_key >> 24);
|
||
key_buf[1] = (uint8_t)(calculated_key >> 16);
|
||
key_buf[2] = (uint8_t)(calculated_key >> 8);
|
||
key_buf[3] = (uint8_t)calculated_key;
|
||
|
||
return 0;
|
||
}
|
||
|
||
/******************************************************************************
|
||
* 函数名称: static void handle_uds_state_machine(uint32_t id, uint8_t* frame_buf, uint8_t frame_dlc)
|
||
* 功能说明: UDS状态机处理
|
||
* 输入参数: uint32_t id --消息帧 ID
|
||
uint8_t* frame_buf --接收报文帧数据首地址
|
||
uint8_t frame_dlc --接收报文帧数据长度
|
||
* 输出参数: 无
|
||
* 函数返回: 无
|
||
* 其它说明: ecu响应，处理状态机
|
||
|
||
******************************************************************************/
|
||
static void handle_uds_state_machine(uint32_t id, uint8_t *frame_buf, uint8_t frame_dlc)
|
||
{
|
||
if (frame_dlc < 2) {
|
||
return;
|
||
}
|
||
rt_mutex_take(g_uds_state_mutex, RT_WAITING_FOREVER);
|
||
switch (g_current_state) {
|
||
case UDS_STATE_ENTER_EXT_SESSION:
|
||
if (IS_RESPONSE_WITH_SUBFUNC(frame_buf, UDS_SERVICE_DIAG_SESSION_CTRL, 0x03)) {
|
||
send_uds_service();
|
||
}
|
||
break;
|
||
case UDS_STATE_READ_ECU_INFO:
|
||
if (IS_POSITIVE_RESPONSE(frame_buf, UDS_SERVICE_READ_DATA)) {
|
||
send_uds_service();
|
||
}
|
||
break;
|
||
case UDS_STATE_ENTER_PROG_SESSION:
|
||
if (IS_RESPONSE_WITH_SUBFUNC(frame_buf, UDS_SERVICE_DIAG_SESSION_CTRL, 0x02)) {
|
||
send_uds_service();
|
||
}
|
||
break;
|
||
case UDS_STATE_SAFE_ACCESS:
|
||
if (IS_RESPONSE_WITH_SUBFUNC(frame_buf, UDS_SERVICE_SECURITY_ACCESS, 0x01)) {
|
||
// Calculate the key
|
||
uds_calculate_key_direct(frame_buf, g_key);
|
||
send_uds_service();
|
||
}
|
||
break;
|
||
case UDS_STATE_SAFE_ACCESS_SEND_VERIFY:
|
||
if (IS_RESPONSE_WITH_SUBFUNC(frame_buf, UDS_SERVICE_SECURITY_ACCESS, 0x02)) {
|
||
send_uds_service();
|
||
}
|
||
break;
|
||
case UDS_STATE_ERASE_CODE:
|
||
if (IS_RESPONSE_WITH_SUBFUNC(frame_buf, UDS_SERVICE_ROUTINE_CTRL, 0x01)) {
|
||
send_uds_service();
|
||
}
|
||
break;
|
||
case UDS_STATE_REQUEST_UPLOAD:
|
||
break;
|
||
case UDS_STATE_SEND_UPLOAD:
|
||
break;
|
||
case UDS_STATE_REQUEST_UPLOAD_EXIT:
|
||
break;
|
||
case UDS_STATE_APP_CHECK:
|
||
break;
|
||
case UDS_STATE_CODE_CHECK:
|
||
break;
|
||
case UDS_STATE_REQUEST_UPLOAD_WAIT:
|
||
g_current_state = UDS_STATE_REQUEST_UPLOAD;
|
||
break;
|
||
case UDS_STATE_WAIT_DOWNLOAD_RESPONSE:
|
||
if (frame_dlc >= 5 && IS_POSITIVE_RESPONSE(frame_buf, UDS_SERVICE_REQUEST_DOWNLOAD)) {
|
||
g_max_block_size = (frame_buf[3] << 8) | frame_buf[4];
|
||
g_bytes_per_block = (g_max_block_size > 0) ? g_max_block_size : 32;
|
||
g_current_state = UDS_STATE_SEND_UPLOAD;
|
||
} else if (IS_NEGATIVE_RESPONSE_FOR_SERVICE(frame_buf, UDS_SERVICE_REQUEST_DOWNLOAD)) {
|
||
g_current_state = UDS_STATE_ERROR;
|
||
} else {
|
||
uds_tp_recv_frame(0, frame_buf, frame_dlc);
|
||
}
|
||
break;
|
||
case UDS_STATE_WAIT_TRANSFER_RESPONSE:
|
||
if (IS_POSITIVE_RESPONSE(frame_buf, UDS_SERVICE_TRANSFER_DATA)) {
|
||
rt_sem_release(&g_flow_control_sem);
|
||
|
||
} else if (IS_NEGATIVE_RESPONSE_FOR_SERVICE(frame_buf, UDS_SERVICE_TRANSFER_DATA)) {
|
||
g_current_state = UDS_STATE_ERROR;
|
||
} else {
|
||
uds_tp_recv_frame(0, frame_buf, frame_dlc);
|
||
}
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
rt_mutex_release(g_uds_state_mutex);
|
||
}
|
||
|
||
/******************************************************************************
|
||
* 函数名称: void uds_recv_frame(uint32_t id, uint8_t* frame_buf, uint8_t frame_dlc)
|
||
* 功能说明: 接收到一帧报文
|
||
* 输入参数: uint32_t id --消息帧 ID
|
||
uint8_t* frame_buf --接收报文帧数据首地址
|
||
uint8_t frame_dlc --接收报文帧数据长度
|
||
* 输出参数: 无
|
||
* 函数返回: 无
|
||
* 其它说明: frame_dlc 长度必须等于 FRAME_SIZE，否则会被判断为无效帧
|
||
******************************************************************************/
|
||
void uds_recv_frame(uint32_t id, uint8_t *frame_buf, uint8_t frame_dlc)
|
||
{
|
||
if (UDS_REQUEST_ID == id)
|
||
uds_tp_recv_frame(0, frame_buf, frame_dlc);
|
||
else if (UDS_FUNCTION_ID == id)
|
||
uds_tp_recv_frame(1, frame_buf, frame_dlc);
|
||
else if (UDS_RESPONSE_ID == id)
|
||
handle_uds_state_machine(id, frame_buf, frame_dlc);
|
||
}
|
||
|
||
/******************************************************************************
|
||
* 函数名称: void uds_send_frame(uint32_t id, uint8_t* frame_buf, uint8_t frame_dlc)
|
||
* 功能说明: 发送一帧报文
|
||
* 输入参数: uint8_t response_id --应答 ID
|
||
uint8_t* frame_buf --发送报文帧数据首地址
|
||
uint8_t frame_dlc --发送报文帧数据长度
|
||
* 输出参数: 无
|
||
* 函数返回: 无
|
||
* 其它说明: frame_dlc 长度应当等于 FRAME_SIZE
|
||
******************************************************************************/
|
||
void uds_send_frame(uint32_t id, uint8_t *data, uint8_t len, uint8_t expect_response)
|
||
{
|
||
rt_size_t size;
|
||
struct rt_can_msg msg = { 0 };
|
||
msg.id = id;
|
||
msg.ide = (id > 0x7FF) ? 1 : 0;
|
||
msg.rtr = 0;
|
||
msg.len = len;
|
||
|
||
memcpy(msg.data, data, len);
|
||
size = rt_device_write(g_can_tx_dev, 0, &msg, sizeof(msg));
|
||
if (size != sizeof(msg)) {
|
||
rt_kprintf("[UDS] Error: Failed to send frame\n");
|
||
return;
|
||
}
|
||
if (expect_response) {
|
||
// rt_kprintf("[UDS] Waiting for response... (timeout=2000ms)\n");
|
||
}
|
||
}
|
||
|
||
/******************************************************************************
|
||
* 函数名称: void uds_init(void)
|
||
* 功能说明: UDS 初始化
|
||
* 输入参数: 无
|
||
* 输出参数: 无
|
||
* 函数返回: 无
|
||
* 其它说明: 无
|
||
******************************************************************************/
|
||
void uds_init(void)
|
||
{
|
||
service_init();
|
||
}
|
||
|
||
/******************************************************************************
|
||
* 函数名称: void uds_1ms_task(void)
|
||
* 功能说明: UDS 周期任务
|
||
* 输入参数: 无
|
||
* 输出参数: 无
|
||
* 函数返回: 无
|
||
* 其它说明: 该函数需要被 1ms 周期调用
|
||
******************************************************************************/
|
||
void uds_1ms_task(void)
|
||
{
|
||
network_task();
|
||
service_task();
|
||
}
|