diff --git a/doc/protocol.md b/doc/protocol.md index 62b651d..6d7d1b5 100644 --- a/doc/protocol.md +++ b/doc/protocol.md @@ -172,6 +172,35 @@ Reception of a CAN frame with CAN ID 0x123 , data length 4 and data 0x1A, 0x22, < frame 123 23.424242 1A220344 > +#### CAN FD frames #### +If the CAN interface supports CAN FD (i.e. its MTU is at least `CANFD_MTU`, e.g. `ip link set can0 type can bitrate 500000 dbitrate 2000000 fd on` or `ip link set vcan0 mtu 72`), CAN FD frames are supported in RAW mode as well. Classic CAN frames keep using the messages described above, so clients that only support classic CAN are not affected. + +Received CAN FD frames are sent in the following format: + + < fdframe can_id flags seconds.useconds [data]* > + +* can_id - CAN identifier (3 hex digits for standard, 8 hex digits for extended identifiers) +* flags - hex value of the CAN FD flags: `CANFD_BRS` (0x01, bit rate switch) and `CANFD_ESI` (0x02, error state indicator) +* data - up to 64 bytes, padded uppercase hex without spaces inbetween + +Example: +Reception of a CAN FD frame with CAN ID 0x18DAF110 and bit rate switch, data length 12 at time 23.424242. + + < fdframe 18DAF110 01 23.424242 000A62F19031323334353637 > + +A single CAN FD frame is sent with the 'fdsend' command. The data is lowercase unpadded hex with spaces inbetween. The command is rejected if the CAN interface does not support CAN FD. + + < fdsend can_id flags len [data]* > + +* can_id - CAN identifier; as for 'send', 8 hex digits select an extended identifier +* flags - hex value of the CAN FD flags `CANFD_BRS` (0x01) and `CANFD_ESI` (0x02) +* len - number of data bytes (0 .. 64). Payloads that are not a valid CAN FD data length (0 .. 8, 12, 16, 20, 24, 32, 48, 64) are padded with zeros up to the next valid length. + +Example: +Send a CAN FD frame with ID 0x123, bit rate switch, data length 12 + + < fdsend 123 01 12 11 22 33 44 55 66 77 88 99 aa bb cc > + #### Switch to BCM mode #### With '< bcmmode >' it is possible to switch back to BCM mode. diff --git a/src/state_raw.c b/src/state_raw.c index 8b40611..614d1b1 100644 --- a/src/state_raw.c +++ b/src/state_raw.c @@ -27,11 +27,26 @@ 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 int canfd_on; + +/* round up to the next valid CAN FD data length (0..8, 12, 16, 20, 24, 32, 48, 64) */ +static __u8 canfd_valid_len(unsigned int len) +{ + static const __u8 valid_len[] = { 8, 12, 16, 20, 24, 32, 48, 64 }; + unsigned int i; + + for (i = 0; i < sizeof(valid_len); i++) { + if (len <= valid_len[i]) + return len <= CAN_MAX_DLEN ? len : valid_len[i]; + } + + return CANFD_MAX_DLEN; +} void state_raw(void) { @@ -56,6 +71,23 @@ void state_raw(void) addr.can_family = AF_CAN; addr.can_ifindex = ifr.ifr_ifindex; + /* + * Enable CAN FD frames if the interface supports them (CAN FD + * capable interfaces have at least CANFD_MTU). On classic CAN + * interfaces only CAN_MTU sized frames are exchanged; the + * canfd_frame buffer is layout-compatible with struct can_frame. + */ + canfd_on = 0; + if (ioctl(raw_socket, SIOCGIFMTU, &ifr) == 0 && ifr.ifr_mtu >= (int)CANFD_MTU) { + const int canfd_frames = 1; + if (setsockopt(raw_socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_frames, sizeof(canfd_frames)) < 0) { + PRINT_ERROR("Could not enable CAN FD frames\n"); + state = STATE_SHUTDOWN; + return; + } + canfd_on = 1; + } + const int timestamp_on = 1; if (setsockopt(raw_socket, SOL_SOCKET, SO_TIMESTAMP, ×tamp_on, sizeof(timestamp_on)) < 0) { PRINT_ERROR("Could not enable CAN timestamps\n"); @@ -114,7 +146,7 @@ void state_raw(void) msg.msg_controllen = sizeof(ctrlmsg); ret = recvmsg(raw_socket, &msg, 0); - if (ret < sizeof(struct can_frame)) { + if (ret != (int)CAN_MTU && ret != (int)CANFD_MTU) { PRINT_ERROR("Error reading frame from RAW socket\n"); } else { /* read timestamp data */ @@ -134,6 +166,23 @@ void state_raw(void) tcp_quickack(client_socket); } else if (frame.can_id & CAN_RTR_FLAG) { /* TODO implement */ + } else if (ret == (int)CANFD_MTU) { + /* CAN FD frame: < fdframe can_id flags seconds.useconds [data]* > */ + __u8 flags = frame.flags & (CANFD_BRS | CANFD_ESI); + + if (frame.can_id & CAN_EFF_FLAG) { + ret = sprintf(buf, "< fdframe %08X %02X %lld.%06lld ", frame.can_id & CAN_EFF_MASK, + flags, (signed long long) tv.tv_sec, (signed long long) tv.tv_usec); + } else { + ret = sprintf(buf, "< fdframe %03X %02X %lld.%06lld ", frame.can_id & CAN_SFF_MASK, + flags, (signed long long) tv.tv_sec, (signed long long) tv.tv_usec); + } + for (i = 0; i < frame.len && i < CANFD_MAX_DLEN; i++) { + ret += sprintf(buf + ret, "%02X", frame.data[i]); + } + sprintf(buf + ret, " >"); + send(client_socket, buf, strlen(buf), 0); + tcp_quickack(client_socket); } else { if (frame.can_id & CAN_EFF_FLAG) { ret = sprintf(buf, "< frame %08X %lld.%06lld ", frame.can_id & CAN_EFF_MASK, @@ -142,7 +191,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, " >"); @@ -172,11 +221,12 @@ void state_raw(void) /* Send a single frame */ if (!strncmp("< send ", buf, 7)) { + memset(&frame, 0, sizeof(frame)); items = sscanf(buf, "< %*s %x %hhu " "%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], @@ -187,8 +237,8 @@ void state_raw(void) &frame.data[7]); if ((items < 2) || - (frame.can_dlc > 8) || - (items != 2 + frame.can_dlc)) { + (frame.len > CAN_MAX_DLEN) || + (items != 2 + frame.len)) { PRINT_ERROR("Syntax error in send command\n"); return; } @@ -197,7 +247,64 @@ void state_raw(void) if (element_length(buf, 2) == 8) frame.can_id |= CAN_EFF_FLAG; - ret = send(raw_socket, &frame, sizeof(struct can_frame), 0); + ret = send(raw_socket, &frame, CAN_MTU, 0); + if (ret == -1) { + state = STATE_SHUTDOWN; + return; + } + + /* Send a single CAN FD frame */ + } else if (!strncmp("< fdsend ", buf, 9)) { + unsigned int can_id, flags, len; + int pos = 0, n = 0; + + if (!canfd_on) { + PRINT_ERROR("CAN FD is not supported by bus %s\n", bus_name); + return; + } + + memset(&frame, 0, sizeof(frame)); + items = sscanf(buf, "< %*s %x %x %u %n", &can_id, &flags, &len, &pos); + if ((items != 3) || + (len > CANFD_MAX_DLEN) || + (flags & ~(CANFD_BRS | CANFD_ESI))) { + PRINT_ERROR("Syntax error in fdsend command\n"); + return; + } + + for (i = 0; i < (int)len; i++) { + if (sscanf(buf + pos, "%hhx %n", &frame.data[i], &n) != 1) { + PRINT_ERROR("Syntax error in fdsend command\n"); + return; + } + pos += n; + } + + if (strcmp(buf + pos, ">")) { + PRINT_ERROR("Syntax error in fdsend command\n"); + return; + } + + /* < fdsend XXXXXXXX ... > check for extended identifier */ + if (element_length(buf, 2) == 8) { + if (can_id > CAN_EFF_MASK) { + PRINT_ERROR("Syntax error in fdsend command\n"); + return; + } + frame.can_id = can_id | CAN_EFF_FLAG; + } else { + if (can_id > CAN_SFF_MASK) { + PRINT_ERROR("Syntax error in fdsend command\n"); + return; + } + frame.can_id = can_id; + } + + /* pad with zeros up to the next valid CAN FD data length */ + frame.len = canfd_valid_len(len); + frame.flags = flags | CANFD_FDF; + + ret = send(raw_socket, &frame, CANFD_MTU, 0); if (ret == -1) { state = STATE_SHUTDOWN; return;