914 lines
29 KiB
C
914 lines
29 KiB
C
/*
|
|
* Dino Control Unit (DCU) Simulator
|
|
*
|
|
* A simulated prehistoric vehicle DCU that accepts UDS (Unified Dino Services)
|
|
* messages over a TCP socket using CAN frame framing.
|
|
*
|
|
* Players connect via TCP and send/receive raw 16-byte CAN frames,
|
|
* exactly as they would on a Bedrock-era CAN bus (struct can_frame layout).
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
#include <signal.h>
|
|
#include <stdint.h>
|
|
#include <sys/socket.h>
|
|
#include <netinet/in.h>
|
|
#include <arpa/inet.h>
|
|
#include <errno.h>
|
|
|
|
/* CAN frame structure that matches the SocketCAN struct can_frame layout. */
|
|
typedef uint32_t canid_t;
|
|
|
|
struct can_frame {
|
|
canid_t can_id; /* CAN ID + EFF/RTR/ERR flags */
|
|
uint8_t can_dlc; /* data length code: 0..8 */
|
|
uint8_t __pad;
|
|
uint8_t __res0;
|
|
uint8_t __res1;
|
|
uint8_t data[8]; /* payload */
|
|
} __attribute__((packed));
|
|
|
|
#define CAN_SFF_MASK 0x000007FFU /* standard frame format (SFF) */
|
|
|
|
/* ============================================================
|
|
* Constants
|
|
* ============================================================ */
|
|
|
|
#define ECU_PORT 18088 /* TCP port for CAN-over-TCP (Bedrock Diagnostic Port) */
|
|
|
|
/* UDS CAN IDs (ISO 15765) */
|
|
#define UDS_REQUEST_ID 0x7DF /* Broadcast request */
|
|
#define UDS_PHYSICAL_ID 0x7E0 /* Physical request to this ECU */
|
|
#define UDS_RESPONSE_ID 0x7E8 /* ECU response */
|
|
|
|
/* UDS Service IDs */
|
|
#define SID_DIAG_SESSION_CTRL 0x10
|
|
#define SID_SECURITY_ACCESS 0x27
|
|
#define SID_READ_DID 0x22
|
|
#define SID_WRITE_DID 0x2E
|
|
#define SID_CLEAR_DTC 0x14
|
|
#define SID_READ_DTC 0x19
|
|
#define SID_REQUEST_DOWNLOAD 0x34
|
|
#define SID_TRANSFER_DATA 0x36
|
|
#define SID_ROUTINE_CTRL 0x31
|
|
|
|
/* UDS Negative Response Codes */
|
|
#define NRC_GENERAL_REJECT 0x10
|
|
#define NRC_SERVICE_NOT_SUPPORTED 0x11
|
|
#define NRC_SUBFUNCTION_NOT_SUPPORTED 0x12
|
|
#define NRC_INCORRECT_MSG_LEN 0x13
|
|
#define NRC_CONDITIONS_NOT_CORRECT 0x22
|
|
#define NRC_SECURITY_ACCESS_DENIED 0x33
|
|
#define NRC_INVALID_KEY 0x35
|
|
|
|
/* Positive response = SID + 0x40 */
|
|
#define POSITIVE_RESPONSE(sid) ((sid) + 0x40)
|
|
|
|
/* Limits */
|
|
#define MAX_DTC_ENTRIES 16
|
|
#define MAX_DID_RECORDS 8
|
|
#define DID_DATA_SIZE 64
|
|
#define DTC_DESC_SIZE 56
|
|
|
|
/* Session types */
|
|
#define SESSION_DEFAULT 0x01
|
|
#define SESSION_EXTENDED 0x03
|
|
#define SESSION_SECURITY 0x04
|
|
|
|
/* ============================================================
|
|
* Data Structures
|
|
* ============================================================ */
|
|
|
|
/* DTC (Dino Trouble Code) entry */
|
|
typedef struct dtc_entry {
|
|
uint32_t dtc_code;
|
|
uint8_t status;
|
|
uint8_t severity;
|
|
uint16_t occurrence_count;
|
|
void (*report_fn)(struct dtc_entry *self);
|
|
char description[DTC_DESC_SIZE];
|
|
uint8_t _pad[8];
|
|
} __attribute__((packed)) dtc_entry_t;
|
|
|
|
/* DID (Data Identifier) record */
|
|
typedef struct did_record {
|
|
uint16_t did_id;
|
|
uint16_t data_len;
|
|
uint8_t data[DID_DATA_SIZE];
|
|
uint8_t writable;
|
|
uint8_t padding[11];
|
|
} __attribute__((packed)) did_record_t;
|
|
|
|
typedef struct {
|
|
uint8_t session_type;
|
|
uint8_t security_level;
|
|
uint32_t seed;
|
|
uint8_t seed_sent;
|
|
} session_state_t;
|
|
|
|
typedef struct {
|
|
uint8_t active;
|
|
uint32_t total_size;
|
|
uint32_t received;
|
|
uint8_t *buffer;
|
|
uint8_t block_counter;
|
|
} transfer_state_t;
|
|
|
|
|
|
/* ============================================================
|
|
* Globals
|
|
* ============================================================ */
|
|
|
|
static int listen_sock = -1;
|
|
static int can_sock = -1; /* current client connection */
|
|
static session_state_t session = { .session_type = SESSION_DEFAULT };
|
|
static transfer_state_t xfer = { 0 };
|
|
|
|
/* DTC table */
|
|
static dtc_entry_t *dtc_table[MAX_DTC_ENTRIES];
|
|
static int dtc_count = 0;
|
|
|
|
/* DID table */
|
|
static did_record_t *did_table[MAX_DID_RECORDS];
|
|
static int did_count = 0;
|
|
|
|
static const char *FLAG_PATH = "/flag.txt";
|
|
|
|
/* ============================================================
|
|
* Utility Functions
|
|
* ============================================================ */
|
|
|
|
static void send_can_response(uint8_t *data, uint8_t len) {
|
|
struct can_frame frame;
|
|
memset(&frame, 0, sizeof(frame));
|
|
frame.can_id = UDS_RESPONSE_ID;
|
|
frame.can_dlc = (len > 8) ? 8 : len;
|
|
memcpy(frame.data, data, frame.can_dlc);
|
|
write(can_sock, &frame, sizeof(struct can_frame));
|
|
}
|
|
|
|
static void send_negative_response(uint8_t sid, uint8_t nrc) {
|
|
uint8_t resp[3] = { 0x7F, sid, nrc };
|
|
send_can_response(resp, 3);
|
|
}
|
|
|
|
static void send_positive_response(uint8_t sid, uint8_t *data, uint8_t len) {
|
|
uint8_t resp[8];
|
|
resp[0] = POSITIVE_RESPONSE(sid);
|
|
if (data && len > 0) {
|
|
uint8_t copy_len = (len > 7) ? 7 : len;
|
|
memcpy(&resp[1], data, copy_len);
|
|
send_can_response(resp, 1 + copy_len);
|
|
} else {
|
|
send_can_response(resp, 1);
|
|
}
|
|
}
|
|
|
|
/* Default DTC report function, prints to DCU console */
|
|
static void default_dtc_report(dtc_entry_t *self) {
|
|
printf("[DCU] Dino Trouble Report: Code=0x%06X Status=0x%02X Severity=%d Count=%d\n",
|
|
self->dtc_code, self->status, self->severity, self->occurrence_count);
|
|
printf("[DCU] Description: %s\n", self->description);
|
|
}
|
|
|
|
void __attribute__((used)) reset_code(dtc_entry_t *arg) {
|
|
FILE *f = fopen(FLAG_PATH, "r");
|
|
if (f) {
|
|
char flag[128];
|
|
memset(flag, 0, sizeof(flag));
|
|
if (fgets(flag, sizeof(flag), f)) {
|
|
printf("[DCU] YABBA DABBA DOO! DIAGNOSTIC OVERRIDE ACTIVATED\n");
|
|
printf("[DCU] RESET CODE: %s\n", flag);
|
|
size_t flag_len = strlen(flag);
|
|
if (flag_len > 0 && flag[flag_len - 1] == '\n') {
|
|
flag[--flag_len] = '\0';
|
|
}
|
|
size_t offset = 0;
|
|
while (offset < flag_len) {
|
|
uint8_t resp[8];
|
|
memset(resp, 0, sizeof(resp));
|
|
resp[0] = 0xFF;
|
|
size_t chunk = flag_len - offset;
|
|
if (chunk > 7) chunk = 7;
|
|
memcpy(&resp[1], &flag[offset], chunk);
|
|
send_can_response(resp, 1 + chunk);
|
|
offset += chunk;
|
|
}
|
|
}
|
|
fclose(f);
|
|
} else {
|
|
printf("[DCU] Flag file not found. Create /flag.txt\n");
|
|
}
|
|
fflush(stdout);
|
|
}
|
|
|
|
/* ============================================================
|
|
* UDS Service Handlers
|
|
* ============================================================ */
|
|
|
|
/*
|
|
* 0x10 - DiagnosticSessionControl
|
|
* Subfunction: session type (0x01=default, 0x03=extended, 0x04=security)
|
|
*/
|
|
static void handle_diag_session_ctrl(uint8_t *data, uint8_t len) {
|
|
if (len < 2) {
|
|
send_negative_response(SID_DIAG_SESSION_CTRL, NRC_INCORRECT_MSG_LEN);
|
|
return;
|
|
}
|
|
|
|
uint8_t requested_session = data[1];
|
|
|
|
if (requested_session != SESSION_DEFAULT &&
|
|
requested_session != SESSION_EXTENDED &&
|
|
requested_session != SESSION_SECURITY) {
|
|
send_negative_response(SID_DIAG_SESSION_CTRL, NRC_SUBFUNCTION_NOT_SUPPORTED);
|
|
return;
|
|
}
|
|
|
|
session.session_type = requested_session;
|
|
session.security_level = 0;
|
|
session.seed_sent = 0;
|
|
|
|
printf("[DCU] Session changed to 0x%02X\n", requested_session);
|
|
|
|
uint8_t resp_data[1] = { requested_session };
|
|
send_positive_response(SID_DIAG_SESSION_CTRL, resp_data, 1);
|
|
}
|
|
|
|
/*
|
|
* 0x27 - SecurityAccess
|
|
* Sub 0x01 = requestSeed, Sub 0x02 = sendKey
|
|
*/
|
|
static void handle_security_access(uint8_t *data, uint8_t len) {
|
|
if (session.session_type == SESSION_DEFAULT) {
|
|
send_negative_response(SID_SECURITY_ACCESS, NRC_CONDITIONS_NOT_CORRECT);
|
|
return;
|
|
}
|
|
|
|
if (len < 2) {
|
|
send_negative_response(SID_SECURITY_ACCESS, NRC_INCORRECT_MSG_LEN);
|
|
return;
|
|
}
|
|
|
|
uint8_t subfunc = data[1];
|
|
|
|
if (subfunc == 0x01) {
|
|
/* Request seed */
|
|
session.seed = 0xDEAD0000 | (rand() & 0xFFFF);
|
|
session.seed_sent = 1;
|
|
|
|
uint8_t resp[5];
|
|
resp[0] = 0x01;
|
|
memcpy(&resp[1], &session.seed, 4);
|
|
send_positive_response(SID_SECURITY_ACCESS, resp, 5);
|
|
printf("[DCU] Security seed sent: 0x%08X\n", session.seed);
|
|
|
|
} else if (subfunc == 0x02) {
|
|
/* Send key */
|
|
if (!session.seed_sent) {
|
|
send_negative_response(SID_SECURITY_ACCESS, NRC_CONDITIONS_NOT_CORRECT);
|
|
return;
|
|
}
|
|
if (len < 6) {
|
|
send_negative_response(SID_SECURITY_ACCESS, NRC_INCORRECT_MSG_LEN);
|
|
return;
|
|
}
|
|
|
|
uint32_t key;
|
|
memcpy(&key, &data[2], 4);
|
|
|
|
uint32_t expected = session.seed ^ 0xCAFEBABE;
|
|
|
|
if (key == expected) {
|
|
session.security_level = 1;
|
|
printf("[DCU] Security access GRANTED\n");
|
|
uint8_t resp[1] = { 0x02 };
|
|
send_positive_response(SID_SECURITY_ACCESS, resp, 1);
|
|
} else {
|
|
printf("[DCU] Security access DENIED (got 0x%08X, expected 0x%08X)\n", key, expected);
|
|
send_negative_response(SID_SECURITY_ACCESS, NRC_INVALID_KEY);
|
|
}
|
|
} else {
|
|
send_negative_response(SID_SECURITY_ACCESS, NRC_SUBFUNCTION_NOT_SUPPORTED);
|
|
}
|
|
}
|
|
|
|
/*
|
|
* 0x22 - ReadDataByIdentifier
|
|
* Reads a DID record.
|
|
*/
|
|
static void handle_read_did(uint8_t *data, uint8_t len) {
|
|
if (len < 3) {
|
|
send_negative_response(SID_READ_DID, NRC_INCORRECT_MSG_LEN);
|
|
return;
|
|
}
|
|
|
|
uint16_t did_id = (data[1] << 8) | data[2];
|
|
|
|
/* Search for the DID */
|
|
for (int i = 0; i < did_count; i++) {
|
|
if (did_table[i] && did_table[i]->did_id == did_id) {
|
|
uint8_t resp[7];
|
|
resp[0] = (did_id >> 8) & 0xFF;
|
|
resp[1] = did_id & 0xFF;
|
|
uint8_t copy = (did_table[i]->data_len > 5) ? 5 : did_table[i]->data_len;
|
|
memcpy(&resp[2], did_table[i]->data, copy);
|
|
send_positive_response(SID_READ_DID, resp, 2 + copy);
|
|
return;
|
|
}
|
|
}
|
|
|
|
send_negative_response(SID_READ_DID, NRC_CONDITIONS_NOT_CORRECT);
|
|
}
|
|
|
|
/*
|
|
* 0x2E - WriteDataByIdentifier
|
|
* Allocates or updates a DID record.
|
|
*/
|
|
static void handle_write_did(uint8_t *data, uint8_t len) {
|
|
if (session.session_type == SESSION_DEFAULT) {
|
|
send_negative_response(SID_WRITE_DID, NRC_CONDITIONS_NOT_CORRECT);
|
|
return;
|
|
}
|
|
|
|
if (len < 4) {
|
|
send_negative_response(SID_WRITE_DID, NRC_INCORRECT_MSG_LEN);
|
|
return;
|
|
}
|
|
|
|
uint16_t did_id = (data[1] << 8) | data[2];
|
|
uint8_t write_len = len - 3;
|
|
|
|
/* Check if DID already exists */
|
|
for (int i = 0; i < did_count; i++) {
|
|
if (did_table[i] && did_table[i]->did_id == did_id) {
|
|
/* Update existing */
|
|
memcpy(did_table[i]->data, &data[3], write_len);
|
|
did_table[i]->data_len = write_len;
|
|
send_positive_response(SID_WRITE_DID, &data[1], 2);
|
|
printf("[DCU] DID 0x%04X updated (%d bytes)\n", did_id, write_len);
|
|
return;
|
|
}
|
|
}
|
|
|
|
/* Create new DID record */
|
|
if (did_count >= MAX_DID_RECORDS) {
|
|
send_negative_response(SID_WRITE_DID, NRC_CONDITIONS_NOT_CORRECT);
|
|
return;
|
|
}
|
|
|
|
did_record_t *rec = (did_record_t *)malloc(sizeof(did_record_t));
|
|
if (!rec) {
|
|
send_negative_response(SID_WRITE_DID, NRC_GENERAL_REJECT);
|
|
return;
|
|
}
|
|
|
|
memset(rec, 0, sizeof(did_record_t));
|
|
rec->did_id = did_id;
|
|
rec->writable = 1;
|
|
rec->data_len = write_len;
|
|
memcpy(rec->data, &data[3], write_len);
|
|
|
|
did_table[did_count++] = rec;
|
|
|
|
printf("[DCU] DID 0x%04X created (%d bytes) at %p\n", did_id, write_len, (void*)rec);
|
|
send_positive_response(SID_WRITE_DID, &data[1], 2);
|
|
}
|
|
|
|
/*
|
|
* 0x31 - RoutineControl
|
|
* Sub 0x01 = startRoutine, Sub 0x03 = requestRoutineResults
|
|
* Routine 0xFF01: multi-frame write to a DID.
|
|
*/
|
|
typedef struct {
|
|
uint8_t active;
|
|
uint16_t did_id;
|
|
uint16_t total_len;
|
|
uint16_t received;
|
|
uint8_t buffer[96]; /* accumulation buffer */
|
|
} extended_write_state_t;
|
|
|
|
static extended_write_state_t ext_write = { 0 };
|
|
|
|
static void handle_routine_control(uint8_t *data, uint8_t len) {
|
|
if (session.session_type == SESSION_DEFAULT) {
|
|
send_negative_response(SID_ROUTINE_CTRL, NRC_CONDITIONS_NOT_CORRECT);
|
|
return;
|
|
}
|
|
|
|
if (len < 4) {
|
|
send_negative_response(SID_ROUTINE_CTRL, NRC_INCORRECT_MSG_LEN);
|
|
return;
|
|
}
|
|
|
|
uint8_t subfunc = data[1];
|
|
uint16_t routine_id = (data[2] << 8) | data[3];
|
|
|
|
if (routine_id != 0xFF01) {
|
|
send_negative_response(SID_ROUTINE_CTRL, NRC_CONDITIONS_NOT_CORRECT);
|
|
return;
|
|
}
|
|
|
|
if (subfunc == 0x01) {
|
|
if (len < 7) {
|
|
send_negative_response(SID_ROUTINE_CTRL, NRC_INCORRECT_MSG_LEN);
|
|
return;
|
|
}
|
|
|
|
ext_write.did_id = (data[4] << 8) | data[5];
|
|
ext_write.total_len = data[6];
|
|
if (ext_write.total_len > sizeof(ext_write.buffer)) {
|
|
ext_write.total_len = sizeof(ext_write.buffer);
|
|
}
|
|
ext_write.received = 0;
|
|
ext_write.active = 1;
|
|
|
|
printf("[DCU] Extended write started: DID=0x%04X, len=%d\n",
|
|
ext_write.did_id, ext_write.total_len);
|
|
|
|
uint8_t resp[3] = { 0x01, 0xFF, 0x01 };
|
|
send_positive_response(SID_ROUTINE_CTRL, resp, 3);
|
|
|
|
} else if (subfunc == 0x03) {
|
|
if (!ext_write.active) {
|
|
send_negative_response(SID_ROUTINE_CTRL, NRC_CONDITIONS_NOT_CORRECT);
|
|
return;
|
|
}
|
|
|
|
uint8_t chunk_len = len - 4;
|
|
uint16_t remaining = ext_write.total_len - ext_write.received;
|
|
if (chunk_len > remaining) chunk_len = remaining;
|
|
|
|
memcpy(&ext_write.buffer[ext_write.received], &data[4], chunk_len);
|
|
ext_write.received += chunk_len;
|
|
|
|
printf("[DCU] Extended write chunk: %d/%d bytes\n",
|
|
ext_write.received, ext_write.total_len);
|
|
|
|
if (ext_write.received >= ext_write.total_len) {
|
|
/* Write complete, find or create the DID */
|
|
int found = -1;
|
|
for (int i = 0; i < did_count; i++) {
|
|
if (did_table[i] && did_table[i]->did_id == ext_write.did_id) {
|
|
found = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (found >= 0) {
|
|
/* Update existing DID with full buffer content */
|
|
uint16_t copy_len = ext_write.total_len;
|
|
if (copy_len > DID_DATA_SIZE) copy_len = DID_DATA_SIZE;
|
|
memcpy(did_table[found]->data, ext_write.buffer, copy_len);
|
|
did_table[found]->data_len = copy_len;
|
|
printf("[DCU] Extended write complete to existing DID 0x%04X\n", ext_write.did_id);
|
|
} else {
|
|
/* Create new DID */
|
|
if (did_count < MAX_DID_RECORDS) {
|
|
did_record_t *rec = (did_record_t *)malloc(sizeof(did_record_t));
|
|
if (rec) {
|
|
memset(rec, 0, sizeof(did_record_t));
|
|
rec->did_id = ext_write.did_id;
|
|
rec->writable = 1;
|
|
rec->data_len = ext_write.total_len;
|
|
if (rec->data_len > DID_DATA_SIZE) rec->data_len = DID_DATA_SIZE;
|
|
memcpy(rec->data, ext_write.buffer, rec->data_len);
|
|
did_table[did_count++] = rec;
|
|
printf("[DCU] Extended write complete, new DID 0x%04X at %p\n",
|
|
ext_write.did_id, (void*)rec);
|
|
}
|
|
}
|
|
}
|
|
ext_write.active = 0;
|
|
|
|
uint8_t resp[3] = { 0x03, 0xFF, 0x01 };
|
|
send_positive_response(SID_ROUTINE_CTRL, resp, 3);
|
|
} else {
|
|
uint8_t resp[3] = { 0x03, 0xFF, 0x01 };
|
|
send_positive_response(SID_ROUTINE_CTRL, resp, 3);
|
|
}
|
|
} else {
|
|
send_negative_response(SID_ROUTINE_CTRL, NRC_SUBFUNCTION_NOT_SUPPORTED);
|
|
}
|
|
}
|
|
|
|
/* 0x14 - ClearDiagnosticInformation */
|
|
static void handle_clear_dtc(uint8_t *data, uint8_t len) {
|
|
if (len < 4) {
|
|
send_negative_response(SID_CLEAR_DTC, NRC_INCORRECT_MSG_LEN);
|
|
return;
|
|
}
|
|
|
|
uint32_t group = ((uint32_t)data[1] << 16) | ((uint32_t)data[2] << 8) | data[3];
|
|
|
|
if (group == 0xFFFFFF) {
|
|
/* Clear ALL DTCs */
|
|
printf("[DCU] Clearing ALL Dino Trouble Codes (%d entries)\n", dtc_count);
|
|
for (int i = 0; i < dtc_count; i++) {
|
|
if (dtc_table[i]) {
|
|
printf("[DCU] Freeing DTC entry at %p\n", (void*)dtc_table[i]);
|
|
free(dtc_table[i]);
|
|
}
|
|
}
|
|
} else {
|
|
/* Clear specific DTC */
|
|
for (int i = 0; i < dtc_count; i++) {
|
|
if (dtc_table[i] && dtc_table[i]->dtc_code == group) {
|
|
printf("[DCU] Freeing DTC 0x%06X at %p\n", group, (void*)dtc_table[i]);
|
|
free(dtc_table[i]);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
send_positive_response(SID_CLEAR_DTC, NULL, 0);
|
|
}
|
|
|
|
/* 0x19 - ReadDTCInformation */
|
|
static void handle_read_dtc(uint8_t *data, uint8_t len) {
|
|
if (len < 2) {
|
|
send_negative_response(SID_READ_DTC, NRC_INCORRECT_MSG_LEN);
|
|
return;
|
|
}
|
|
|
|
uint8_t subfunc = data[1];
|
|
|
|
if (subfunc == 0x01) {
|
|
/* Report number of DTCs */
|
|
uint8_t resp[4];
|
|
resp[0] = 0x01;
|
|
resp[1] = 0xFF; /* availability mask */
|
|
resp[2] = (dtc_count >> 8) & 0xFF;
|
|
resp[3] = dtc_count & 0xFF;
|
|
send_positive_response(SID_READ_DTC, resp, 4);
|
|
|
|
} else if (subfunc == 0x02) {
|
|
/* Report DTC by status mask */
|
|
uint8_t status_mask = (len >= 3) ? data[2] : 0xFF;
|
|
|
|
printf("[DCU] Reading DTCs with mask 0x%02X\n", status_mask);
|
|
|
|
for (int i = 0; i < dtc_count; i++) {
|
|
if (dtc_table[i]) {
|
|
dtc_entry_t *entry = dtc_table[i];
|
|
|
|
if (entry->status & status_mask) {
|
|
/* Send DTC info over CAN */
|
|
uint8_t resp[7];
|
|
resp[0] = 0x02;
|
|
resp[1] = (entry->dtc_code >> 16) & 0xFF;
|
|
resp[2] = (entry->dtc_code >> 8) & 0xFF;
|
|
resp[3] = entry->dtc_code & 0xFF;
|
|
resp[4] = entry->status;
|
|
resp[5] = entry->severity;
|
|
send_positive_response(SID_READ_DTC, resp, 6);
|
|
|
|
if (entry->report_fn) {
|
|
printf("[DCU] Calling report function at %p\n", (void*)entry->report_fn);
|
|
entry->report_fn(entry);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
} else if (subfunc == 0x06) {
|
|
/* Report specific DTC extended data */
|
|
if (len < 5) {
|
|
send_negative_response(SID_READ_DTC, NRC_INCORRECT_MSG_LEN);
|
|
return;
|
|
}
|
|
uint32_t dtc_code = ((uint32_t)data[2] << 16) | ((uint32_t)data[3] << 8) | data[4];
|
|
|
|
for (int i = 0; i < dtc_count; i++) {
|
|
if (dtc_table[i] && dtc_table[i]->dtc_code == dtc_code) {
|
|
dtc_entry_t *entry = dtc_table[i];
|
|
uint8_t resp[7];
|
|
resp[0] = 0x06;
|
|
resp[1] = (entry->dtc_code >> 16) & 0xFF;
|
|
resp[2] = (entry->dtc_code >> 8) & 0xFF;
|
|
resp[3] = entry->dtc_code & 0xFF;
|
|
resp[4] = entry->status;
|
|
resp[5] = entry->occurrence_count & 0xFF;
|
|
send_positive_response(SID_READ_DTC, resp, 6);
|
|
|
|
if (entry->report_fn) {
|
|
printf("[DCU] Calling report function at %p\n", (void*)entry->report_fn);
|
|
entry->report_fn(entry);
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
send_negative_response(SID_READ_DTC, NRC_CONDITIONS_NOT_CORRECT);
|
|
|
|
} else {
|
|
send_negative_response(SID_READ_DTC, NRC_SUBFUNCTION_NOT_SUPPORTED);
|
|
}
|
|
}
|
|
|
|
/* 0x34 - RequestDownload (register a new DTC) */
|
|
static void handle_request_download(uint8_t *data, uint8_t len) {
|
|
if (session.session_type == SESSION_DEFAULT) {
|
|
send_negative_response(SID_REQUEST_DOWNLOAD, NRC_CONDITIONS_NOT_CORRECT);
|
|
return;
|
|
}
|
|
|
|
if (len < 7) {
|
|
send_negative_response(SID_REQUEST_DOWNLOAD, NRC_INCORRECT_MSG_LEN);
|
|
return;
|
|
}
|
|
|
|
if (dtc_count >= MAX_DTC_ENTRIES) {
|
|
send_negative_response(SID_REQUEST_DOWNLOAD, NRC_CONDITIONS_NOT_CORRECT);
|
|
return;
|
|
}
|
|
|
|
dtc_entry_t *entry = (dtc_entry_t *)malloc(sizeof(dtc_entry_t));
|
|
if (!entry) {
|
|
send_negative_response(SID_REQUEST_DOWNLOAD, NRC_GENERAL_REJECT);
|
|
return;
|
|
}
|
|
|
|
memset(entry, 0, sizeof(dtc_entry_t));
|
|
entry->severity = data[2];
|
|
entry->dtc_code = ((uint32_t)data[3] << 16) | ((uint32_t)data[4] << 8) | data[5];
|
|
entry->status = data[6];
|
|
entry->occurrence_count = 1;
|
|
entry->report_fn = default_dtc_report;
|
|
snprintf(entry->description, DTC_DESC_SIZE, "DTC-0x%06X", entry->dtc_code);
|
|
|
|
dtc_table[dtc_count] = entry;
|
|
int idx = dtc_count++;
|
|
|
|
printf("[DCU] DTC entry created: idx=%d code=0x%06X at %p (report_fn=%p)\n",
|
|
idx, entry->dtc_code, (void*)entry, (void*)entry->report_fn);
|
|
|
|
uint8_t resp[4];
|
|
resp[0] = 0x20; /* lengthFormatIdentifier */
|
|
resp[1] = (idx >> 8) & 0xFF;
|
|
resp[2] = idx & 0xFF;
|
|
resp[3] = sizeof(dtc_entry_t) & 0xFF;
|
|
send_positive_response(SID_REQUEST_DOWNLOAD, resp, 4);
|
|
}
|
|
|
|
/* 0x36 - TransferData (append description to a DTC entry) */
|
|
static void handle_transfer_data(uint8_t *data, uint8_t len) {
|
|
if (session.session_type == SESSION_DEFAULT) {
|
|
send_negative_response(SID_TRANSFER_DATA, NRC_CONDITIONS_NOT_CORRECT);
|
|
return;
|
|
}
|
|
|
|
if (len < 3) {
|
|
send_negative_response(SID_TRANSFER_DATA, NRC_INCORRECT_MSG_LEN);
|
|
return;
|
|
}
|
|
|
|
uint8_t dtc_idx = data[1];
|
|
if (dtc_idx >= dtc_count || !dtc_table[dtc_idx]) {
|
|
send_negative_response(SID_TRANSFER_DATA, NRC_CONDITIONS_NOT_CORRECT);
|
|
return;
|
|
}
|
|
|
|
/* Write description data */
|
|
dtc_entry_t *entry = dtc_table[dtc_idx];
|
|
uint8_t write_len = len - 2;
|
|
|
|
/* Find current end of description */
|
|
size_t cur_len = strlen(entry->description);
|
|
if (cur_len + write_len >= DTC_DESC_SIZE) {
|
|
write_len = DTC_DESC_SIZE - cur_len - 1;
|
|
}
|
|
if (write_len > 0) {
|
|
memcpy(&entry->description[cur_len], &data[2], write_len);
|
|
entry->description[cur_len + write_len] = '\0';
|
|
}
|
|
|
|
printf("[DCU] TransferData to DTC[%d]: wrote %d bytes desc\n", dtc_idx, write_len);
|
|
|
|
uint8_t resp[1] = { dtc_idx };
|
|
send_positive_response(SID_TRANSFER_DATA, resp, 1);
|
|
}
|
|
|
|
/* ============================================================
|
|
* Main UDS Dispatcher
|
|
* ============================================================ */
|
|
|
|
static void process_uds_request(uint8_t *data, uint8_t len) {
|
|
if (len < 1) return;
|
|
|
|
uint8_t sid = data[0];
|
|
printf("\n[DCU] >>> Received SID 0x%02X (len=%d)\n", sid, len);
|
|
|
|
switch (sid) {
|
|
case SID_DIAG_SESSION_CTRL:
|
|
handle_diag_session_ctrl(data, len);
|
|
break;
|
|
case SID_SECURITY_ACCESS:
|
|
handle_security_access(data, len);
|
|
break;
|
|
case SID_READ_DID:
|
|
handle_read_did(data, len);
|
|
break;
|
|
case SID_WRITE_DID:
|
|
handle_write_did(data, len);
|
|
break;
|
|
case SID_CLEAR_DTC:
|
|
handle_clear_dtc(data, len);
|
|
break;
|
|
case SID_READ_DTC:
|
|
handle_read_dtc(data, len);
|
|
break;
|
|
case SID_REQUEST_DOWNLOAD:
|
|
handle_request_download(data, len);
|
|
break;
|
|
case SID_TRANSFER_DATA:
|
|
handle_transfer_data(data, len);
|
|
break;
|
|
case SID_ROUTINE_CTRL:
|
|
handle_routine_control(data, len);
|
|
break;
|
|
default:
|
|
printf("[DCU] Unknown service 0x%02X\n", sid);
|
|
send_negative_response(sid, NRC_SERVICE_NOT_SUPPORTED);
|
|
break;
|
|
}
|
|
fflush(stdout);
|
|
}
|
|
|
|
/* ============================================================
|
|
* CAN Interface Setup & Main Loop
|
|
* ============================================================ */
|
|
|
|
/* Read exactly n bytes from a socket (TCP can fragment) */
|
|
static int read_exact(int fd, void *buf, size_t n) {
|
|
size_t total = 0;
|
|
while (total < n) {
|
|
ssize_t r = read(fd, (uint8_t *)buf + total, n - total);
|
|
if (r <= 0) return -1;
|
|
total += r;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static int setup_tcp_server(int port) {
|
|
int s = socket(AF_INET, SOCK_STREAM, 0);
|
|
if (s < 0) {
|
|
perror("[DCU] socket");
|
|
return -1;
|
|
}
|
|
|
|
int opt = 1;
|
|
setsockopt(s, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
|
|
|
|
struct sockaddr_in addr;
|
|
memset(&addr, 0, sizeof(addr));
|
|
addr.sin_family = AF_INET;
|
|
addr.sin_addr.s_addr = INADDR_ANY;
|
|
addr.sin_port = htons(port);
|
|
|
|
if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
|
perror("[DCU] bind");
|
|
close(s);
|
|
return -1;
|
|
}
|
|
|
|
if (listen(s, 1) < 0) {
|
|
perror("[DCU] listen");
|
|
close(s);
|
|
return -1;
|
|
}
|
|
|
|
return s;
|
|
}
|
|
|
|
static void print_banner(int port) {
|
|
printf("\n");
|
|
printf("╔══════════════════════════════════════════════════════════════╗\n");
|
|
printf("║ .---. _ ║\n");
|
|
printf("║ / o o \\ ___| |_ Canopysaurus v1.1.31 ║\n");
|
|
printf("║ | >.< | / __| __| Dino Control Unit (DCU) ║\n");
|
|
printf("║ \\_-_/ | (__| |_ Bedrock Vehicle Diagnostics ║\n");
|
|
printf("║ /| |\\ \\___|\\__| (Unified Dino Services) ║\n");
|
|
printf("║ (_| |_) ║\n");
|
|
printf("║ ║\n");
|
|
printf("║ Transport: CAN frames over TCP ║\n");
|
|
printf("║ Listening: 0.0.0.0:%-5d ║\n", port);
|
|
printf("║ Request ID: 0x7DF (broadcast) / 0x7E0 (physical) ║\n");
|
|
printf("║ Response ID: 0x7E8 ║\n");
|
|
printf("║ Frame size: 16 bytes (struct can_frame) ║\n");
|
|
printf("╚══════════════════════════════════════════════════════════════╝\n");
|
|
printf("\n");
|
|
printf("[DCU] Booting up... Brontosaurus engine online!\n");
|
|
printf("[DCU] Supported services: 0x10 0x27 0x22 0x2E 0x14 0x19 0x34 0x36 0x31\n");
|
|
printf("[DCU] reset_code located at %p\n\n", (void*)reset_code);
|
|
fflush(stdout);
|
|
}
|
|
|
|
static void reset_state(void) {
|
|
session.session_type = SESSION_DEFAULT;
|
|
session.security_level = 0;
|
|
session.seed = 0;
|
|
session.seed_sent = 0;
|
|
|
|
memset(&xfer, 0, sizeof(xfer));
|
|
|
|
dtc_count = 0;
|
|
did_count = 0;
|
|
memset(dtc_table, 0, sizeof(dtc_table));
|
|
memset(did_table, 0, sizeof(did_table));
|
|
memset(&ext_write, 0, sizeof(ext_write));
|
|
}
|
|
|
|
static void cleanup(int sig) {
|
|
(void)sig;
|
|
printf("\n[DCU] Shutting down...\n");
|
|
if (can_sock >= 0) close(can_sock);
|
|
if (listen_sock >= 0) close(listen_sock);
|
|
exit(0);
|
|
}
|
|
|
|
int main(int argc, char *argv[]) {
|
|
int port = ECU_PORT;
|
|
|
|
if (argc > 1) {
|
|
port = atoi(argv[1]);
|
|
if (port <= 0 || port > 65535) port = ECU_PORT;
|
|
}
|
|
|
|
/* Disable buffering for Docker/remote */
|
|
setbuf(stdout, NULL);
|
|
setbuf(stderr, NULL);
|
|
|
|
signal(SIGINT, cleanup);
|
|
signal(SIGTERM, cleanup);
|
|
srand(0x45435500);
|
|
|
|
print_banner(port);
|
|
|
|
listen_sock = setup_tcp_server(port);
|
|
if (listen_sock < 0) {
|
|
fprintf(stderr, "[DCU] Failed to start TCP server on port %d\n", port);
|
|
return 1;
|
|
}
|
|
|
|
printf("[DCU] TCP server listening on port %d\n", port);
|
|
printf("[DCU] Send 16-byte CAN frames (struct can_frame) over TCP\n\n");
|
|
|
|
/* Accept loop */
|
|
while (1) {
|
|
struct sockaddr_in client_addr;
|
|
socklen_t client_len = sizeof(client_addr);
|
|
|
|
printf("[DCU] Waiting for connection...\n");
|
|
fflush(stdout);
|
|
|
|
can_sock = accept(listen_sock, (struct sockaddr *)&client_addr, &client_len);
|
|
if (can_sock < 0) {
|
|
perror("[DCU] accept");
|
|
continue;
|
|
}
|
|
|
|
printf("[DCU] Client connected from %s:%d\n",
|
|
inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port));
|
|
|
|
/* Reset all state for new connection */
|
|
reset_state();
|
|
|
|
/* Per-client message loop */
|
|
struct can_frame frame;
|
|
while (1) {
|
|
/* Read exactly one CAN frame (16 bytes) */
|
|
if (read_exact(can_sock, &frame, sizeof(struct can_frame)) < 0) {
|
|
printf("[DCU] Client disconnected\n");
|
|
break;
|
|
}
|
|
|
|
/* Only process UDS requests */
|
|
canid_t id = frame.can_id & CAN_SFF_MASK;
|
|
if (id != UDS_REQUEST_ID && id != UDS_PHYSICAL_ID) {
|
|
continue;
|
|
}
|
|
|
|
/* First byte of CAN data is PCI/length in single-frame ISO-TP */
|
|
uint8_t pci = frame.data[0];
|
|
uint8_t sf_len = pci & 0x0F; /* Single Frame length */
|
|
|
|
if (sf_len == 0 || sf_len > 7) {
|
|
/* Invalid single frame or multi-frame */
|
|
continue;
|
|
}
|
|
|
|
/* Process the UDS payload (after PCI byte) */
|
|
process_uds_request(&frame.data[1], sf_len);
|
|
}
|
|
|
|
close(can_sock);
|
|
can_sock = -1;
|
|
}
|
|
|
|
cleanup(0);
|
|
return 0;
|
|
}
|