Skip to content

Commit

Permalink
rawmode: support CAN FD frames
Browse files Browse the repository at this point in the history 
As requested by #12
this patch adds the possibility to send and receive CAN FD frames
in the CAN_RAW mode "< rawmode >".

Signed-off-by: Oliver Hartkopp <[email protected]>
  • Loading branch information
@hartkopp
hartkopp committed May 15, 2020
1 parent 985ec67 commit 7f894ba
Show file tree
Hide file tree
Showing 3 changed files with 107 additions and 9 deletions.
31 changes: 31 additions & 0 deletions socketcand.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -189,6 +189,37 @@ int asc2nibble(char c)
return 16; /* error */
}

/* 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 can_dlc with sanitized can_dlc */
unsigned char can_dlc2len(unsigned char can_dlc)
{
return dlc2len[can_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_len2dlc(unsigned char len)
{
if (len > 64)
return 0xF;

return len2dlc[len];
}

int main(int argc, char **argv)
{
int i, found;
Expand Down
8 changes: 8 additions & 0 deletions socketcand.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,3 +57,11 @@ int state_changed(char *buf, int current_state);
char *element_start(char *buf, int element);
int element_length(char *buf, int element);
int asc2nibble(char c);

/* CAN DLC to real data length conversion helpers especially for CAN FD */

/* get data length from can_dlc with sanitized can_dlc */
unsigned char can_dlc2len(unsigned char can_dlc);

/* map the sanitized data length to an appropriate data length code */
unsigned char can_len2dlc(unsigned char len);
77 changes: 68 additions & 9 deletions state_raw.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,13 +21,14 @@
#include <syslog.h>

#include <linux/can.h>
#include <linux/can/raw.h>

int raw_socket;
struct ifreq ifr;
struct sockaddr_can addr;
fd_set readfds;
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;
Expand Down Expand Up @@ -62,6 +63,10 @@ void state_raw() {
return;
}

/* try to switch the socket into CAN FD mode */
const int canfd_on = 1;
setsockopt(raw_socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on));

if(bind(raw_socket, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
PRINT_ERROR("Error while binding RAW socket %s\n", strerror(errno));
state = STATE_SHUTDOWN;
Expand Down Expand Up @@ -98,13 +103,15 @@ void state_raw() {
}

if(FD_ISSET(raw_socket, &readfds)) {
char *fd = "";

iov.iov_len = sizeof(frame);
msg.msg_namelen = sizeof(addr);
msg.msg_flags = 0;
msg.msg_controllen = sizeof(ctrlmsg);

ret = recvmsg(raw_socket, &msg, 0);
if(ret < sizeof(struct can_frame)) {
if((ret != CAN_MTU) && (ret != CANFD_MTU)) {
PRINT_ERROR("Error reading frame from RAW socket\n")
} else {
/* read timestamp data */
Expand All @@ -116,6 +123,9 @@ void state_raw() {
}
}

if(ret == CANFD_MTU)
fd = "fd";

if(frame.can_id & CAN_ERR_FLAG) {
canid_t class = frame.can_id & CAN_EFF_MASK;
ret = sprintf(buf, "< error %03X %ld.%06ld >", class, tv.tv_sec, tv.tv_usec);
Expand All @@ -124,11 +134,11 @@ void state_raw() {
/* 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);
ret = sprintf(buf, "< %sframe %08X %ld.%06ld ", fd, 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);
ret = sprintf(buf, "< %sframe %03X %ld.%06ld ", fd, frame.can_id & CAN_SFF_MASK, 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, " >");
Expand Down Expand Up @@ -160,7 +170,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],
Expand All @@ -171,8 +181,8 @@ void state_raw() {
&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;
}
Expand All @@ -182,7 +192,56 @@ void state_raw() {
frame.can_id |= CAN_EFF_FLAG;

ret = send(raw_socket, &frame, sizeof(struct can_frame), 0);
if(ret==-1) {
if(ret < 0) {
state = STATE_SHUTDOWN;
return;
}

} else if(!strncmp("< sendfd ", buf, 9)) { /* Send a single CAN FD frame */

/* ok, this doesn't win a coding style price */
items = sscanf(buf, "< %*s %x %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 >",
&frame.can_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.len > CANFD_MAX_DLEN) ||
(items != 2 + frame.len) ||
(frame.len != can_dlc2len(can_len2dlc(frame.len)))) {
PRINT_ERROR("Syntax error in sendfd command\n")
return;
}

/* < send 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 < 0) {
state = STATE_SHUTDOWN;
return;
}
Expand Down

0 comments on commit 7f894ba

Please sign in to comment.