diff --git a/src/socketcandcl.c b/src/socketcandcl.c index 50716eb..183223d 100644 --- a/src/socketcandcl.c +++ b/src/socketcandcl.c @@ -34,6 +34,7 @@ #include #include +#include #define MAXLEN 4000 #define PORT 29536 @@ -266,7 +267,7 @@ int main(int argc, char **argv) inline void state_connected() { int ret; - static struct can_frame frame; + static struct canfd_frame frame; static struct ifreq ifr; static struct sockaddr_can addr; fd_set readfds; @@ -296,6 +297,22 @@ inline void state_connected() state = STATE_SHUTDOWN; return; } + + if (ioctl(raw_socket,SIOCGIFMTU,&ifr) < 0) { + PRINT_ERROR("Error while searching for bus MTU %s\n", strerror(errno)); + state = STATE_SHUTDOWN; + return; + } + + if (ifr.ifr_mtu == CANFD_MTU) { + const int canfd_on = 1; + if (setsockopt(raw_socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on)) < 0) { + PRINT_ERROR("Could not enable CAN FD support\n"); + state = STATE_SHUTDOWN; + return; + } + } + /* bind socket */ if (bind(raw_socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) { PRINT_ERROR("Error while binding RAW socket %s\n", strerror(errno)); @@ -342,7 +359,7 @@ inline void state_connected() if ((s - buf - 7) > 4) frame.can_id |= CAN_EFF_FLAG; - frame.can_dlc = strlen(data_str) / 2; + frame.len = strlen(data_str) / 2; sscanf(data_str, "%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx", &frame.data[0], &frame.data[1], @@ -351,7 +368,55 @@ inline void state_connected() &frame.data[6], &frame.data[7]); ret = write(raw_socket, &frame, sizeof(struct can_frame)); - if (ret < sizeof(struct can_frame)) { + if (ret != sizeof(struct can_frame)) { + perror("Writing CAN frame to can socket\n"); + } + } else if (!strncmp("< fdframe", buf, 9)) { + char data_str[2*64]; + + sscanf(buf, "< fdframe %x %hhx %*d.%*d %s >", &frame.can_id, &frame.flags, + data_str); + + char* s = buf + 9; + for (; ++s;) { + if (*s== ' ') { + break; + } + } + if ((s - buf - 9) > 4) + frame.can_id |= CAN_EFF_FLAG; + + frame.len = strlen(data_str) / 2; + + sscanf(data_str, + "%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx" + "%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx" + "%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx" + "%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx" + "%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx" + "%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx" + "%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx" + "%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx", + &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] + ); + + ret = write(raw_socket, &frame, sizeof(struct canfd_frame)); + if (ret != sizeof(struct canfd_frame)) { perror("Writing CAN frame to can socket\n"); } } @@ -377,7 +442,7 @@ inline void state_connected() } if (FD_ISSET(raw_socket, &readfds)) { - ret = recv(raw_socket, &frame, sizeof(struct can_frame), MSG_WAITALL); + ret = recv(raw_socket, &frame, sizeof(struct canfd_frame), MSG_WAITALL); if (ret < sizeof(struct can_frame)) { PRINT_ERROR("Error reading frame from RAW socket\n"); perror("Reading CAN socket\n"); @@ -388,14 +453,25 @@ inline void state_connected() /* TODO implement */ } else { int i; - if (frame.can_id & CAN_EFF_FLAG) { - ret = sprintf(buf, "< send %08X %d ", - frame.can_id & CAN_EFF_MASK, frame.can_dlc); - } else { - ret = sprintf(buf, "< send %03X %d ", - frame.can_id & CAN_SFF_MASK, frame.can_dlc); + + if (ret == sizeof(struct can_frame)) { + if (frame.can_id & CAN_EFF_FLAG) { + ret = sprintf(buf, "< send %08X %d ", + frame.can_id & CAN_EFF_MASK, frame.len); + } else { + ret = sprintf(buf, "< send %03X %d ", + frame.can_id & CAN_SFF_MASK, frame.len); + } + } else if (ret == sizeof(struct canfd_frame)) { + if (frame.can_id & CAN_EFF_FLAG) { + ret = sprintf(buf, "< fdsend %08X %02X %d ", + frame.can_id & CAN_EFF_MASK, frame.flags, frame.len); + } else { + ret = sprintf(buf, "< fdsend %03X %02X %d ", + frame.can_id & CAN_SFF_MASK, frame.flags, frame.len); + } } - 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, " >"); diff --git a/src/state_bcm.c b/src/state_bcm.c index 993e66f..ac4a712 100644 --- a/src/state_bcm.c +++ b/src/state_bcm.c @@ -25,7 +25,7 @@ #include #include -#define RXLEN 128 +#define RXLEN 256 int sc = -1; @@ -41,7 +41,7 @@ void state_bcm() struct { struct bcm_msg_head msg_head; - struct can_frame frame; + struct canfd_frame frame; } msg; struct { @@ -103,12 +103,12 @@ void state_bcm() /* Check if this is an error frame */ if (msg.msg_head.can_id & CAN_ERR_FLAG) { - if (msg.frame.can_dlc != CAN_ERR_DLC) { + if (msg.frame.len != CAN_ERR_DLC) { PRINT_ERROR("Error frame has a wrong DLC!\n"); } else { snprintf(rxmsg, RXLEN, "< error %03X %ld.%06ld ", msg.msg_head.can_id, tv.tv_sec, tv.tv_usec); - for (i = 0; i < msg.frame.can_dlc; i++) + for (i = 0; i < msg.frame.len; i++) snprintf(rxmsg + strlen(rxmsg), RXLEN - strlen(rxmsg), "%02X ", msg.frame.data[i]); @@ -117,15 +117,33 @@ void state_bcm() tcp_quickack(client_socket); } } else { - if (msg.msg_head.can_id & CAN_EFF_FLAG) { - snprintf(rxmsg, RXLEN, "< frame %08X %ld.%06ld ", - msg.msg_head.can_id & CAN_EFF_MASK, tv.tv_sec, tv.tv_usec); - } else { - snprintf(rxmsg, RXLEN, "< frame %03X %ld.%06ld ", - msg.msg_head.can_id & CAN_SFF_MASK, tv.tv_sec, tv.tv_usec); + switch(ret) { + // if the frame is a classic CAN frame + case sizeof(struct can_frame): + if (msg.msg_head.can_id & CAN_EFF_FLAG) { + snprintf(rxmsg, RXLEN, "< frame %08X %ld.%06ld ", + msg.msg_head.can_id & CAN_EFF_MASK, tv.tv_sec, tv.tv_usec); + } else { + snprintf(rxmsg, RXLEN, "< frame %03X %ld.%06ld ", + msg.msg_head.can_id & CAN_SFF_MASK, tv.tv_sec, tv.tv_usec); + } + break; + // if the frame is a CAN FD frame + case sizeof(struct canfd_frame): + if (msg.msg_head.can_id & CAN_EFF_FLAG) { + snprintf(rxmsg, RXLEN, "< fdframe %08X %02X %ld.%06ld ", + msg.msg_head.can_id & CAN_EFF_MASK, msg.msg_head.flags, tv.tv_sec, tv.tv_usec); + } else { + snprintf(rxmsg, RXLEN, "< fdframe %03X %02X %ld.%06ld ", + msg.msg_head.can_id & CAN_SFF_MASK, msg.msg_head.flags, tv.tv_sec, tv.tv_usec); + } + break; + default: + PRINT_ERROR("Unknown frame size %d\n", ret); + return; } - for (i = 0; i < msg.frame.can_dlc; i++) + for (i = 0; i < msg.frame.len; i++) snprintf(rxmsg + strlen(rxmsg), RXLEN - strlen(rxmsg), "%02X ", msg.frame.data[i]); @@ -172,7 +190,7 @@ void state_bcm() "%hhx %hhx %hhx %hhx %hhx %hhx " "%hhx %hhx >", &msg.msg_head.can_id, - &msg.frame.can_dlc, + &msg.frame.len, &msg.frame.data[0], &msg.frame.data[1], &msg.frame.data[2], @@ -183,8 +201,8 @@ void state_bcm() &msg.frame.data[7]); if ((items < 2) || - (msg.frame.can_dlc > 8) || - (items != 2 + msg.frame.can_dlc)) { + (msg.frame.len > 8) || + (items != 2 + msg.frame.len)) { PRINT_ERROR("Syntax error in send command\n"); return; } @@ -202,6 +220,72 @@ void state_bcm() (struct sockaddr *)&caddr, sizeof(caddr)); } /* Add a send job */ + } else if (!strncmp("< fdsend ", buf, 9)) { + // First, read the fixed part of the frame + items = sscanf(buf, "< %*s %x %hhx %hhu", + &msg.msg_head.can_id, + &msg.frame.flags, + &msg.frame.len); + + if (items != 3) { + PRINT_ERROR("Syntax error in fdsend command\n"); + return; + } + + // Ensure frame.len does not exceed the maximum allowed length + if (msg.frame.len > 64) { + PRINT_ERROR("Frame length exceeds maximum allowed length\n"); + return; + } + + // Construct the format string for the dynamically based on frame.len + char format[512]; + snprintf(format, sizeof(format), "< %%*s %%x %%hhx %%hhu"); + for (int i = 0; i < msg.frame.len; i++) { + strncat(format, " %hhx", sizeof(format) - strlen(format) - 1); + } + strncat(format, " >", sizeof(format) - strlen(format) - 1); + + // Read the variable-length frame.data + items = sscanf(buf, format, + &msg.msg_head.can_id, + &msg.frame.flags, + &msg.frame.len, + &msg.frame.data[0], &msg.frame.data[1], &msg.frame.data[2], &msg.frame.data[3], + &msg.frame.data[4], &msg.frame.data[5], &msg.frame.data[6], &msg.frame.data[7], + &msg.frame.data[8], &msg.frame.data[9], &msg.frame.data[10], &msg.frame.data[11], + &msg.frame.data[12], &msg.frame.data[13], &msg.frame.data[14], &msg.frame.data[15], + &msg.frame.data[16], &msg.frame.data[17], &msg.frame.data[18], &msg.frame.data[19], + &msg.frame.data[20], &msg.frame.data[21], &msg.frame.data[22], &msg.frame.data[23], + &msg.frame.data[24], &msg.frame.data[25], &msg.frame.data[26], &msg.frame.data[27], + &msg.frame.data[28], &msg.frame.data[29], &msg.frame.data[30], &msg.frame.data[31], + &msg.frame.data[32], &msg.frame.data[33], &msg.frame.data[34], &msg.frame.data[35], + &msg.frame.data[36], &msg.frame.data[37], &msg.frame.data[38], &msg.frame.data[39], + &msg.frame.data[40], &msg.frame.data[41], &msg.frame.data[42], &msg.frame.data[43], + &msg.frame.data[44], &msg.frame.data[45], &msg.frame.data[46], &msg.frame.data[47], + &msg.frame.data[48], &msg.frame.data[49], &msg.frame.data[50], &msg.frame.data[51], + &msg.frame.data[52], &msg.frame.data[53], &msg.frame.data[54], &msg.frame.data[55], + &msg.frame.data[56], &msg.frame.data[57], &msg.frame.data[58], &msg.frame.data[59], + &msg.frame.data[60], &msg.frame.data[61], &msg.frame.data[62], &msg.frame.data[63]); + + if ((items < 2) || + (msg.frame.len > 64) || + (items != 3 + msg.frame.len)) { + PRINT_ERROR("Syntax error in fdsend command\n"); + return; + } + + /* < fdsend XXXXXXXX ... > check for extended identifier */ + if (element_length(buf, 2) == 8) + msg.msg_head.can_id |= CAN_EFF_FLAG; + + msg.msg_head.opcode = TX_SEND; + msg.frame.can_id = msg.msg_head.can_id; + + if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { + caddr.can_ifindex = ifr.ifr_ifindex; + sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*)&caddr, sizeof(caddr)); + } } else if (!strncmp("< add ", buf, 6)) { items = sscanf(buf, "< %*s %lu %lu %x %hhu " "%hhx %hhx %hhx %hhx %hhx %hhx " @@ -209,7 +293,7 @@ void state_bcm() &msg.msg_head.ival2.tv_sec, &msg.msg_head.ival2.tv_usec, &msg.msg_head.can_id, - &msg.frame.can_dlc, + &msg.frame.len, &msg.frame.data[0], &msg.frame.data[1], &msg.frame.data[2], @@ -220,8 +304,8 @@ void state_bcm() &msg.frame.data[7]); if ((items < 4) || - (msg.frame.can_dlc > 8) || - (items != 4 + msg.frame.can_dlc)) { + (msg.frame.len > 8) || + (items != 4 + msg.frame.len) ) { PRINT_ERROR("Syntax error in add command.\n"); return; } @@ -245,7 +329,7 @@ void state_bcm() "%hhx %hhx %hhx %hhx %hhx %hhx " "%hhx %hhx >", &msg.msg_head.can_id, - &msg.frame.can_dlc, + &msg.frame.len, &msg.frame.data[0], &msg.frame.data[1], &msg.frame.data[2], @@ -256,8 +340,8 @@ void state_bcm() &msg.frame.data[7]); if ((items < 2) || - (msg.frame.can_dlc > 8) || - (items != 2 + msg.frame.can_dlc)) { + (msg.frame.len > 8) || + (items != 2 + msg.frame.len)) { PRINT_ERROR("Syntax error in update send job command\n"); return; } @@ -305,7 +389,7 @@ void state_bcm() &msg.msg_head.ival2.tv_sec, &msg.msg_head.ival2.tv_usec, &msg.msg_head.can_id, - &msg.frame.can_dlc, + &msg.frame.len, &msg.frame.data[0], &msg.frame.data[1], &msg.frame.data[2], @@ -316,8 +400,8 @@ void state_bcm() &msg.frame.data[7]); if ((items < 4) || - (msg.frame.can_dlc > 8) || - (items != 4 + msg.frame.can_dlc)) { + (msg.frame.len > 8) || + (items != 4 + msg.frame.len) ) { PRINT_ERROR("syntax error in filter command.\n"); return; } diff --git a/src/state_raw.c b/src/state_raw.c index 8246f1f..7d4d335 100644 --- a/src/state_raw.c +++ b/src/state_raw.c @@ -21,12 +21,13 @@ #include #include +#include int raw_socket; struct ifreq ifr; struct sockaddr_can addr; struct msghdr msg; -struct can_frame frame; +struct canfd_frame frame; struct iovec iov; char ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(__u32))]; struct timeval tv; @@ -62,7 +63,26 @@ void state_raw() return; } - if (bind(raw_socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + if(ioctl(raw_socket,SIOCGIFMTU,&ifr) < 0) { + PRINT_ERROR("Error while searching for bus MTU %s\n", strerror(errno)); + state = STATE_SHUTDOWN; + return; + } + + /* + *if the device supports CAN FD, use it + * if you don't want to use CAN FD, you should initialize the device as classic CAN + */ + if (ifr.ifr_mtu == CANFD_MTU) { + const int canfd_on = 1; + if(setsockopt(raw_socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on)) < 0) { + PRINT_ERROR("Could not enable CAN FD support\n"); + state = STATE_SHUTDOWN; + return; + } + } + + if(bind(raw_socket, (struct sockaddr *) &addr, sizeof(addr)) < 0) { PRINT_ERROR("Error while binding RAW socket %s\n", strerror(errno)); state = STATE_SHUTDOWN; return; @@ -104,7 +124,7 @@ void state_raw() msg.msg_controllen = sizeof(ctrlmsg); ret = recvmsg(raw_socket, &msg, 0); - if (ret < sizeof(struct can_frame)) { + if(ret != sizeof(struct canfd_frame) && ret != sizeof(struct can_frame)) { PRINT_ERROR("Error reading frame from RAW socket\n"); } else { /* read timestamp data */ @@ -124,12 +144,25 @@ void state_raw() } else if (frame.can_id & CAN_RTR_FLAG) { /* TODO implement */ } else { - if (frame.can_id & CAN_EFF_FLAG) { - ret = sprintf(buf, "< frame %08X %ld.%06ld ", frame.can_id & CAN_EFF_MASK, tv.tv_sec, tv.tv_usec); - } else { - ret = sprintf(buf, "< frame %03X %ld.%06ld ", frame.can_id & CAN_SFF_MASK, tv.tv_sec, tv.tv_usec); + switch(ret) { + // if the frame is a classic CAN frame + case sizeof(struct can_frame): + if(frame.can_id & CAN_EFF_FLAG) { + ret = sprintf(buf, "< frame %08X %ld.%06ld ", frame.can_id & CAN_EFF_MASK, tv.tv_sec, tv.tv_usec); + } else { + ret = sprintf(buf, "< frame %03X %ld.%06ld ", frame.can_id & CAN_SFF_MASK, tv.tv_sec, tv.tv_usec); + } + break; + // if the frame is a CAN FD frame + case sizeof(struct canfd_frame): + if(frame.can_id & CAN_EFF_FLAG) { + ret = sprintf(buf, "< fdframe %08X %02X %ld.%06ld ", frame.can_id & CAN_EFF_MASK, frame.flags, tv.tv_sec, tv.tv_usec); + } else { + ret = sprintf(buf, "< fdframe %03X %02X %ld.%06ld ", frame.can_id & CAN_SFF_MASK, frame.flags, tv.tv_sec, 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, " >"); @@ -163,7 +196,7 @@ void state_raw() "%hhx %hhx %hhx %hhx %hhx %hhx " "%hhx %hhx >", &frame.can_id, - &frame.can_dlc, + &frame.len, &frame.data[0], &frame.data[1], &frame.data[2], @@ -174,8 +207,8 @@ void state_raw() &frame.data[7]); if ((items < 2) || - (frame.can_dlc > 8) || - (items != 2 + frame.can_dlc)) { + (frame.len > 8) || + (items != 2 + frame.len)) { PRINT_ERROR("Syntax error in send command\n"); return; } @@ -189,6 +222,71 @@ void state_raw() state = STATE_SHUTDOWN; return; } + /* Send a single CANFD frame */ + } else if(!strncmp("< fdsend ", buf, 9)) { + // First, read the fixed part of the frame + items = sscanf(buf, "< %*s %x %hhx %hhu", + &frame.can_id, + &frame.flags, + &frame.len); + + if (items != 3) { + PRINT_ERROR("Syntax error in fdsend command\n"); + return; + } + + // Ensure frame.len does not exceed the maximum allowed length + if (frame.len > 64) { + PRINT_ERROR("Frame length exceeds maximum allowed length\n"); + return; + } + + // Construct the format string dynamically based on frame.len + char format[512]; + snprintf(format, sizeof(format), "< %%*s %%x %%hhx %%hhu"); + for (int i = 0; i < frame.len; i++) { + strncat(format, " %hhx", sizeof(format) - strlen(format) - 1); + } + strncat(format, " >", sizeof(format) - strlen(format) - 1); + + // Read the variable-length frame.data + items = sscanf(buf, format, + &frame.can_id, + &frame.flags, + &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.len > 64) || + (items != 3 + frame.len)) { + PRINT_ERROR("Syntax error in fdsend command\n"); + return; + } + + /* < fdsend XXXXXXXX ... > check for extended identifier */ + if(element_length(buf, 2) == 8) + frame.can_id |= CAN_EFF_FLAG; + + ret = send(raw_socket, &frame, sizeof(struct canfd_frame), 0); + if(ret==-1) { + state = STATE_SHUTDOWN; + return; + } } else { PRINT_ERROR("unknown command '%s'\n", buf);