Skip to content

Commit

Permalink
Merge pull request #36 from boschglobal/bugfix/can-fd
Browse files Browse the repository at this point in the history 
Modified CAN read/write to work with classical CAN
  • Loading branch information
@nayakned
nayakned committed Aug 16, 2024
2 parents 740951e + 3e17866 commit e7fba30
Show file tree
Hide file tree
Showing 3 changed files with 55 additions and 27 deletions.
6 changes: 6 additions & 0 deletions examples/acf-can/acf-can-common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,4 +29,10 @@

#include "avtp/acf/Can.h"

/* CAN CC/FD frame union */
typedef union {
struct can_frame cc;
struct canfd_frame fd;
} frame_t;

int setup_can_socket(const char* can_ifname, Avtp_CanVariant_t can_variant);
40 changes: 26 additions & 14 deletions examples/acf-can/acf-can-listener.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -181,7 +181,8 @@ static int new_packet(int sk_fd, int can_socket) {
uint16_t payload_length, pdu_length;
uint8_t pdu[MAX_PDU_SIZE], i;
uint8_t *cf_pdu, *acf_pdu, *udp_pdu, *can_payload;
struct canfd_frame frame;
frame_t frame;
canid_t can_id;

memset(&frame, 0, sizeof(struct canfd_frame));
res = recv(sk_fd, pdu, MAX_PDU_SIZE, 0);
Expand Down Expand Up @@ -232,41 +233,52 @@ static int new_packet(int sk_fd, int can_socket) {

Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_CAN_IDENTIFIER,
&(can_frame_id));
frame.can_id = can_frame_id;
can_id = can_frame_id;

can_payload = Avtp_Can_GetPayload((Avtp_Can_t*)acf_pdu, &payload_length, &pdu_length);
msg_proc_bytes += pdu_length*4;

// Handle EFF Flag
Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_EFF, &flag);
if (frame.can_id > 0x7FF && !flag) {
if (can_id > 0x7FF && !flag) {
fprintf(stderr, "Error: CAN ID is > 0x7FF but the EFF bit is not set.\n");
return -1;
}
if (flag) frame.can_id |= CAN_EFF_FLAG;
if (flag) can_id |= CAN_EFF_FLAG;

// Handle RTR Flag
Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_RTR, &flag);
if (flag) frame.can_id |= CAN_RTR_FLAG;
if (flag) can_id |= CAN_RTR_FLAG;

if (can_variant == AVTP_CAN_FD) {

Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_BRS, &flag);
if (flag) frame.flags |= CANFD_BRS;
if (flag) frame.fd.flags |= CANFD_BRS;

Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_FDF, &flag);
if (flag) frame.flags |= CANFD_FDF;
if (flag) frame.fd.flags |= CANFD_FDF;

Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_ESI, &flag);
if (flag) frame.flags |= CANFD_ESI;
}
if (flag) frame.fd.flags |= CANFD_ESI;

frame.len = payload_length;
memcpy(frame.data, can_payload, payload_length);
res = write(can_socket, &frame, sizeof(struct canfd_frame)) != sizeof(struct canfd_frame);
if (res < 0) {
return res;
frame.fd.can_id = can_id;
frame.fd.len = payload_length;
memcpy(frame.fd.data, can_payload, payload_length);
res = write(can_socket, &frame.fd, sizeof(struct canfd_frame));

} else {

frame.cc.can_id = can_id;
frame.cc.len = payload_length;
memcpy(frame.cc.data, can_payload, payload_length);
res = write(can_socket, &frame.cc, sizeof(struct can_frame));
}

if(res < 0)
{
perror("Failed to write to CAN bus");
return 0;
}
}
return 1;
}
Expand Down
36 changes: 23 additions & 13 deletions examples/acf-can/acf-can-talker.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -198,35 +198,41 @@ static int update_cf_length(uint8_t* cf_pdu, uint64_t length)
}

static int prepare_acf_packet(uint8_t* acf_pdu,
struct canfd_frame frame) {
frame_t frame) {

int processedBytes;
struct timespec now;
Avtp_Can_t* pdu = (Avtp_Can_t*) acf_pdu;
canid_t can_id;

// Clear bits
memset(pdu, 0, AVTP_CAN_HEADER_LEN);

// Prepare ACF PDU for CAN
Avtp_Can_Init(pdu);
clock_gettime(CLOCK_REALTIME, &now);
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_MESSAGE_TIMESTAMP,
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_MESSAGE_TIMESTAMP,
(uint64_t)now.tv_nsec + (uint64_t)(now.tv_sec * 1e9));
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_MTV, 1U);

// Set required CAN Flags
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_RTR, frame.can_id & CAN_RTR_FLAG);
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_EFF, frame.can_id & CAN_EFF_FLAG);
can_id = (can_variant == AVTP_CAN_FD) ? frame.fd.can_id : frame.cc.can_id;
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_RTR, can_id & CAN_RTR_FLAG);
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_EFF, can_id & CAN_EFF_FLAG);

if (can_variant == AVTP_CAN_FD) {
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_BRS, frame.flags & CANFD_BRS);
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_FDF, frame.flags & CANFD_FDF);
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_ESI, frame.flags & CANFD_ESI);
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_BRS, frame.fd.flags & CANFD_BRS);
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_FDF, frame.fd.flags & CANFD_FDF);
Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_ESI, frame.fd.flags & CANFD_ESI);
}

// Copy payload to ACF CAN PDU
processedBytes = Avtp_Can_SetPayload(pdu, frame.can_id & CAN_EFF_MASK, frame.data,
frame.len, can_variant);
if(can_variant == AVTP_CAN_FD)
processedBytes = Avtp_Can_SetPayload(pdu, frame.fd.can_id & CAN_EFF_MASK, frame.fd.data,
frame.fd.len, can_variant);
else
processedBytes = Avtp_Can_SetPayload(pdu, frame.cc.can_id & CAN_EFF_MASK, frame.cc.data,
frame.cc.len, can_variant);

return processedBytes;
}
Expand All @@ -239,7 +245,7 @@ int main(int argc, char *argv[])
struct sockaddr_in sk_udp_addr;
uint8_t pdu[MAX_PDU_SIZE];
uint16_t pdu_length, cf_length;
struct canfd_frame can_frame;
frame_t can_frame;

argp_parse(&argp, argc, argv, 0, NULL, NULL);

Expand All @@ -254,7 +260,7 @@ int main(int argc, char *argv[])
} else {
fd = create_talker_socket(priority);
if (fd < 0) return fd;
res = setup_socket_address(fd, ifname, macaddr,
res = setup_socket_address(fd, ifname, macaddr,
ETH_P_TSN, &sk_ll_addr);
}
if (res < 0) goto err;
Expand All @@ -271,7 +277,7 @@ int main(int argc, char *argv[])
pdu_length = 0;
cf_length = 0;

// Usage of UDP means the PDU needs a
// Usage of UDP means the PDU needs a
if (use_udp) {
Avtp_Udp_t *udp_pdu = (Avtp_Udp_t *) pdu;
Avtp_Udp_SetField(udp_pdu, AVTP_UDP_FIELD_ENCAPSULATION_SEQ_NO,
Expand All @@ -290,7 +296,11 @@ int main(int argc, char *argv[])
while (i < num_acf_msgs) {
// Get payload -- will 'spin' here until we get the requested number
// of CAN frames.
res = read(can_socket, &can_frame, sizeof(struct canfd_frame));
if(can_variant == AVTP_CAN_FD){
res = read(can_socket, &can_frame.fd, sizeof(struct canfd_frame));
} else {
res = read(can_socket, &can_frame.cc, sizeof(struct can_frame));
}
if (!res) continue;

uint8_t* acf_pdu = pdu + pdu_length;
Expand Down

0 comments on commit e7fba30

Please sign in to comment.