diff --git a/examples/acf-can/acf-can-common.h b/examples/acf-can/acf-can-common.h index ab69e88..ec23846 100644 --- a/examples/acf-can/acf-can-common.h +++ b/examples/acf-can/acf-can-common.h @@ -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); \ No newline at end of file diff --git a/examples/acf-can/acf-can-listener.c b/examples/acf-can/acf-can-listener.c index fb18228..51b668d 100644 --- a/examples/acf-can/acf-can-listener.c +++ b/examples/acf-can/acf-can-listener.c @@ -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); @@ -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; } diff --git a/examples/acf-can/acf-can-talker.c b/examples/acf-can/acf-can-talker.c index 981859d..b6e17f4 100644 --- a/examples/acf-can/acf-can-talker.c +++ b/examples/acf-can/acf-can-talker.c @@ -198,11 +198,12 @@ 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); @@ -210,23 +211,28 @@ static int prepare_acf_packet(uint8_t* acf_pdu, // 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; } @@ -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); @@ -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; @@ -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, @@ -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;