cato447/ctf
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
Files
a9fdeb7e5d403ab0a1e276934f87f18424f736c7
ctf/2026/cscg/pwn/canopysaurus/ecu_sim.c
cato447 d1017bae51 huge commit
2026-03-09 21:44:25 +01:00

914 lines
29 KiB
C
Raw Blame History

/*
* 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;
}
Reference in New Issue View Git Blame Copy Permalink