From b64ecdb1eaea35dc9ece7ed611344111e57a656c Mon Sep 17 00:00:00 2001 From: victors21 Date: Thu, 5 Jun 2025 15:48:06 +0300 Subject: [PATCH 1/2] added support for canfd frames for SavvyCan V220 app --- src/state_raw.c | 105 ++++++++++++++++++++++++++++++++++++------------ 1 file changed, 79 insertions(+), 26 deletions(-) diff --git a/src/state_raw.c b/src/state_raw.c index 8b40611..b4cc755 100644 --- a/src/state_raw.c +++ b/src/state_raw.c @@ -27,11 +27,44 @@ static int raw_socket; static struct ifreq ifr; static struct sockaddr_can addr; static struct msghdr msg; -static struct can_frame frame; +static struct canfd_frame frame; static struct iovec iov; static char ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(__u32))]; static struct timeval tv; -static struct cmsghdr *cmsg; +static struct cmsghdr *cmsg; + + +/* CAN DLC to real data length conversion helpers */ + +static const unsigned char dlc2len[] = {0, 1, 2, 3, 4, 5, 6, 7, + 8, 12, 16, 20, 24, 32, 48, 64}; + +/* get data length from raw data length code (DLC) */ +unsigned char can_fd_dlc2len(unsigned char dlc) +{ + return dlc2len[dlc & 0x0F]; +} + +static const unsigned char len2dlc[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, /* 0 - 8 */ + 9, 9, 9, 9, /* 9 - 12 */ + 10, 10, 10, 10, /* 13 - 16 */ + 11, 11, 11, 11, /* 17 - 20 */ + 12, 12, 12, 12, /* 21 - 24 */ + 13, 13, 13, 13, 13, 13, 13, 13, /* 25 - 32 */ + 14, 14, 14, 14, 14, 14, 14, 14, /* 33 - 40 */ + 14, 14, 14, 14, 14, 14, 14, 14, /* 41 - 48 */ + 15, 15, 15, 15, 15, 15, 15, 15, /* 49 - 56 */ + 15, 15, 15, 15, 15, 15, 15, 15}; /* 57 - 64 */ + +/* map the sanitized data length to an appropriate data length code */ +unsigned char can_fd_len2dlc(unsigned char len) +{ + if (len > 64) + return 0xF; + + return len2dlc[len]; +} + void state_raw(void) { @@ -62,7 +95,14 @@ void state_raw(void) state = STATE_SHUTDOWN; return; } - + + int enable = 1; + if (setsockopt(raw_socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable, sizeof(enable)) < 0) { + PRINT_ERROR("Could not enable CAN FD\n"); + state = STATE_SHUTDOWN; + return; + } + if (error_mask != 0) { can_err_mask_t err_mask = (error_mask & CAN_ERR_MASK); if (setsockopt(raw_socket, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask, sizeof(err_mask)) < 0) { @@ -142,7 +182,7 @@ void state_raw(void) ret = sprintf(buf, "< frame %03X %lld.%06lld ", frame.can_id & CAN_SFF_MASK, (signed long long) tv.tv_sec, (signed long long) tv.tv_usec); } - for (i = 0; i < frame.can_dlc; i++) { + for (i = 0; i < frame.len; i++) { ret += sprintf(buf + ret, "%02X", frame.data[i]); } sprintf(buf + ret, " >"); @@ -171,37 +211,50 @@ void state_raw(void) } /* Send a single frame */ - if (!strncmp("< send ", buf, 7)) { - items = sscanf(buf, "< %*s %x %hhu " - "%hhx %hhx %hhx %hhx %hhx %hhx " - "%hhx %hhx >", - &frame.can_id, - &frame.can_dlc, - &frame.data[0], - &frame.data[1], - &frame.data[2], - &frame.data[3], - &frame.data[4], - &frame.data[5], - &frame.data[6], - &frame.data[7]); + if (!strncmp("< send ", buf, 7)) { + //int id_len = element_length(buf, 2); + frame.can_id = 0; + frame.flags = CANFD_FDF; + long long unsigned int header_id = 0; + items = sscanf(buf, "< %*s %llx %hhu" + "%hhx %hhx %hhx %hhx %hhx %hhx %hhx %hhx" + "%hhx %hhx %hhx %hhx %hhx %hhx %hhx %hhx" + "%hhx %hhx %hhx %hhx %hhx %hhx %hhx %hhx" + "%hhx %hhx %hhx %hhx %hhx %hhx %hhx %hhx" + "%hhx %hhx %hhx %hhx %hhx %hhx %hhx %hhx" + "%hhx %hhx %hhx %hhx %hhx %hhx %hhx %hhx" + "%hhx %hhx %hhx %hhx %hhx %hhx %hhx %hhx" + "%hhx %hhx %hhx %hhx %hhx %hhx %hhx %hhx >", + &header_id, + &frame.len, + &frame.data[0],&frame.data[1],&frame.data[2],&frame.data[3],&frame.data[4],&frame.data[5],&frame.data[6],&frame.data[7], + &frame.data[8],&frame.data[9],&frame.data[10],&frame.data[11],&frame.data[12],&frame.data[13],&frame.data[14],&frame.data[15], + &frame.data[16],&frame.data[17],&frame.data[18],&frame.data[19],&frame.data[20],&frame.data[21],&frame.data[22],&frame.data[23], + &frame.data[24],&frame.data[25],&frame.data[26],&frame.data[27],&frame.data[28],&frame.data[29],&frame.data[30],&frame.data[31], + &frame.data[32],&frame.data[33],&frame.data[34],&frame.data[35],&frame.data[36],&frame.data[37],&frame.data[38],&frame.data[39], + &frame.data[40],&frame.data[41],&frame.data[42],&frame.data[43],&frame.data[44],&frame.data[45],&frame.data[46],&frame.data[47], + &frame.data[48],&frame.data[49],&frame.data[50],&frame.data[51],&frame.data[52],&frame.data[53],&frame.data[54],&frame.data[55], + &frame.data[56],&frame.data[57],&frame.data[58],&frame.data[59],&frame.data[60],&frame.data[61],&frame.data[62],&frame.data[63]); if ((items < 2) || - (frame.can_dlc > 8) || - (items != 2 + frame.can_dlc)) { - PRINT_ERROR("Syntax error in send command\n"); + (frame.len > CANFD_MAX_DLEN) || + (items != 2 + frame.len)) { + strcpy(buf, "< Syntax error in send command >"); + send(client_socket, buf, strlen(buf), 0); + tcp_quickack(client_socket); + PRINT_ERROR("Syntax error in send command\n"); return; } - + frame.can_id |= (header_id&0xFFFFFFFF); /* < send XXXXXXXX ... > check for extended identifier */ - if (element_length(buf, 2) == 8) + if (frame.can_id > 0x7FF) frame.can_id |= CAN_EFF_FLAG; - - ret = send(raw_socket, &frame, sizeof(struct can_frame), 0); + frame.len = can_fd_dlc2len(can_fd_len2dlc(frame.len)); + ret = send(raw_socket, &frame, sizeof(frame), 0); if (ret == -1) { state = STATE_SHUTDOWN; return; - } + } } else { PRINT_ERROR("unknown command '%s'\n", buf); From 15e9539a7586356fbd2d4a57dcab0b96714a5185 Mon Sep 17 00:00:00 2001 From: victors21 Date: Thu, 5 Jun 2025 19:18:07 +0300 Subject: [PATCH 2/2] added support for canfd frames for SavvyCan V220 app with new option like -f (allow CAN-FD frames in socket raw mode. Use only if your harware support it) --- src/socketcand.c | 12 +++++++++--- src/socketcand.h | 1 + src/state_raw.c | 14 +++++++------- 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/src/socketcand.c b/src/socketcand.c index 31be3e8..e1975e0 100644 --- a/src/socketcand.c +++ b/src/socketcand.c @@ -64,6 +64,7 @@ int cmd_index = 0; char *description; char *afuxname; int more_elements = 0; +int can_fd_mode_flag = 0; can_err_mask_t error_mask = 0; struct sockaddr_in saddr, broadcast_addr; struct sockaddr_un unaddr; @@ -214,6 +215,7 @@ int main(int argc, char **argv) { "version", no_argument, 0, 'z' }, { "no-beacon", no_argument, 0, 'n' }, { "error-mask", required_argument, 0, 'e' }, + { "can-fd", no_argument, 0, 'f' }, { "help", no_argument, 0, 'h' }, { 0, 0, 0, 0 } }; @@ -278,11 +280,14 @@ int main(int argc, char **argv) case 'h': print_usage(); return 0; - + + case 'f': + can_fd_mode_flag = 1; + break; + case '?': print_usage(); return 0; - default: print_usage(); return -1; @@ -648,7 +653,7 @@ void print_usage(void) { printf("%s Version %s\n", PACKAGE_NAME, PACKAGE_VERSION); printf("Report bugs to %s\n\n", PACKAGE_BUGREPORT); - printf("Usage: socketcand [-v | --verbose] [-i interfaces | --interfaces interfaces]\n\t\t[-p port | --port port] [-q | --quick-ack]\n\t\t[-l interface | --listen interface] [-u name | --afuxname name]\n\t\t[-e error_mask | --error-mask error_mask]\n\t\t[-n | --no-beacon] [-d | --daemon] [-h | --help]\n\n"); + printf("Usage: socketcand [-v | --verbose] [-i interfaces | --interfaces interfaces]\n\t\t[-p port | --port port] [-q | --quick-ack]\n\t\t[-l interface | --listen interface] [-u name | --afuxname name]\n\t\t[-e error_mask | --error-mask error_mask]\n\t\t[-n | --no-beacon] [-f | --can-fd] [-d | --daemon] [-h | --help]\n\n"); printf("Options:\n"); printf("\t-v (activates verbose output to STDOUT)\n"); printf("\t-i (comma separated list of CAN interfaces the daemon\n\t\tshall provide access to e.g. '-i can0,vcan1' - default: %s)\n", DEFAULT_BUSNAME); @@ -657,6 +662,7 @@ void print_usage(void) printf("\t-l (changes the default network interface the daemon will\n\t\tbind to - default: %s)\n", DEFAULT_INTERFACE); printf("\t-u (the AF_UNIX socket path - an abstract name is used when\n\t\tthe leading '/' is missing. N.B. the AF_UNIX binding will\n\t\tsupersede the port/interface settings)\n"); printf("\t-n (deactivates the discovery beacon)\n"); + printf("\t-f (allow CAN-FD frames in socket raw mode. Use only if your harware support it)\n"); printf("\t-e (enable CAN error frames in raw mode providing an\n\t\thexadecimal error mask, e.g: 0x1FFFFFFF)\n"); printf("\t-d (set this flag if you want log to syslog instead of STDOUT)\n"); printf("\t-h (prints this message)\n"); diff --git a/src/socketcand.h b/src/socketcand.h index 0c46262..68c6ce0 100644 --- a/src/socketcand.h +++ b/src/socketcand.h @@ -61,6 +61,7 @@ extern int interface_count; extern int port; extern int verbose_flag; extern int daemon_flag; +extern int can_fd_mode_flag; extern int state; extern int previous_state; extern char bus_name[]; diff --git a/src/state_raw.c b/src/state_raw.c index b4cc755..a1860f2 100644 --- a/src/state_raw.c +++ b/src/state_raw.c @@ -95,14 +95,14 @@ void state_raw(void) state = STATE_SHUTDOWN; return; } - - int enable = 1; - if (setsockopt(raw_socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable, sizeof(enable)) < 0) { - PRINT_ERROR("Could not enable CAN FD\n"); - state = STATE_SHUTDOWN; - return; + if (can_fd_mode_flag != 0) { + int enable = 1; + if (setsockopt(raw_socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable, sizeof(enable)) < 0) { + PRINT_ERROR("Could not enable CAN FD\n"); + state = STATE_SHUTDOWN; + return; + } } - if (error_mask != 0) { can_err_mask_t err_mask = (error_mask & CAN_ERR_MASK); if (setsockopt(raw_socket, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask, sizeof(err_mask)) < 0) {