Eun0us/espilon-source
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
main
espilon-source/espilon_bot/components/mod_canbus/canbus_uds.c
Eun0us 6d45770d98 epsilon: merge command system into core + add 5 new modules 
Move command registry from components/command/ into components/core/.
New modules: mod_canbus, mod_honeypot, mod_fallback, mod_redteam, mod_ota.
Replace mod_proxy with tun_core (multiplexed SOCKS5 tunnel).
Kconfig extended with per-module settings and async worker config.
2026-02-28 20:07:59 +01:00

344 lines
11 KiB
C
Raw Permalink Blame History

/*
* canbus_uds.c
* UDS (ISO 14229) diagnostic services implementation.
*
* Each function builds a UDS payload, sends via ISO-TP,
* parses the response (positive = SID+0x40, negative = 0x7F+SID+NRC).
* Handles NRC 0x78 (ResponsePending) with extended timeout.
*/
#include "sdkconfig.h"
#if defined(CONFIG_MODULE_CANBUS) && defined(CONFIG_CANBUS_UDS)
#include <string.h>
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "canbus_uds.h"
#include "canbus_isotp.h"
#include "canbus_driver.h"
#include "utils.h"
#define TAG "CAN_UDS"
/* Max retries for ResponsePending (NRC 0x78) */
#define MAX_PENDING_RETRIES 10
#define PENDING_TIMEOUT_MS 5000
/* ============================================================
* Internal: UDS request with ResponsePending handling
* ============================================================ */
static int uds_transact(uds_ctx_t *ctx,
const uint8_t *req, size_t req_len,
uint8_t *resp, size_t resp_cap, size_t *resp_len)
{
int timeout = ctx->timeout_ms > 0 ? ctx->timeout_ms : 2000;
for (int retry = 0; retry <= MAX_PENDING_RETRIES; retry++) {
int t = (retry == 0) ? timeout : PENDING_TIMEOUT_MS;
isotp_status_t st = isotp_request(
ctx->tx_id, ctx->rx_id,
req, req_len,
resp, resp_cap, resp_len,
t
);
if (st == ISOTP_TIMEOUT) {
if (retry > 0) {
ESP_LOGW(TAG, "ResponsePending timeout after %d retries", retry);
}
return -1;
}
if (st != ISOTP_OK) return -1;
/* Check for Negative Response */
if (*resp_len >= 3 && resp[0] == 0x7F) {
uint8_t nrc = resp[2];
/* ResponsePending — ECU needs more time */
if (nrc == UDS_NRC_RESPONSE_PENDING) {
ESP_LOGI(TAG, "ResponsePending from 0x%03lX (retry %d/%d)",
(unsigned long)ctx->rx_id, retry + 1, MAX_PENDING_RETRIES);
/* Re-listen for the real response (no re-send needed) */
*resp_len = 0;
isotp_status_t st2 = isotp_recv(
ctx->rx_id, resp, resp_cap, resp_len, PENDING_TIMEOUT_MS
);
if (st2 == ISOTP_TIMEOUT) continue; /* Try again */
if (st2 != ISOTP_OK) return -1;
/* Check if we got another NRC 0x78 or the real response */
if (*resp_len >= 3 && resp[0] == 0x7F && resp[2] == UDS_NRC_RESPONSE_PENDING) {
continue; /* Still pending */
}
/* Got the real response, fall through to return */
}
/* Other negative responses */
if (resp[0] == 0x7F && resp[2] != UDS_NRC_RESPONSE_PENDING) {
ESP_LOGW(TAG, "NRC 0x%02X (%s) for SID 0x%02X from 0x%03lX",
resp[2], uds_nrc_name(resp[2]), resp[1],
(unsigned long)ctx->rx_id);
return -1;
}
}
/* Positive response or parsed NRC */
return (int)*resp_len;
}
return -1;
}
/* ============================================================
* Public API
* ============================================================ */
int uds_diagnostic_session(uds_ctx_t *ctx, uint8_t session_type)
{
uint8_t req[2] = { UDS_DIAG_SESSION_CTRL, session_type };
uint8_t resp[64];
size_t resp_len = 0;
int ret = uds_transact(ctx, req, 2, resp, sizeof(resp), &resp_len);
if (ret < 0) return -1;
/* Positive response: 0x50 + session type */
if (resp_len >= 2 && resp[0] == (UDS_DIAG_SESSION_CTRL + 0x40)) {
ctx->session = session_type;
return 0;
}
return -1;
}
int uds_tester_present(uds_ctx_t *ctx)
{
uint8_t req[2] = { UDS_TESTER_PRESENT, 0x00 }; /* subFunction = 0 */
uint8_t resp[16];
size_t resp_len = 0;
int ret = uds_transact(ctx, req, 2, resp, sizeof(resp), &resp_len);
if (ret < 0) return -1;
if (resp_len >= 2 && resp[0] == (UDS_TESTER_PRESENT + 0x40)) {
return 0;
}
return -1;
}
int uds_read_data_by_id(uds_ctx_t *ctx, uint16_t did,
uint8_t *out, size_t cap)
{
uint8_t req[3] = {
UDS_READ_DATA_BY_ID,
(uint8_t)(did >> 8),
(uint8_t)(did & 0xFF),
};
uint8_t resp[512];
size_t resp_len = 0;
int ret = uds_transact(ctx, req, 3, resp, sizeof(resp), &resp_len);
if (ret < 0) return -1;
/* Positive: 0x62 + DID (2 bytes) + data */
if (resp_len >= 3 && resp[0] == (UDS_READ_DATA_BY_ID + 0x40)) {
size_t data_len = resp_len - 3;
if (data_len > cap) data_len = cap;
memcpy(out, &resp[3], data_len);
return (int)data_len;
}
return -1;
}
int uds_security_access_seed(uds_ctx_t *ctx, uint8_t level,
uint8_t *seed, size_t *seed_len)
{
/* Seed request: odd subFunction (level = 0x01, 0x03, ...) */
uint8_t req[2] = { UDS_SECURITY_ACCESS, level };
uint8_t resp[64];
size_t resp_len = 0;
int ret = uds_transact(ctx, req, 2, resp, sizeof(resp), &resp_len);
if (ret < 0) return -1;
/* Positive: 0x67 + level + seed bytes */
if (resp_len >= 2 && resp[0] == (UDS_SECURITY_ACCESS + 0x40)) {
size_t slen = resp_len - 2;
if (seed && seed_len) {
memcpy(seed, &resp[2], slen);
*seed_len = slen;
}
return 0;
}
return -1;
}
int uds_security_access_key(uds_ctx_t *ctx, uint8_t level,
const uint8_t *key, size_t key_len)
{
/* Key send: even subFunction (level+1 = 0x02, 0x04, ...) */
uint8_t req[34] = { UDS_SECURITY_ACCESS, (uint8_t)(level + 1) };
if (key_len > 32) return -1;
memcpy(&req[2], key, key_len);
uint8_t resp[16];
size_t resp_len = 0;
int ret = uds_transact(ctx, req, 2 + key_len, resp, sizeof(resp), &resp_len);
if (ret < 0) return -1;
if (resp_len >= 2 && resp[0] == (UDS_SECURITY_ACCESS + 0x40)) {
ctx->security_unlocked = true;
return 0;
}
return -1;
}
int uds_read_memory(uds_ctx_t *ctx, uint32_t addr, uint16_t size,
uint8_t *out)
{
/* addressAndLengthFormatIdentifier: 0x24 = 2 bytes size, 4 bytes addr */
uint8_t req[7] = {
UDS_READ_MEM_BY_ADDR,
0x24, /* format: 2+4 */
(uint8_t)(addr >> 24),
(uint8_t)(addr >> 16),
(uint8_t)(addr >> 8),
(uint8_t)(addr),
(uint8_t)(size >> 8),
};
/* Append size low byte */
uint8_t req_full[8];
memcpy(req_full, req, 7);
req_full[7] = (uint8_t)(size & 0xFF);
/* Wait — need proper format. Let's redo:
* SID(1) + addressAndLengthFormatId(1) + memAddr(4) + memSize(2) = 8 bytes */
uint8_t request[8] = {
UDS_READ_MEM_BY_ADDR,
0x24,
(uint8_t)(addr >> 24),
(uint8_t)(addr >> 16),
(uint8_t)(addr >> 8),
(uint8_t)(addr),
(uint8_t)(size >> 8),
(uint8_t)(size),
};
uint8_t resp[512];
size_t resp_len = 0;
int ret = uds_transact(ctx, request, 8, resp, sizeof(resp), &resp_len);
if (ret < 0) return -1;
/* Positive: 0x63 + data */
if (resp_len >= 1 && resp[0] == (UDS_READ_MEM_BY_ADDR + 0x40)) {
size_t data_len = resp_len - 1;
if (out) memcpy(out, &resp[1], data_len);
return (int)data_len;
}
return -1;
}
int uds_raw_request(uds_ctx_t *ctx,
const uint8_t *req, size_t req_len,
uint8_t *resp, size_t resp_cap, size_t *resp_len)
{
return uds_transact(ctx, req, req_len, resp, resp_cap, resp_len);
}
/* ============================================================
* ECU Discovery
* ============================================================ */
int uds_scan_ecus(uint32_t *found_ids, int max_ecus)
{
int found = 0;
/* Standard UDS range: 0x7E0-0x7E7 (physical addressing) */
for (uint32_t tx = 0x7E0; tx <= 0x7E7 && found < max_ecus; tx++) {
uint32_t rx = tx + 0x08; /* Response IDs: 0x7E8-0x7EF */
uds_ctx_t ctx = {
.tx_id = tx,
.rx_id = rx,
.timeout_ms = 200, /* Short timeout for scan */
.session = UDS_SESSION_DEFAULT,
.security_unlocked = false,
};
if (uds_tester_present(&ctx) == 0) {
ESP_LOGI(TAG, "ECU found: TX=0x%03lX RX=0x%03lX",
(unsigned long)tx, (unsigned long)rx);
found_ids[found++] = tx;
}
}
/* Extended range: 0x700-0x7DF */
for (uint32_t tx = 0x700; tx <= 0x7DF && found < max_ecus; tx++) {
/* Skip standard range (already scanned) */
if (tx >= 0x7E0) break;
uint32_t rx = tx + 0x08;
uds_ctx_t ctx = {
.tx_id = tx,
.rx_id = rx,
.timeout_ms = 100,
.session = UDS_SESSION_DEFAULT,
.security_unlocked = false,
};
if (uds_tester_present(&ctx) == 0) {
ESP_LOGI(TAG, "ECU found: TX=0x%03lX RX=0x%03lX",
(unsigned long)tx, (unsigned long)rx);
found_ids[found++] = tx;
}
/* Yield every 16 IDs to avoid watchdog */
if ((tx & 0x0F) == 0x0F) {
vTaskDelay(pdMS_TO_TICKS(1));
}
}
return found;
}
/* ============================================================
* NRC Name Lookup
* ============================================================ */
const char *uds_nrc_name(uint8_t nrc)
{
switch (nrc) {
case 0x10: return "generalReject";
case 0x11: return "serviceNotSupported";
case 0x12: return "subFunctionNotSupported";
case 0x13: return "incorrectMessageLength";
case 0x14: return "responseTooLong";
case 0x21: return "busyRepeatRequest";
case 0x22: return "conditionsNotCorrect";
case 0x24: return "requestSequenceError";
case 0x25: return "noResponseFromSubnet";
case 0x26: return "failurePreventsExecution";
case 0x31: return "requestOutOfRange";
case 0x33: return "securityAccessDenied";
case 0x35: return "invalidKey";
case 0x36: return "exceededNumberOfAttempts";
case 0x37: return "requiredTimeDelayNotExpired";
case 0x70: return "uploadDownloadNotAccepted";
case 0x71: return "transferDataSuspended";
case 0x72: return "generalProgrammingFailure";
case 0x73: return "wrongBlockSequenceCounter";
case 0x78: return "responsePending";
case 0x7E: return "subFunctionNotSupportedInActiveSession";
case 0x7F: return "serviceNotSupportedInActiveSession";
default: return "unknown";
}
}
#endif /* CONFIG_MODULE_CANBUS && CONFIG_CANBUS_UDS */
Reference in New Issue View Git Blame Copy Permalink