内容来源于 https://github.com/zombieCraig/uds-server 仓库，转载到此，方便自己网络不便时查看。
/* (c) 2015 Open Garages */
/* Helper Macros */
#define SET_BIT(val, bitIndex) val |= (1 << bitIndex)
#define CLEAR_BIT(val, bitIndex) val &= ~(1 << bitIndex)
#define TOGGLE_BIT(val, bitIndex) val ^= (1 << bitIndex)
#define IS_SET(val, bitIndex) (val & (1 << bitIndex))
/* OBD-II Modes */
#define OBD_MODE_SHOW_CURRENT_DATA 0x01
#define OBD_MODE_SHOW_FREEZE_FRAME 0x02
#define OBD_MODE_READ_DTC 0x03
#define OBD_MODE_CLEAR_DTC 0x04
#define OBD_MODE_TEST_RESULTS_NON_CAN 0x05
#define OBD_MODE_TEST_RESULTS_CAN 0x06
#define OBD_MODE_READ_PENDING_DTC 0x07
#define OBD_MODE_CONTROL_OPERATIONS 0x08
#define OBD_MODE_VEHICLE_INFORMATION 0x09
#define OBD_MODE_READ_PERM_DTC 0x0A
/* UDS SIDs */
#define UDS_SID_DIAGNOSTIC_CONTROL 0x10 // GMLAN = Initiate Diagnostics
#define UDS_SID_ECU_RESET 0x11
#define UDS_SID_GM_READ_FAILURE_RECORD 0x12 // GMLAN
#define UDS_SID_CLEAR_DTC 0x14
#define UDS_SID_READ_DTC 0x19
#define UDS_SID_GM_READ_DID_BY_ID 0x1A // GMLAN - Read DID By ID
#define UDS_SID_RESTART_COMMUNICATIONS 0x20 // GMLAN - Restart a stopped com
#define UDS_SID_READ_DATA_BY_ID 0x22
#define UDS_SID_READ_MEM_BY_ADDRESS 0x23
#define UDS_SID_READ_SCALING_BY_ID 0x24
#define UDS_SID_SECURITY_ACCESS 0x27
#define UDS_SID_COMMUNICATION_CONTROL 0x28 // GMLAN Stop Communications
#define UDS_SID_READ_DATA_BY_ID_PERIODIC 0x2A
#define UDS_SID_DEFINE_DATA_ID 0x2C
#define UDS_SID_WRITE_DATA_BY_ID 0x2E
#define UDS_SID_IO_CONTROL_BY_ID 0x2F
#define UDS_SID_ROUTINE_CONTROL 0x31
#define UDS_SID_REQUEST_DOWNLOAD 0x34
#define UDS_SID_REQUEST_UPLOAD 0x35
#define UDS_SID_TRANSFER_DATA 0x36
#define UDS_SID_REQUEST_XFER_EXIT 0x37
#define UDS_SID_REQUEST_XFER_FILE 0x38
#define UDS_SID_WRITE_MEM_BY_ADDRESS 0x3D
#define UDS_SID_TESTER_PRESENT 0x3E
#define UDS_SID_ACCESS_TIMING 0x83
#define UDS_SID_SECURED_DATA_TRANS 0x84
#define UDS_SID_CONTROL_DTC_SETTINGS 0x85
#define UDS_SID_RESPONSE_ON_EVENT 0x86
#define UDS_SID_LINK_CONTROL 0x87
#define UDS_SID_GM_PROGRAMMED_STATE 0xA2
#define UDS_SID_GM_PROGRAMMING_MODE 0xA5
#define UDS_SID_GM_READ_DIAG_INFO 0xA9
#define UDS_SID_GM_READ_DATA_BY_ID 0xAA
#define UDS_SID_GM_DEVICE_CONTROL 0xAE
/* GM READ DIAG SUB FUNCS */
#define UDS_READ_STATUS_BY_MASK 0x81
/* DTC MASK Bitflags */
#define DTC_SUPPORTED_BY_CALIBRATION 1
#define DTC_CURRENT_DTC 2
#define DTC_TEST_NOT_PASSED_SINCE_CLEARED 4
#define DTC_TEST_FAILED_SINCE_CLEARED 8
#define DTC_HISTORY 16
#define DTC_TEST_NOT_PASSED_SINCE_POWER 32
#define DTC_CURRENT_DTC_SINCE_POWER 64
#define DTC_WARNING_INDICATOR_STATE 128
/* Periodic Data Message types */
#define PENDING_READ_DATA_BY_ID_GM 1
/*
* Instrument cluster simulator
*
* (c) 2014 Open Garages - Craig Smith <craig@theialabs.com>
*/
#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <signal.h>
#include <getopt.h>
#include <time.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include "uds-server.h"
#define DEBUG 0
//#define VIN "1G1ZT53826F109149"
//#define VIN "5YJSA1S2FEFA00001"
#define VIN "WAUZZZ8V9FA149850"
//#define VIN "2B3KA43R86H389824"
#define DATA_ALPHA 0
#define DATA_ALPHANUM 1
#define DATA_BINARY 2
/* Globals */
int running = 0;
int verbose = 0;
int no_flow_control = 0;
int fuzz_level = 0;
int keep_spec = 0;
FILE *plogfp = NULL;
char *vin = VIN;
struct timeval start_tv;
int pending_data;
struct can_frame gm_data_by_id;
long gm_lastcms = 0;
/* This is for flow control packets */
char gBuffer[255];
int gBufSize;
int gBufLengthRemaining;
int gBufCounter;
/* Prototypes */
void print_pkt(struct canfd_frame);
void print_bin(unsigned char *, int);
void usage(char *app, char *msg) {
printf("Simulates UDS responses\n");
if (msg) printf("%s\n", msg);
printf("Usage: %s [options] <can_interface>\n", app);
printf("\t-z\t\tIncrease fuzz level\n");
printf("\t-v\t\tVerbose\n");
printf("\t-l <logfile>\tLog output to file instead of STDOUT\n");
printf("\t-c\t\tDon't fuzz ISOTP Spec, just data\n");
printf("\t-F\t\tDisable flow control (Functional Addressing)\n");
printf("\t-V <vin>\tSpecify VIN (Default: %s)\n", VIN);
printf("\n");
exit(1);
}
// Simple function to print logging info to screen or to a file
void plog(char *fmt, ...) {
va_list args;
char buf[2046];
int len;
va_start(args, fmt);
len = vsnprintf(buf, 2045, fmt, args);
va_end(args);
if(plogfp) {
len = fwrite(buf, 1, len, plogfp);
} else {
printf("%s", buf);
}
}
void intHandler(int sig) {
running = 0;
}
// Generates data into a buff and returns it.
char *gen_data(int scope, int size) {
char *charset, *buf;
unsigned char byte;
int num;
int i;
buf = malloc(size);
memset(buf,0,size);
switch(scope) {
case DATA_ALPHA:
charset = "ABCDEFGHIJKLMNOPQRSTUVWXYZ";
for(i = 0; i < size; i++) {
buf[i] = charset[rand() % strlen(charset)];
}
break;
case DATA_ALPHANUM:
charset = "ABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789";
for(i = 0; i < size; i++) {
num = rand() % strlen(charset);
byte = charset[num];
if (DEBUG) printf("DEBUG: random byte[%d] = %d %02X\n", i, num, byte);
buf[i] = byte;
}
break;
case DATA_BINARY:
for(i = 0; i < size; i++) {
buf[i] = rand() % 256;
}
default:
break;
}
return buf;
}
// If a flow control packet comes in, push out more data
// This isn't fully supported, just a hack at the moment
void flow_control_push_to(int can, int id) {
struct canfd_frame frame;
int nbytes;
if(no_flow_control) return;
if(verbose) plog("FC: Flushing ISOTP buffers\n");
frame.can_id = id;
while(gBufLengthRemaining > 0) {
if(gBufLengthRemaining > 7) {
frame.len = 8;
frame.data[0] = gBufCounter;
memcpy(&frame.data[1], gBuffer+(gBufSize-gBufLengthRemaining), 7);
nbytes = write(can, &frame, CAN_MTU);
if(nbytes < 0) perror("Write packet (FC)");
gBufCounter++;
gBufLengthRemaining -= 7;
} else {
frame.len = gBufLengthRemaining + 1;
frame.data[0] = gBufCounter;
memcpy(&frame.data[1], gBuffer+(gBufSize-gBufLengthRemaining), CAN_MTU);
nbytes = write(can, &frame, CAN_MTU);
if(nbytes < 0) perror("Write packet (FC Final)");
gBufLengthRemaining = 0;
}
}
}
void flow_control_push(int can) {
flow_control_push_to(can, 0x7e8);
}
void isotp_send_to(int can, char *data, int size, int dest) {
struct canfd_frame frame;
int left = size;
int counter;
int nbytes;
if(size > 256) return;
frame.can_id = dest;
if(size < 7) {
frame.len = size + 1;
frame.data[0] = size;
memcpy(&frame.data[1], data, size);
nbytes = write(can, &frame, CAN_MTU);
if(nbytes < 0) perror("Write packet");
} else {
frame.len = 8;
frame.data[0] = 0x10;
if(fuzz_level > 2 && keep_spec == 0) {
frame.data[1] = rand() % 256;
printf("Breaking ISOTP specs real size = %d reported size = %d\n", size, frame.data[1]);
} else {
frame.data[1] = (char)size-1;
}
memcpy(&frame.data[2], data, 6);
nbytes = write(can, &frame, CAN_MTU);
if(nbytes < 0) perror("Write packet");
left -= 6;
counter = 0x21;
if(no_flow_control) {
while(left > 0) {
if(left > 7) {
frame.len = 8;
frame.data[0] = counter;
memcpy(&frame.data[1], data+(size-left), 7);
write(can, &frame, CAN_MTU);
counter++;
left -= 7;
} else {
frame.len = left + 1;
frame.data[0] = counter;
memcpy(&frame.data[1], data+(size-left), CAN_MTU);
write(can, &frame, frame.len);
left = 0;
}
}
} else { // FC
memcpy(gBuffer, data, size); // Size is restricted to <256
gBufSize = size;
gBufLengthRemaining = left;
gBufCounter = counter;
}
}
}
void isotp_send(int can, char *data, int size) {
isotp_send_to(can, data, size, 0x7e8);
}
/*
* Some UDS queries requiest periodic data. This handles those
*/
void handle_pending_data(int can) {
struct canfd_frame frame;
struct timeval tv;
long currcms;
int i, offset, datacnt;
if(!pending_data) return;
gettimeofday(&tv, NULL);
currcms = (tv.tv_sec - start_tv.tv_sec) * 100 + (tv.tv_usec / 10000);
if(IS_SET(pending_data, PENDING_READ_DATA_BY_ID_GM)) {
if(gm_data_by_id.data[0] == 0xFE) {
offset = 1;
} else {
offset = 0;
}
frame.can_id = gm_data_by_id.can_id;
frame.len = 8;
switch(gm_data_by_id.data[2 + offset]) { // Subfunctions
case 0x02: // Slow Rate
if (currcms - gm_lastcms > 1000) {
for(i=3; i < gm_data_by_id.data[0]+1; i++) {
frame.data[0] = gm_data_by_id.data[i];
for(datacnt=1; datacnt < 8; datacnt++) {
frame.data[datacnt] = rand() % 255;
}
write(can, &frame, CAN_MTU);
if(verbose > 1) plog(" + Sending GM data (%02X) at a slow rate\n", frame.data[0]);
}
gm_lastcms = currcms;
}
break;
case 0x03: // Medium Rate
if (currcms - gm_lastcms > 100) {
for(i=3; i < gm_data_by_id.data[0]+1; i++) {
frame.data[0] = gm_data_by_id.data[i];
for(datacnt=1; datacnt < 8; datacnt++) {
frame.data[datacnt] = rand() % 255;
}
write(can, &frame, CAN_MTU);
if(verbose > 1) plog(" + Sending GM data (%02X) at a medium rate\n", frame.data[0]);
}
gm_lastcms = currcms;
}
break;
case 0x04: // Fast Rate
if (currcms - gm_lastcms > 20) {
for(i=3; i < gm_data_by_id.data[0]+1; i++) {
frame.data[0] = gm_data_by_id.data[i];
for(datacnt=1; datacnt < 8; datacnt++) {
frame.data[datacnt] = rand() % 255;
}
write(can, &frame, CAN_MTU);
if(verbose > 1) plog(" + Sending GM data (%02X) at a fast rate\n", frame.data[0]);
}
gm_lastcms = currcms;
}
break;
default:
plog("Unknown subfunction timer\n");
break;
}
} // IS_SET PENDING_READ_DATA_BY_ID_GM
}
void send_dtcs(int can, char total, struct canfd_frame frame) {
char resp[1024];
char i;
memset(resp, 0, 1024);
switch(fuzz_level) {
case 0: // Default is to make P01XX where XX = total number of DTCs
resp[0] = frame.data[1] + 0x40;
resp[1] = total; // Total DTCs
for(i = 0; i <= total*2; i+=2) {
resp[2+i] = 1;
resp[2+i+1] = i;
}
if(total == 0) {
isotp_send(can, resp, 2);
} else if (total < 3) {
isotp_send(can, resp, 2+(total*2));
} else {
isotp_send(can, resp, total*2);
}
break;
case 1:
resp[0] = frame.data[1] + 0x40;
resp[1] = rand() % 256;
if (verbose) plog("Randomized total DTCs to %d real DTCs %d\n", resp[1], total);
for(i = 0; i <= total*2; i+=2) {
resp[2+i] = 1;
resp[2+i+1] = i;
}
if(total == 0) {
isotp_send(can, resp, 2);
} else if (total < 3) {
isotp_send(can, resp, 2+(total*2));
} else {
isotp_send(can, resp, total*2);
}
break;
case 2:
default:
resp[0] = frame.data[1] + 0x40;
total = rand() % 128;
resp[1] = total;
if (verbose) plog("Randomized total DTCs to %d\n", resp[1]);
for(i = 0; i <= total*2; i+=2) {
resp[2+i] = rand() % 256;
resp[2+i+1] = rand() % 256;
}
if (verbose) {
plog("DTC random data is:\n");
print_bin(&resp[2], total*2);
}
if(total == 0) {
isotp_send(can, resp, 2);
} else if (total < 3) {
isotp_send(can, resp, 2+(total*2));
} else {
isotp_send(can, resp, total*2);
}
break;
}
}
unsigned char calc_vin_checksum(char *vin, int size) {
char w[17] = { 8, 7, 6, 5, 4, 3, 2, 10, 0, 9, 8, 7, 6, 5, 4, 3, 2 };
int i;
int checksum = 0;
int num;
for(i=0; i < size; i++) {
if(vin[i] == 'I' || vin[i] == 'O' || vin[i] == 'Q') {
num = 0;
} else {
if(vin[i] >= '0' && vin[i] <='9') num = vin[i] - '0';
if(vin[i] >= 'A' && vin[i] <='I') num = (vin[i] - 'A') + 1;
if(vin[i] >= 'J' && vin[i] <='R') num = (vin[i] - 'J') + 1;
if(vin[i] >= 'S' && vin[i] <='Z') num = (vin[i] - 'S') + 2;
}
checksum += num * w[i];
}
checksum = checksum % 11;
if (checksum == 10) return 'X';
return ('0' + checksum);
}
void send_error_snfs(int can, struct canfd_frame frame) {
char resp[4];
if(verbose) plog("Responded with Sub Function Not Supported\n");
resp[0] = 0x7f;
resp[1] = frame.data[1];
resp[2] = 12; // SubFunctionNotSupported
isotp_send(can, resp, 3);
}
void send_error_roor(int can, struct canfd_frame frame, int id) {
char resp[4];
if(verbose) plog("Responded with Sub Function Not Supported\n");
resp[0] = 0x7f;
resp[1] = frame.data[1];
resp[2] = 31; // RequestOutOfRange
isotp_send_to(can, resp, 3, id);
}
void generic_OK_resp(int can, struct canfd_frame frame) {
char resp[4];
if(verbose > 1) plog("Responding with a generic OK message\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0;
isotp_send(can, resp, 3);
}
void generic_OK_resp_to(int can, struct canfd_frame frame, int id) {
char resp[4];
if(verbose > 1) plog("Responding with a generic OK message\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0;
isotp_send_to(can, resp, 3, id);
}
void handle_current_data(int can, struct canfd_frame frame) {
if(verbose) plog("Received Current info request\n");
char resp[8];
switch(frame.data[2]) {
case 0x00: // Supported PIDs
if(verbose) plog("Responding with a generic set of PIDs (1-20)\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0xBF;
resp[3] = 0xBF;
resp[4] = 0xB9;
resp[5] = 0x93;
isotp_send(can, resp, 6);
break;
case 0x01: // MIL & DTC Status
if(verbose) plog("Responding to MIL and DTC Status request\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0x00;
resp[3] = 0x07;
resp[4] = 0xE5;
resp[5] = 0xE5;
isotp_send(can, resp, 6);
break;
case 0x20: // More supported PIDs (21-40)
if(verbose) plog("Responding with PIDs supported (21-40)\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0xBF;
resp[3] = 0xBF;
resp[4] = 0xB9;
resp[5] = 0x93;
isotp_send(can, resp, 6);
break;
case 0x40: // More supported PIDs (41-60)
if(verbose) plog("Responding with PIDs supported (41-60)\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0xBF;
resp[3] = 0xBF;
resp[4] = 0xB9;
resp[5] = 0x93;
isotp_send(can, resp, 6);
break;
case 0x41: // Monitor status this drive cycle
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0;
resp[3] = 0x0F;
resp[4] = 0xFF;
resp[5] = 0x00;
isotp_send(can, resp, 6);
break;
case 0x60: // More supported PIDs (61-80)
if(verbose) plog("Responding with PIDs supported (61-80)\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0xBF;
resp[3] = 0xBF;
resp[4] = 0xB9;
resp[5] = 0x93;
isotp_send(can, resp, 6);
break;
case 0x80: // More supported PIDs (81-100)
if(verbose) plog("Responding with PIDs supported (81-100)\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0xBF;
resp[3] = 0xBF;
resp[4] = 0xB9;
resp[5] = 0x93;
isotp_send(can, resp, 6);
break;
case 0xA0: // More Supported PIDs (101-120)
if(verbose) plog("Responding with PIDs supported (101-120)\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0xBF;
resp[3] = 0xBF;
resp[4] = 0xB9;
resp[5] = 0x93;
isotp_send(can, resp, 6);
break;
case 0xC0: // More supported PIDs (121-140)
if(verbose) plog("Responding with PIDs supported (121-140)\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0xBF;
resp[3] = 0xBF;
resp[4] = 0xB9;
resp[5] = 0x93;
isotp_send(can, resp, 6);
break;
default:
if(verbose) plog("Note: Requested unsupported service %02X\n", frame.data[2]);
break;
}
}
void handle_vehicle_info(int can, struct canfd_frame frame) {
char *buf;
int pktsize = 0;
unsigned char chksum;
if(verbose) plog("Received Vehicle info request\n");
char resp[300];
switch(frame.data[2]) {
case 0x00: // Supported PIDs
if(verbose) plog("Replying with ALL Pids supported\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0x55;
resp[3] = 0;
resp[4] = 0;
resp[5] = 0;
isotp_send(can, resp, 6);
break;
case 0x02: // Get VIN
switch(fuzz_level) {
case 0:
if(verbose) plog("Sending VIN %s\n", vin);
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 1;
memcpy(&resp[3], vin, strlen(vin));
isotp_send(can, resp, 4 + strlen(vin));
break;
case 1:
if(verbose) plog("Fuzzing VIN with printable chars\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 1;
buf = gen_data(DATA_ALPHANUM, 17);
chksum = calc_vin_checksum(buf, 17);
buf[8] = chksum;
if(verbose) plog("Using VIN: %s\n", buf);
memcpy(&resp[3], buf, 17);
free(buf);
isotp_send(can, resp, 4 + 17);
break;
case 2:
case 3: // At 3 the ISOTP spec gets flaky
pktsize = rand() % 252;
if(verbose) plog("Fuzzing big VIN with printable chars\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 1;
buf = gen_data(DATA_ALPHANUM, pktsize);
chksum = calc_vin_checksum(buf, pktsize);
buf[8] = chksum;
if(verbose) plog("Using big VIN (%d chars): %s\n",pktsize, buf);
memcpy(&resp[3], buf, pktsize);
free(buf);
isotp_send(can, resp, 4 + pktsize);
break;
case 4:
if(verbose) plog("Fuzzing VIN with binary data\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 1;
buf = gen_data(DATA_BINARY, 17);
chksum = calc_vin_checksum(buf, 17);
buf[8] = chksum;
if(verbose) print_bin(buf, 17);
memcpy(&resp[3], buf, 17);
free(buf);
isotp_send(can, resp, 4 + 17);
break;
case 5:
default:
pktsize = rand() % 252;
if(verbose) plog("Fuzzing VIN with binary data with size %d\n", pktsize);
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 1;
buf = gen_data(DATA_BINARY, pktsize);
if(verbose) print_bin(buf, pktsize);
memcpy(&resp[3], buf, pktsize);
free(buf);
isotp_send(can, resp, 4 + pktsize);
break;
}
break;
default:
break;
}
}
void handle_pending_codes(int can, struct canfd_frame frame) {
if(verbose) plog("Received request for pending trouble codes\n");
send_dtcs(can, 20, frame);
}
void handle_stored_codes(int can, struct canfd_frame frame) {
if(verbose) plog("Received request for stored trouble codes\n");
send_dtcs(can, 2, frame);
}
// TODO: This is wrong. Record a real transaction to see the format
void handle_freeze_frame(int can, struct canfd_frame frame) {
if(verbose) plog("Received request for freeze frame code\n");
//send_dtcs(can, 1, frame);
char resp[4];
resp[0] = frame.data[1] + 0x40;
resp[1] = 0x01;
resp[2] = 0x01;
isotp_send(can, resp, 3);
}
void handle_perm_codes(int can, struct canfd_frame frame) {
if(verbose) plog("Received request for permanent trouble codes\n");
send_dtcs(can, 0, frame);
}
void handle_dsc(int can, struct canfd_frame frame) {
//if(verbose) plog("Received DSC Request\n");
//send_error_snfs(can, frame);
if(verbose) plog("Received DSC Request giving VCDS respose\n");
frame.can_id = 0x77A;
frame.len = 8;
frame.data[0] = 0x06;
frame.data[1] = 0x50;
frame.data[2] = 0x03;
frame.data[3] = 0x00;
frame.data[4] = 0x32;
frame.data[5] = 0x01;
frame.data[6] = 0xF4;
frame.data[7] = 0xAA;
write(can, &frame, CAN_MTU);
}
/*
ECU Memory, based on VCDS response for now
*/
void handle_read_data_by_id(int can, struct canfd_frame frame) {
if(verbose) plog("Recieved Read Data by ID %02X %02X\n", frame.data[2], frame.data[3]);
char resp[120];
if(frame.data[2] == 0xF1) {
switch(frame.data[3]) {
case 0x87:
if(verbose) plog("Read data by ID 0x87\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0x87;
resp[3] = 0x30;
resp[4] = 0x34;
resp[5] = 0x45;
resp[6] = 0x39;
resp[7] = 0x30;
resp[8] = 0x36;
resp[9] = 0x33;
resp[10] = 0x32;
resp[11] = 0x33;
resp[12] = 0x46;
resp[13] = 0x20; // Note VCDS pads with 55's
isotp_send_to(can, resp, 14, 0x77A);
break;
case 0x89:
if(verbose) plog("Read data by ID 0x89\n");
frame.can_id = 0x7E8;
frame.len = 8;
frame.data[0] = 0x07;
frame.data[1] = 0x62;
frame.data[2] = 0xF1;
frame.data[3] = 0x89;
frame.data[4] = 0x38; //8
frame.data[5] = 0x34; //4
frame.data[6] = 0x31; //1
frame.data[7] = 0x30; //0
write(can, &frame, CAN_MTU);
break;
case 0x9E:
if(verbose) plog("Read data by ID 0x9E\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0x45;
resp[3] = 0x56;
resp[4] = 0x5F;
resp[5] = 0x47;
resp[6] = 0x61;
resp[7] = 0x74;
resp[8] = 0x65;
resp[9] = 0x77;
resp[10] = 0x45;
resp[11] = 0x56;
resp[12] = 0x43;
resp[13] = 0x6F;
resp[14] = 0x6E;
resp[15] = 0x74;
resp[16] = 0x69;
resp[17] = 0x00;
isotp_send(can, resp, 0x13);
break;
case 0xA2:
if(verbose) plog("Read data by ID 0xA2\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0xA2;
resp[3] = 0x30; // 004010
resp[4] = 0x30;
resp[5] = 0x34;
resp[6] = 0x30;
resp[7] = 0x31;
resp[8] = 0x30;
isotp_send(can, resp, 9);
break;
default:
if(verbose) plog("Not responding to ID %02X\n", frame.data[3]);
break;
}
} else if(frame.data[2] == 0x06) {
switch(frame.data[3]) {
case 0x00:
if(verbose) plog("Read data by ID 0x9E\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0x02;
resp[3] = 0x01;
resp[4] = 0x00;
resp[5] = 0x17;
resp[6] = 0x26;
resp[7] = 0xF2;
resp[8] = 0x00;
resp[9] = 0x00;
resp[10] = 0x5B;
resp[11] = 0x00;
resp[12] = 0x12;
resp[13] = 0x08;
resp[14] = 0x58;
resp[15] = 0x00;
resp[16] = 0x00;
resp[17] = 0x00;
resp[18] = 0x00;
resp[19] = 0x01;
resp[20] = 0x01;
resp[21] = 0x01;
resp[22] = 0x00;
resp[23] = 0x01;
resp[24] = 0x00;
resp[25] = 0x00;
resp[26] = 0x00;
resp[27] = 0x00;
resp[28] = 0x00;
resp[29] = 0x00;
resp[30] = 0x00;
resp[31] = 0x00;
isotp_send(can, resp, 0x21);
break;
case 0x01:
if(verbose) plog("Read data by ID 0x01\n");
send_error_roor(can, frame, 0x7E8);
break;
default:
if(verbose) plog("Not responding to ID %02X\n", frame.data[3]);
break;
}
} else {
if(verbose) plog("Unknown read data by ID %02X\n", frame.data[2]);
}
}
/*
GM
*/
// Read DID from ID (GM)
// For now we are only setting this up to work with the BCM
// 244 [3] 02 1A 90
void handle_gm_read_did_by_id(int can, struct canfd_frame frame) {
if(verbose) plog("Received GM Read DID by ID Request\n");
char resp[300];
char *buf;
char *tracenum = "874602RA51950204";
unsigned char chksum;
int pktsize;
switch(frame.data[2]) {
case 0x90: // VIN
if(verbose) plog(" + Requested VIN\n");
switch(fuzz_level) {
case 0:
if(verbose) plog("Sending VIN %s\n", vin);
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
memcpy(&resp[2], vin, strlen(vin));
isotp_send_to(can, resp, 3 + strlen(vin), 0x644);
break;
case 1:
if(verbose) plog("Fuzzing VIN with printable chars\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
buf = gen_data(DATA_ALPHANUM, 17);
chksum = calc_vin_checksum(buf, 17);
buf[8] = chksum;
if(verbose) plog("Using VIN: %s\n", buf);
memcpy(&resp[2], buf, 17);
free(buf);
isotp_send_to(can, resp, 3 + 17, 0x644);
break;
case 2:
case 3: // At 3 the ISOTP spec gets flaky
pktsize = rand() % 252;
if(verbose) plog("Fuzzing big VIN with printable chars\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
buf = gen_data(DATA_ALPHANUM, pktsize);
chksum = calc_vin_checksum(buf, pktsize);
buf[8] = chksum;
if(verbose) plog("Using big VIN (%d chars): %s\n",pktsize, buf);
memcpy(&resp[2], buf, pktsize);
free(buf);
isotp_send_to(can, resp, 3 + pktsize, 0x644);
break;
case 4:
if(verbose) plog("Fuzzing VIN with binary data\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
buf = gen_data(DATA_BINARY, 17);
chksum = calc_vin_checksum(buf, 17);
buf[8] = chksum;
if(verbose) print_bin(buf, 17);
memcpy(&resp[2], buf, 17);
free(buf);
isotp_send_to(can, resp, 3 + 17, 0x644);
break;
case 5:
default:
pktsize = rand() % 252;
if(verbose) plog("Fuzzing VIN with binary data with size %d\n", pktsize);
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
buf = gen_data(DATA_BINARY, pktsize);
if(verbose) print_bin(buf, pktsize);
memcpy(&resp[2], buf, pktsize);
free(buf);
isotp_send_to(can, resp, 3 + pktsize, 0x644);
break;
}
break;
case 0xA1: // SDM Primary Key
if(verbose) plog(" + Requested SDM Primary Key\n");
switch(fuzz_level) {
case 0:
default:
if(verbose) plog("Sending SDM Key %04X\n", 0x6966);
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0x69;
resp[3] = 0x66;
isotp_send_to(can, resp, 5, 0x644);
break;
}
break;
case 0xB4: // Traceability Number
if(verbose) plog(" + Requested Traceability Number\n");
switch(fuzz_level) {
case 0:
default:
if(verbose) plog("Sending Traceabiliity number %s\n", tracenum);
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
memcpy(&resp[2], tracenum, strlen(tracenum));
isotp_send_to(can, resp, 3 + strlen(tracenum), 0x644);
break;
}
break;
case 0xB7: // Software Number
if(verbose) plog(" + Requested Software Number\n");
switch(fuzz_level) {
case 0:
default:
if(verbose) plog("Sending SW # %d\n", 600);
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0x42;
resp[3] = 0xAA;
resp[4] = 6;
resp[5] = 2; // 600
resp[6] = 0x58;
isotp_send_to(can, resp, 6, 0x644);
break;
}
break;
case 0xCB: // End Model Part #
if(verbose) plog(" + Requested End Model Part Number\n");
switch(fuzz_level) {
case 0:
default:
if(verbose) plog("Sending End Model Part Number %d\n", 15804602);
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0x00;
resp[3] = 0xF1;
resp[4] = 0x28;
resp[5] = 0xBA;
isotp_send_to(can, resp, 6, 0x644);
break;
}
break;
default:
break;
}
}
/* GM Read Data via PID */
/* 244 [5] 04 AA 03 02 07 */
/* 544#0738408D8B000200 */
/* 544#02508D8D00000000 */
void handle_gm_read_data_by_id(int can, struct canfd_frame frame) {
if(verbose) plog("Received GM Read Data by ID Request\n");
int offset = 0;
int i;
int datacnt;
char datacpy[8];
if (frame.data[0] == 0xFE) offset = 1;
memcpy(&datacpy, &frame.data, 8);
if(frame.can_id == 0x7e0) {
frame.can_id = 0x5e8;
} else {
frame.can_id = 0x500 + (frame.can_id & 0xFF);
}
frame.len = 8;
switch(frame.data[2 + offset]) { // Subfunctions
case 0x00: // Stop
if(verbose) plog(" + Stop Data Request\n");
memset(frame.data, 0, 8);
write(can, &frame, CAN_MTU);
CLEAR_BIT(pending_data, PENDING_READ_DATA_BY_ID_GM);
break;
case 0x01: // One Response
if(verbose) plog(" + One Response\n");
for(i=3; i < datacpy[0]+1; i++) {
frame.data[0] = datacpy[i];
for(datacnt=1; datacnt < 8; datacnt++) {
frame.data[datacnt] = rand() % 256;
}
write(can, &frame, CAN_MTU);
sleep(0.5);
}
break;
case 0x02: // Slow Rate
if(verbose) plog(" + Slow Rate\n");
SET_BIT(pending_data, PENDING_READ_DATA_BY_ID_GM);
memcpy(&gm_data_by_id, &frame, sizeof(frame));
break;
case 0x03: // Medium Rate
if(verbose) plog(" + Medium Rate\n");
SET_BIT(pending_data, PENDING_READ_DATA_BY_ID_GM);
memcpy(&gm_data_by_id, &frame, sizeof(frame));
break;
case 0x04: // Fast Rate
if(verbose) plog(" + Fast Rate\n");
SET_BIT(pending_data, PENDING_READ_DATA_BY_ID_GM);
memcpy(&gm_data_by_id, &frame, sizeof(frame));
break;
default:
plog("Unknown subfunction timer\n");
break;
}
}
/* GM Diag format is either
101#FE 03 A9 81 52 (Functional addressing: Where FE is the extended address)
7E0#03 A9 81 52 (no extended addressing)
*/
void handle_gm_read_diag(int can, struct canfd_frame frame) {
if(verbose) plog("Received GM Read Diagnostic Request\n");
int offset = 0;
int i, total;
char resp[150];
if(frame.data[0] == 0xFE) offset = 1;
switch(frame.data[2 + offset]) { // Subfunctions
case UDS_READ_STATUS_BY_MASK: // Read DTCs by mask
if(verbose) {
plog(" + Read DTCs by mask\n");
if(frame.data[3 + offset] & DTC_SUPPORTED_BY_CALIBRATION) plog(" - Supported By Calibration\n");
if(frame.data[3 + offset] & DTC_CURRENT_DTC) plog(" - Current DTC\n");
if(frame.data[3 + offset] & DTC_TEST_NOT_PASSED_SINCE_CLEARED) plog(" - Tests not passed since DTC cleared\n");
if(frame.data[3 + offset] & DTC_TEST_FAILED_SINCE_CLEARED) plog(" - Tests failed since DTC cleared\n");
if(frame.data[3 + offset] & DTC_HISTORY) plog(" - DTC History\n");
if(frame.data[3 + offset] & DTC_TEST_NOT_PASSED_SINCE_POWER) plog(" - Tests not passed since power up\n");
if(frame.data[3 + offset] & DTC_CURRENT_DTC_SINCE_POWER) plog(" - Tests failed since power up\n");
if(frame.data[3 + offset] & DTC_WARNING_INDICATOR_STATE) plog(" - Warning Indicator State\n");
}
if(frame.can_id == 0x7e0) {
frame.can_id = 0x5e8;
} else {
frame.can_id = 0x500 + (frame.can_id & 0xFF);
}
frame.len = 8;
frame.data[0] = frame.data[2 + offset];
frame.data[1] = 0; // DTC 1st byte
frame.data[2] = 0x30; // DTC 2nd byte
frame.data[3] = 0;
frame.data[4] = 0x6F; // Last Test/ This Ignition/ Last Clear bitflag
frame.data[5] = 0;
frame.data[6] = 0;
frame.data[7] = 0;
write(can, &frame, CAN_MTU);
sleep(0.2); // Instead of actually processing the FC
if(fuzz_level == 1) {
total = rand() % 1024;
if(verbose) plog("Sending %d DTCs\n", total);
for(i = 0; i < total; i++) {
frame.data[1] = rand() % 256;
frame.data[2] = (rand() % 255) + 1;
frame.data[3] = 0;
frame.data[4] = 0x6F; // Last DTC
write(can, &frame, CAN_MTU);
sleep(1);
}
}
frame.data[1] = 0; // Last frame must be a 0 DTC
frame.data[2] = 0;
frame.data[3] = 0;
frame.data[4] = 0xFF; // Last DTC
write(can, &frame, CAN_MTU);
break;
default:
if(verbose) plog(" + Unknown subfunction request %02X\n", frame.data[2 + offset]);
break;
}
}
/*
Gateway
*/
void handle_vcds_710(int can, struct canfd_frame frame) {
if(verbose) plog("Received VCDS 0x710 gateway request\n");
char resp[150];
if(frame.data[0] == 0x30) { // Flow control
flow_control_push_to(can,0x77A);
return;
}
switch(frame.data[1]) {
//Pkt: 710#02 10 03 55 55 55 55 55
case 0x10: // Diagnostic Session Control
frame.can_id = 0x77A;
frame.len = 8;
frame.data[0] = 0x06;
frame.data[1] = 0x50;
frame.data[2] = 0x03;
frame.data[3] = 0x00;
frame.data[4] = 0x32;
frame.data[5] = 0x01;
frame.data[6] = 0xF4;
frame.data[7] = 0xAA;
write(can, &frame, CAN_MTU);
break;
case 0x22: // Read Data By Identifier
if(frame.data[2] == 0xF1) {
switch(frame.data[3]) {
case 0x87: // VAG Number
if(verbose) plog("Read data by ID 0x87\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0x87;
resp[3] = 0x35;
resp[4] = 0x51;
resp[5] = 0x45;
resp[6] = 0x39;
resp[7] = 0x30;
resp[8] = 0x37;
resp[9] = 0x35;
resp[10] = 0x33;
resp[11] = 0x30;
resp[12] = 0x43;
resp[13] = 0x20; // Note normally this would pad with AA's
isotp_send_to(can, resp, 14, 0x77A);
break;
case 0x89: // VAG Number
if(verbose) plog("Read data by ID 0x89\n");
frame.can_id = 0x77A;
frame.len = 8;
frame.data[0] = 0x07;
frame.data[1] = 0x62;
frame.data[2] = 0xF1;
frame.data[3] = 0x89;
frame.data[4] = 0x33; //3
frame.data[5] = 0x32; //2
frame.data[6] = 0x30; //0
frame.data[7] = 0x33; //3
write(can, &frame, CAN_MTU);
break;
case 0x91: // VAG Number
if(verbose) plog("Read data by ID 0x91\n");
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0x87;
resp[3] = 0x35;
resp[4] = 0x51;
resp[5] = 0x45;
resp[6] = 0x39;
resp[7] = 0x30;
resp[8] = 0x37;
resp[9] = 0x35;
resp[10] = 0x33;
resp[11] = 0x30;
resp[12] = 0x41;
resp[13] = 0x20; // Note normally this would pad with AA's
isotp_send_to(can, resp, 14, 0x77A);
break;
default:
if(verbose) plog("NOTE: Read data by unknown ID %02X\n", frame.data[3]);
resp[0] = frame.data[1] + 0x40;
resp[1] = frame.data[2];
resp[2] = 0x87;
resp[3] = 0x35;
resp[4] = 0x51;
resp[5] = 0x45;
resp[6] = 0x39;
resp[7] = 0x30;
resp[8] = 0x37;
resp[9] = 0x35;
resp[10] = 0x33;
resp[11] = 0x30;
resp[12] = 0x41;
resp[13] = 0x20; // Note normally this would pad with AA's
isotp_send_to(can, resp, 14, 0x77A);
break;
}
} else {
if (verbose) plog("Unknown read data by Identifier %02X\n", frame.data[2]);
}
break;
}
}
// return Mode/SIDs in english
char *get_mode_str(struct canfd_frame frame) {
switch(frame.data[1]) {
case OBD_MODE_SHOW_CURRENT_DATA:
return "Show current Data";
break;
case OBD_MODE_SHOW_FREEZE_FRAME:
return "Show freeze frame";
break;
case OBD_MODE_READ_DTC:
return "Read DTCs";
break;
case OBD_MODE_CLEAR_DTC:
return "Clear DTCs";
break;
case OBD_MODE_TEST_RESULTS_NON_CAN:
return "Mode Test Results (Non-CAN)";
break;
case OBD_MODE_TEST_RESULTS_CAN:
return "Mode Test Results (CAN)";
break;
case OBD_MODE_READ_PENDING_DTC:
return "Read Pending DTCs";
break;
case OBD_MODE_CONTROL_OPERATIONS:
return "Control Operations";
break;
case OBD_MODE_VEHICLE_INFORMATION:
return "Vehicle Information";
break;
case OBD_MODE_READ_PERM_DTC:
return "Read Permanent DTCs";
break;
case UDS_SID_DIAGNOSTIC_CONTROL:
return "Diagnostic Control";
break;
case UDS_SID_ECU_RESET:
return "ECU Reset";
break;
case UDS_SID_CLEAR_DTC:
return "UDS Clear DTCs";
break;
case UDS_SID_READ_DTC:
return "UDS Read DTCs";
break;
case UDS_SID_GM_READ_DID_BY_ID:
return "Read DID by ID (GM)";
break;
case UDS_SID_RESTART_COMMUNICATIONS:
return "Restore Normal Commnications";
break;
case UDS_SID_READ_DATA_BY_ID:
return "Read DATA By ID";
break;
case UDS_SID_READ_MEM_BY_ADDRESS:
return "Read Memory By Address";
break;
case UDS_SID_READ_SCALING_BY_ID:
return "Read Scalling Data by ID";
break;
case UDS_SID_SECURITY_ACCESS:
return "Security Access";
break;
case UDS_SID_COMMUNICATION_CONTROL:
return "Communication Control";
break;
case UDS_SID_READ_DATA_BY_ID_PERIODIC:
return "Read DATA By ID Periodically";
break;
case UDS_SID_DEFINE_DATA_ID:
return "Define DATA By ID";
break;
case UDS_SID_WRITE_DATA_BY_ID:
return "Write DATA By ID";
break;
case UDS_SID_IO_CONTROL_BY_ID:
return "Input/Output Control By ID";
break;
case UDS_SID_ROUTINE_CONTROL:
return "Routine Control";
break;
case UDS_SID_REQUEST_DOWNLOAD:
return "Request Download";
break;
case UDS_SID_REQUEST_UPLOAD:
return "Request Upload";
break;
case UDS_SID_TRANSFER_DATA:
return "Transfer DATA";
break;
case UDS_SID_REQUEST_XFER_EXIT:
return "Request Transfer Exit";
break;
case UDS_SID_REQUEST_XFER_FILE:
return "Request Transfer File";
break;
case UDS_SID_WRITE_MEM_BY_ADDRESS:
return "Write Memory By Address";
break;
case UDS_SID_TESTER_PRESENT:
return "Tester Present";
break;
case UDS_SID_ACCESS_TIMING:
return "Access Timing";
break;
case UDS_SID_SECURED_DATA_TRANS:
return "Secured DATA Transfer";
break;
case UDS_SID_CONTROL_DTC_SETTINGS:
return "Control DTC Settings";
break;
case UDS_SID_RESPONSE_ON_EVENT:
return "Response On Event";
break;
case UDS_SID_LINK_CONTROL:
return "Link Control";
break;
case UDS_SID_GM_PROGRAMMED_STATE:
return "Programmed State (GM)";
break;
case UDS_SID_GM_PROGRAMMING_MODE:
return "Programming Mode (GM)";
break;
case UDS_SID_GM_READ_DIAG_INFO:
return "Read Diagnostic Information (GM)";
break;
case UDS_SID_GM_READ_DATA_BY_ID:
return "Read DATA By ID (GM)";
break;
case UDS_SID_GM_DEVICE_CONTROL:
return "Device Control (GM)";
break;
default:
printf("Unknown mode/sid (%02X)\n", frame.data[1]);
return "";
}
}
// Prints raw packet in ID#DATA format
void print_pkt(struct canfd_frame frame) {
int i;
plog("Pkt: %02X#", frame.can_id);
for(i = 0; i < frame.len; i++) {
plog("%02X ", frame.data[i]);
}
plog("\n");
}
// Prints binary data in hex format
void print_bin(unsigned char *bin, int size) {
int i;
for(i = 0; i < size; i++) {
plog("%02X ", bin[i]);
}
plog("\n");
}
// Handles the incomming CAN Packets
// Each ID that deals with specific controllers a note is
// given where that info came from. There could be a lot of overlap
// and exceptions here. -- Craig
void handle_pkt(int can, struct canfd_frame frame) {
if(DEBUG) print_pkt(frame);
switch(frame.can_id) {
case 0x243: // EBCM / GM / Chevy Malibu 2006
switch(frame.data[1]) {
case UDS_SID_TESTER_PRESENT:
if(verbose > 1) plog("Received TesterPresent\n");
generic_OK_resp_to(can, frame, 0x643);
break;
case UDS_SID_GM_READ_DIAG_INFO:
handle_gm_read_diag(can, frame);
break;
default:
if(verbose) print_pkt(frame);
if(verbose) plog("Unhandled mode/sid: %s\n", get_mode_str(frame));
break;
}
break;
case 0x244: // Body Control Module / GM / Chevy Malibu 2006
if(frame.data[0] == 0x30) { // Flow control
flow_control_push_to(can, 0x644);
return;
}
switch(frame.data[1]) {
case UDS_SID_TESTER_PRESENT:
if(verbose > 1) plog("Received TesterPresent\n");
generic_OK_resp_to(can, frame, 0x644);
break;
case UDS_SID_GM_READ_DIAG_INFO:
handle_gm_read_diag(can, frame);
break;
case UDS_SID_GM_READ_DATA_BY_ID:
handle_gm_read_data_by_id(can, frame);
break;
case UDS_SID_GM_READ_DID_BY_ID:
handle_gm_read_did_by_id(can, frame);
break;
default:
if(verbose) print_pkt(frame);
if(verbose) plog("Unhandled mode/sid: %s\n", get_mode_str(frame));
break;
}
break;
case 0x24A: // Power Steering / GM / Chevy Malibu 2006
switch(frame.data[1]) {
default:
if(verbose) print_pkt(frame);
if(verbose) plog("Unhandled mode/sid: %s\n", get_mode_str(frame));
break;
}
break;
case 0x350: // Unsure. Seen RTRs to this when requesting VIN
if (frame.can_id & CAN_RTR_FLAG) {
if (verbose) plog("Received a RTR at ID %02X\n", frame.can_id);
}
break;
case 0x710: // VCDS
if(verbose) print_pkt(frame);
handle_vcds_710(can, frame);
break;
case 0x7df:
case 0x7e0: // Sometimes flow control comes here
if(verbose) print_pkt(frame);
if(frame.data[0] == 0x30 && gBufLengthRemaining > 0) flow_control_push(can);
if(frame.data[0] == 0 || frame.len == 0) return;
if(frame.data[0] > frame.len) return;
switch (frame.data[1]) {
case OBD_MODE_SHOW_CURRENT_DATA:
handle_current_data(can, frame);
break;
case OBD_MODE_SHOW_FREEZE_FRAME:
handle_freeze_frame(can, frame);
break;
case OBD_MODE_READ_DTC:
handle_stored_codes(can, frame);
break;
case OBD_MODE_READ_PENDING_DTC:
handle_pending_codes(can, frame);
break;
case OBD_MODE_VEHICLE_INFORMATION:
handle_vehicle_info(can, frame);
break;
case OBD_MODE_READ_PERM_DTC:
handle_perm_codes(can, frame);
break;
case UDS_SID_DIAGNOSTIC_CONTROL: // DSC
handle_dsc(can, frame);
break;
case UDS_SID_READ_DATA_BY_ID:
handle_read_data_by_id(can, frame);
break;
case UDS_SID_TESTER_PRESENT:
if(verbose > 1) plog("Received TesterPresent\n");
generic_OK_resp(can, frame);
break;
case UDS_SID_GM_READ_DIAG_INFO:
handle_gm_read_diag(can, frame);
break;
default:
//if(verbose) plog("Unhandled mode/sid: %02X\n", frame.data[1]);
if(verbose) plog("Unhandled mode/sid: %s\n", get_mode_str(frame));
break;
}
break;
default:
if (DEBUG) print_pkt(frame);
if (DEBUG) plog("DEBUG: missed ID %02X\n", frame.can_id);
break;
}
}
int main(int argc, char *argv[]) {
int opt, ret;
int can;
int nbytes;
struct ifreq ifr;
struct sockaddr_can addr;
struct iovec iov;
struct msghdr msg;
struct canfd_frame frame;
char ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(__u32))];
struct sigaction act;
struct timeval timeo;
fd_set rdfs;
verbose = 0;
act.sa_handler = intHandler;
sigaction(SIGINT, &act, NULL);
sigaction(SIGHUP, &act, NULL);
srand(time(NULL));
while ((opt = getopt(argc, argv, "cV:zl:vFh?")) != -1) {
switch(opt) {
case 'c':
keep_spec = 1;
break;
case 'v':
verbose++;
break;
case 'V':
vin = optarg;
break;
case 'F':
no_flow_control = 1;
break;
case 'l':
plogfp = fopen(optarg, "a+");
break;
case 'z':
fuzz_level++;
break;
case 'h':
case '?':
default:
usage(argv[0], NULL);
break;
}
}
if (optind >= argc) usage(argv[0], "You must specify at least one can device");
// Create a new raw CAN socket
can = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if(can < 0) usage(argv[0], "Couldn't create raw socket");
addr.can_family = AF_CAN;
memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
strncpy(ifr.ifr_name, argv[optind], strlen(argv[optind]));
if (verbose) plog("Using CAN interface %s\n", ifr.ifr_name);
if (ioctl(can, SIOCGIFINDEX, &ifr) < 0) {
perror("SIOCGIFINDEX");
exit(1);
}
addr.can_ifindex = ifr.ifr_ifindex;
if (bind(can, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
perror("bind");
return 1;
}
iov.iov_base = &frame;
iov.iov_len = sizeof(frame);
msg.msg_name = &addr;
msg.msg_namelen = sizeof(addr);
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
msg.msg_control = &ctrlmsg;
msg.msg_controllen = sizeof(ctrlmsg);
msg.msg_flags = 0;
if(verbose) plog("Fuzz level set to: %d\n", fuzz_level);
gettimeofday(&start_tv, NULL);
running = 1;
while(running) {
FD_ZERO(&rdfs);
FD_SET(can, &rdfs);
timeo.tv_sec = 0;
timeo.tv_usec = 10000 * 20; // 20 ms
if ((ret = select(can+1, &rdfs, NULL, NULL, &timeo)) < 0) {
running = 0;
continue;
}
if (FD_ISSET(can, &rdfs)) {
nbytes = recvmsg(can, &msg, 0);
if (nbytes < 0) {
perror("read");
return 1;
}
if ((size_t)nbytes != CAN_MTU) {
fprintf(stderr, "read: incomplete CAN frame\n");
return 1;
}
handle_pkt(can, frame);
}
handle_pending_data(can);
}
plog("Got Interrupt. Shutting down gracefully\n");
if(plogfp) fclose(plogfp);
}
./uds-server -v -V "PWN3D OP3N G4R4G3" can0
C=gcc
all: uds-server
uds-userver: uds-server.o
$(CC) -o uds-server uds-server.c
clean:
rm -f uds-server *.o
UDS Server
Unified Diagnostic Services (UDS) Server - is a ECU simulator that provides UDS support.
This application was originally written to go alongside of [ICSim] (https://github.com/zombieCraig/ICSim)
for training.
Running both ICSim and uds-server can give students a more realistic use of tools. You can use
ICSim to understand the basics of reversing CAN and uds-server to dig into the UDS protocol
and Engine Control Unit (ECU) inspections such as memory reads and device I/O controls via the ECU instead of spoofed
Controller Area Network (CAN bus) packets.
In addition, when developing uds-server, it showed several more uses. When a dealership tool
is known to uds-server, it makes it very easy to see what the tool is attempting to do by spoofing
a real target vehicle. This allows you to quickly reverse commands from dealership tools and
see only the packets that matter. Another nice feature, is the ability to fuzz the
dealership/scantools to see if they are doing proper input validation checks. This enables
uds-server to work as a security tool by playing the role of a modified “malicious” vehicle and
seeing how the shop’s tools handle the malformed requests.
Compiling uds-server
Right now the tool was developed on Ubuntu Linux but is simple enough that it should compile on
any standard Linux system. Simply type ‘make’:
$ make
cc uds-server.c -o uds-server
This version is still considered ‘alpha’ but the help screen should look something like:
Simulates UDS responses
Usage: ./uds-server [options] <can_interface>
-z Increase fuzz level
-v Verbose
-l <logfile> Log output to file instead of STDOUT
-c Don't fuzz ISOTP Spec, just data
-F Disable flow control (Functional Addressing)
-V <vin> Specify VIN (Default: WAUZZZ8V9FA149850)
Most of these switches are just for early testing and will eventually be moved
to a config file for more flexibility in fuzzing, etc.
Running uds-server for testing
If you are running uds-server along with ICSim then simply start another terminal window and
run:d
$ uds-server vcan0
Then you can practice commands to get VIN or use things like [CaringCaribou] (https://github.com/CaringCaribou/caringcaribou) to brute force or identify diagnostic services.
If you ware working with a dealership tool or a scan tool then you will use the real can0 interface
instead. You will need a small CAN network to bridge the dealership/scantool with your CAN
sniffer attached to uds-server. You can breadboard this or build a small portable device we lovingly
call the ODB GW.
ODB GW
The ODB Gateway (ODB GW) is a tribute to Ol’ Dirty Bastard (RIP) and the mispronunciation
of OBD (On-board diagnostics) ports. It is a simple device that you can easily build yourself:
- 2 x Female J1962 OBD-II Ports (~$10/ea)
- Project Box (~$5)
- at least 2 120 Ohm Resistors (or 1 240 Ohm) (pennies)
- 12 V Power Supply (~12)
- Some wires and maybe banana plug connectors
The minimum wiring is as follows:
- Connect pin 12 together for power and splice a line to the 12V supply
- Connect pins 4 and 5 together for ground and splice a line to the 12V supply
- Connect pin 6 together for CAN High
- Connect pin 14 together for CAN Low
- Add 240 Ohm resistance across CAN High and Low
You can bridge more pins but how they are wired depends on what type of vehicle scanner you
are testing. For instance, several vehicles have many different CAN buses on the other
available pins while other manufacturers use the other pins for different protocols such
as K-Line/KWP. The above wiring is universal but you may miss out on signals from
dealership tools if you don’t also listen on the other pins.
Reversing Dealership Tools
Using your own CAN network or the ODB GW, plug in a dealership tool or scan tool in one end and
your sniffer on the other. Make sure 12V power is supplied to your virtual bus, some scantools
only operate when they have power from pin 16. Set your CAN bus speeds to be the speed
the dealership tool will expect, for HS CAN this is most likely 500k.
$ sudo ip link set can0 up type can bitrate 500000
Now run uds-server with the verbose option set on your can0 interface and use the
dealership tool like you would on an actual vehicle. For example below we use a GM TechII
and request the doors to unlock via the TechII interface. Looking at the uds-server output
we see:
Pkt: 244#01 3E
Responding with a generic OK message
Pkt: 244#04 AA 03 02 07
Received GM Read Data by ID Request
+ Medium Rate
Pkt: 244#01 3E
Responding with a generic OK message
Pkt: 244#07 AE 01 03 00 00 00 00
Unhandled mode/sid: Device Control (GM)
Pkt: 244#01 3E
Responding with a generic OK message
Pkt: 101#FE 01 3E 55 55 55 55 55
Pkt: 244#01 3E
Responding with a generic OK message
Pkt: 244#02 AE 00
Unhandled mode/sid: Device Control (GM)
Pkt: 244#01 3E
Responding with a generic OK message
Pkt: 244#01 3E
Responding with a generic OK message
Pkt: 244#02 AA 00
Received GM Read Data by ID Request
+ Stop Data Request
In this output the generic OK message refers to a TesterPresent packet sent by the dealership
tool. We simply respond with OK when we see things like this. Next the Tool requests some
data to be sent at a Medium interval rate. uds-server will do that with bogus data. Then we see
a Device Control (GM) request. We don’t handle it because it’s an output request and there is
nothing to spoof. However the packet info is useful:
Pkt: 244#07 AE 01 03 00 00 00 00
Unhandled mode/sid: Device Control (GM)
This means that sending 244#07AE010300000000 after sending TesterPresent (244#013E) will unlock
the driver side door. Later there is another Device Control message to stop doing device
controls 244#02AE00.
This makes it very easy to identify IO controls and to see where data is being requested from.
Often dealership tools won’t use the standard UDS mode $09 to get things like VIN but instead they
request VIN and other information via memory locations.
Fuzzing Dealership Tools
If you want to test the security of a dealership tool or scantool then uds-server has a fuzzing
option. Currently this is still in a Proof of Concept (PoC) stage and it needs to be refined but
the way it currently works is you can specify -z to increase the fuzzing level. The more -z’s you
use the more fuzzing it will do.
For instance:
$ uds-server -v -z can0
Using CAN interface can0
Fuzz level set to: 1
This will do things like randomize the Vehicle Identification Number (VIN) and some Diagnostic
Trouble Code (DTC) messages.
$ uds-server -v -zzzz can0
Using CAN interface can0
Fuzz level set to: 4
This will do things like send WAY too many DTCs (think hundreds) or create HUGE VINs that
also include binary data. A VIN contains an internal checksum that uds-server will automatically
calculate correctly for fuzzing. If you want to specify a VIN you can do so via the command
line:
$ uds-server -v -V "PWN3D OP3N G4R4G3" can0
This will report the vehicles VIN as “PWN3D OP3N G4R4G3” which by the way is a “valid” VIN based on the
checksum byte. Some tools use VIN as the lookup for what type of vehicle it is working with, so
specifying a valid one for your target vehicle can be useful.
uds-server hacking
Right now new ECU modules need to be added by hand. Which means you will need to understand the C
code and add handlers for how you want to respond to different types of packets. Debugging currently
is a hard coded constant as well. This is because uds-server is still in its PoC stage and could
evolve in many different directions.
Feel free to fork the code and add whatever new handlers you want to add. Ultimately the fuzzing
configuration and ECU configurations will be handled by a separate config file.
Credits
Craig Smith - craig@theialabs.com
OpenGarages - opengarages.org (@OpenGarages)
sudo ip link set can0 up type can bitrate 500000
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
