Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Canfd support #33

Open
wants to merge 10 commits into
base: master
Choose a base branch
from
Open

Canfd support #33

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
2456e52
support canfd
nanitsuku Oct 6, 2024
1ab74cc
update comments
nanitsuku Oct 12, 2024
4754078
update message
nanitsuku Oct 12, 2024
01700ea
support canfd for can raw
nanitsuku Oct 12, 2024
97947a8
fix insufficient size of buffer
nanitsuku Oct 13, 2024
0223125
fix cannot handle data size larger than length 8
nanitsuku Dec 27, 2024
e8196b2
fix should be canfd_frame
nanitsuku Dec 27, 2024
41ce96b
fix lack ";"
nanitsuku Dec 27, 2024
210e2c0
fix too short buffer size
nanitsuku Dec 27, 2024
427f76b
fix spacing & delete unnecessary lines
nanitsuku Dec 27, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
98 changes: 87 additions & 11 deletions src/socketcandcl.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
#include <arpa/inet.h>

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

#define MAXLEN 4000
#define PORT 29536
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -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],
Expand All @@ -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");
}
}
Expand All @@ -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");
Expand All @@ -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, " >");
Expand Down
130 changes: 107 additions & 23 deletions src/state_bcm.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
#include <linux/can/error.h>
#include <linux/sockios.h>

#define RXLEN 128
#define RXLEN 256

int sc = -1;

Expand All @@ -41,7 +41,7 @@ void state_bcm()

struct {
struct bcm_msg_head msg_head;
struct can_frame frame;
struct canfd_frame frame;
} msg;

struct {
Expand Down Expand Up @@ -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]);

Expand All @@ -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]);

Expand Down Expand Up @@ -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],
Expand All @@ -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;
}
Expand All @@ -202,14 +220,80 @@ 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 "
"%hhx %hhx >",
&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],
Expand All @@ -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;
}
Expand All @@ -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],
Expand All @@ -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;
}
Expand Down Expand Up @@ -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],
Expand All @@ -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;
}
Expand Down
Loading