diff --git a/CMakeLists.txt b/CMakeLists.txt index eab1481..76b6997 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,12 +49,12 @@ target_include_directories(aaf-talker PRIVATE "examples" "include") target_link_libraries(aaf-talker open1722 open1722examples) # CAN talker app -add_executable(acf-can-talker "examples/acf-can/acf-can-talker.c") +add_executable(acf-can-talker "examples/acf-can/acf-can-talker.c" "examples/acf-can/acf-can-common.c") target_include_directories(acf-can-talker PRIVATE "examples" "include") target_link_libraries(acf-can-talker open1722 open1722examples) # CAN listener app -add_executable(acf-can-listener "examples/acf-can/acf-can-listener.c") +add_executable(acf-can-listener "examples/acf-can/acf-can-listener.c" "examples/acf-can/acf-can-common.c") target_include_directories(acf-can-listener PRIVATE "examples" "include") target_link_libraries(acf-can-listener open1722 open1722examples) diff --git a/examples/acf-can/acf-can-common.c b/examples/acf-can/acf-can-common.c new file mode 100644 index 0000000..872d57b --- /dev/null +++ b/examples/acf-can/acf-can-common.c @@ -0,0 +1,67 @@ +/* + * Copyright (c) 2024, COVESA + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of COVESA nor the names of its contributors may be + * used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include +#include +#include +#include +#include "acf-can-common.h" + +int setup_can_socket(const char* can_ifname, Avtp_CanVariant_t can_variant) { + + int can_socket, res; + struct ifreq ifr; + struct sockaddr_can can_addr; + + can_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (can_socket < 0) return can_socket; + + strcpy(ifr.ifr_name, can_ifname); + ioctl(can_socket, SIOCGIFINDEX, &ifr); + + memset(&can_addr, 0, sizeof(can_addr)); + can_addr.can_family = AF_CAN; + can_addr.can_ifindex = ifr.ifr_ifindex; + + if (can_variant == AVTP_CAN_FD) { + int enable_canfx = 1; + setsockopt(can_socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, + &enable_canfx, sizeof(enable_canfx)); + } + + res = bind(can_socket, (struct sockaddr *)&can_addr, sizeof(can_addr)); + if (res < 0) { + close(can_socket); + return res; + } + + return can_socket; +} diff --git a/examples/acf-can/acf-can-common.h b/examples/acf-can/acf-can-common.h new file mode 100644 index 0000000..ab69e88 --- /dev/null +++ b/examples/acf-can/acf-can-common.h @@ -0,0 +1,32 @@ +/* + * Copyright (c) 2024, COVESA + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of COVESA nor the names of its contributors may be + * used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include "avtp/acf/Can.h" + +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 223053c..fb18228 100644 --- a/examples/acf-can/acf-can-listener.c +++ b/examples/acf-can/acf-can-listener.c @@ -48,16 +48,17 @@ #include "avtp/acf/Common.h" #include "avtp/acf/Can.h" #include "avtp/CommonHeader.h" +#include "acf-can-common.h" #define MAX_PDU_SIZE 1500 -#define CAN_FD_OPTION 500 +#define ARGPARSE_CAN_FD_OPTION 500 static char ifname[IFNAMSIZ]; static uint8_t macaddr[ETH_ALEN]; static uint8_t use_udp; static uint32_t udp_port = 17220; static Avtp_CanVariant_t can_variant = AVTP_CAN_CLASSIC; -static char can_ifname[IFNAMSIZ] = "STDOUT\0"; +static char can_ifname[IFNAMSIZ]; static char doc[] = "\nacf-can-listener -- a program designed to receive CAN messages from \ a remote CAN bus over Ethernet using Open1722 \ @@ -74,7 +75,7 @@ static char args_doc[] = "[ifname] dst-mac-address [can ifname]"; static struct argp_option options[] = { {"port", 'p', "UDP_PORT", 0, "UDP Port to listen on if UDP enabled"}, {"udp", 'u', 0, 0, "Use UDP"}, - {"fd", CAN_FD_OPTION, 0, 0, "Use CAN-FD"}, + {"fd", ARGPARSE_CAN_FD_OPTION, 0, 0, "Use CAN-FD"}, {"can ifname", 0, 0, OPTION_DOC, "CAN interface (set to STDOUT by default)"}, {"dst-mac-address", 0, 0, OPTION_DOC, "Stream destination MAC address (If Ethernet)"}, {"ifname", 0, 0, OPTION_DOC, "Network interface (If Ethernet)" }, @@ -92,7 +93,7 @@ static error_t parser(int key, char *arg, struct argp_state *state) case 'u': use_udp = 1; break; - case CAN_FD_OPTION: + case ARGPARSE_CAN_FD_OPTION: can_variant = AVTP_CAN_FD; case ARGP_KEY_NO_ARGS: @@ -105,7 +106,6 @@ static error_t parser(int key, char *arg, struct argp_state *state) } if(!use_udp){ - strncpy(ifname, arg, sizeof(ifname) - 1); if(state->next < state->argc) @@ -159,7 +159,7 @@ void print_can_acf(uint8_t* acf_pdu) Avtp_Can_t *pdu = (Avtp_Can_t*) acf_pdu; - Avtp_Can_GetField(pdu, AVTP_ACF_FIELD_ACF_MSG_LENGTH, &acf_msg_len); + Avtp_Can_GetField(pdu, AVTP_CAN_FIELD_ACF_MSG_LENGTH, &acf_msg_len); Avtp_Can_GetField(pdu, AVTP_CAN_FIELD_CAN_BUS_ID, &can_bus_id); Avtp_Can_GetField(pdu, AVTP_CAN_FIELD_MESSAGE_TIMESTAMP, ×tamp); Avtp_Can_GetField(pdu, AVTP_CAN_FIELD_CAN_IDENTIFIER, &can_identifier); @@ -177,17 +177,13 @@ static int new_packet(int sk_fd, int can_socket) { int res; uint64_t msg_length, proc_bytes = 0, msg_proc_bytes = 0; - uint64_t can_frame_id, udp_seq_num = 0, subtype; + uint64_t can_frame_id, udp_seq_num, subtype, flag; uint16_t payload_length, pdu_length; - uint8_t *can_payload, i; - uint8_t pdu[MAX_PDU_SIZE]; - uint8_t* cf_pdu; - uint8_t* acf_pdu; - Avtp_Udp_t *udp_pdu; - char stdout_string[1000] = "\0"; + uint8_t pdu[MAX_PDU_SIZE], i; + uint8_t *cf_pdu, *acf_pdu, *udp_pdu, *can_payload; struct canfd_frame frame; - uint64_t eff; + memset(&frame, 0, sizeof(struct canfd_frame)); res = recv(sk_fd, pdu, MAX_PDU_SIZE, 0); if (res < 0 || res > MAX_PDU_SIZE) { @@ -196,8 +192,8 @@ static int new_packet(int sk_fd, int can_socket) { } if (use_udp) { - udp_pdu = (Avtp_Udp_t *) pdu; - Avtp_Udp_GetField(udp_pdu, AVTP_UDP_FIELD_ENCAPSULATION_SEQ_NO, &udp_seq_num); + udp_pdu = pdu; + Avtp_Udp_GetField((Avtp_Udp_t *)udp_pdu, AVTP_UDP_FIELD_ENCAPSULATION_SEQ_NO, &udp_seq_num); cf_pdu = pdu + AVTP_UDP_HEADER_LEN; proc_bytes += AVTP_UDP_HEADER_LEN; } else { @@ -219,15 +215,10 @@ static int new_packet(int sk_fd, int can_socket) { if(subtype == AVTP_SUBTYPE_TSCF){ proc_bytes += AVTP_TSCF_HEADER_LEN; - res = Avtp_Tscf_GetField((Avtp_Tscf_t*)cf_pdu, AVTP_TSCF_FIELD_STREAM_DATA_LENGTH, (uint64_t *) &msg_length); + Avtp_Tscf_GetField((Avtp_Tscf_t*)cf_pdu, AVTP_TSCF_FIELD_STREAM_DATA_LENGTH, (uint64_t *) &msg_length); }else{ proc_bytes += AVTP_NTSCF_HEADER_LEN; - res = Avtp_Ntscf_GetField((Avtp_Ntscf_t*)cf_pdu, AVTP_NTSCF_FIELD_NTSCF_DATA_LENGTH, (uint64_t *) &msg_length); - } - - if (res < 0) { - fprintf(stderr, "Failed to get message length: %d\n", res); - return -1; + Avtp_Ntscf_GetField((Avtp_Ntscf_t*)cf_pdu, AVTP_NTSCF_FIELD_NTSCF_DATA_LENGTH, (uint64_t *) &msg_length); } while (msg_proc_bytes < msg_length) { @@ -239,56 +230,50 @@ static int new_packet(int sk_fd, int can_socket) { return -1; } - res = Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_CAN_IDENTIFIER, - &can_frame_id); - if (res < 0) { - fprintf(stderr, "Error: Getting CAN frame ID\n"); - return -1; - } + Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_CAN_IDENTIFIER, + &(can_frame_id)); + frame.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; - res = Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_EFF, &eff); - if (res < 0) { - fprintf(stderr, "Failed to get eff field: %d\n", res); - return -1; - } - - if (can_frame_id > 0x7FF && !eff) { + // Handle EFF Flag + Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_EFF, &flag); + if (frame.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 (can_socket == 0) { - for (i = 0; i < payload_length; i++) { - sprintf(stdout_string+(2*i), "%02x", can_payload[i]); - } + // 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 (eff) { - fprintf(stdout, "(000000.000000) elmcan 0000%03lx#%s\n", can_frame_id, stdout_string); - } else { - fprintf(stdout, "(000000.000000) elmcan %03lx#%s\n", can_frame_id, stdout_string); - } - fflush(stdout); - } else { - frame.can_id = (canid_t) can_frame_id; - if (eff) { - frame.can_id |= CAN_EFF_FLAG; - } - frame.len = payload_length; - memcpy(frame.data, can_payload, payload_length); - if (write(can_socket, &frame, sizeof(struct canfd_frame)) != sizeof(struct canfd_frame)) { - return 1; - } + 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; + + Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_FDF, &flag); + if (flag) frame.flags |= CANFD_FDF; + + Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_ESI, &flag); + if (flag) frame.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; } + } return 1; } int main(int argc, char *argv[]) { - int sk_fd, res; + int fd, res; struct pollfd fds; int can_socket = 0; @@ -297,40 +282,22 @@ int main(int argc, char *argv[]) argp_parse(&argp, argc, argv, 0, NULL, NULL); - // Open a CAN socket for reading frames if required - if (strcmp(can_ifname, "STDOUT\0")) { - can_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW); - if (can_socket < 0) - return 1; - - strcpy(ifr.ifr_name, can_ifname); - ioctl(can_socket, SIOCGIFINDEX, &ifr); - - memset(&can_addr, 0, sizeof(can_addr)); - can_addr.can_family = AF_CAN; - can_addr.can_ifindex = ifr.ifr_ifindex; - - if (can_variant == AVTP_CAN_FD) { - int enable_canfx = 1; - setsockopt(can_socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, - &enable_canfx, sizeof(enable_canfx)); - } - - if (bind(can_socket, (struct sockaddr *)&can_addr, sizeof(can_addr)) < 0) - return 1; - } - + // Configure an appropriate socket: UDP or Ethernet Raw if (use_udp) { - sk_fd = create_listener_socket_udp(udp_port); + fd = create_listener_socket_udp(udp_port); } else { - sk_fd = create_listener_socket(ifname, macaddr, ETH_P_TSN); + fd = create_listener_socket(ifname, macaddr, ETH_P_TSN); } - fds.fd = sk_fd; + fds.fd = fd; fds.events = POLLIN; - if (sk_fd < 0) + if (fd < 0) return 1; + // Open a CAN socket for reading frames + can_socket = setup_can_socket(can_ifname, can_variant); + if (!can_socket) goto err; + while (1) { res = poll(&fds, 1, -1); @@ -340,7 +307,7 @@ int main(int argc, char *argv[]) } if (fds.revents & POLLIN) { - res = new_packet(sk_fd, can_socket); + res = new_packet(fd, can_socket); if (res < 0) goto err; } @@ -350,7 +317,7 @@ int main(int argc, char *argv[]) return 0; err: - close(sk_fd); + close(fd); return 1; } diff --git a/examples/acf-can/acf-can-talker.c b/examples/acf-can/acf-can-talker.c index 37e82b7..981859d 100644 --- a/examples/acf-can/acf-can-talker.c +++ b/examples/acf-can/acf-can-talker.c @@ -30,9 +30,7 @@ #include #include #include -#include #include -#include #include #include @@ -47,13 +45,13 @@ #include "avtp/Udp.h" #include "avtp/acf/Ntscf.h" #include "avtp/acf/Tscf.h" -#include "avtp/acf/Can.h" #include "avtp/CommonHeader.h" +#include "acf-can-common.h" #define MAX_PDU_SIZE 1500 #define STREAM_ID 0xAABBCCDDEEFF0001 #define CAN_PAYLOAD_MAX_SIZE 16*4 -#define CAN_FD_OPTION 500 +#define ARGPARSE_CAN_FD_OPTION 500 static char ifname[IFNAMSIZ]; static uint8_t macaddr[ETH_ALEN]; @@ -61,11 +59,12 @@ static uint8_t ip_addr[sizeof(struct in_addr)]; static uint32_t udp_port=17220; static int priority = -1; static uint8_t seq_num = 0; +static uint32_t udp_seq_num = 0; static uint8_t use_tscf; static uint8_t use_udp; static Avtp_CanVariant_t can_variant = AVTP_CAN_CLASSIC; -static uint8_t multi_can_frames = 1; -static char can_ifname[IFNAMSIZ] = "STDIN\0"; +static uint8_t num_acf_msgs = 1; +static char can_ifname[IFNAMSIZ]; static char doc[] = "\nacf-can-talker -- a program designed to send CAN messages to \ a remote CAN bus over Ethernet using Open1722 \ @@ -84,7 +83,7 @@ static char args_doc[] = "[ifname] dst-mac-address/dst-nw-address:port [can ifna static struct argp_option options[] = { {"tscf", 't', 0, 0, "Use TSCF"}, {"udp", 'u', 0, 0, "Use UDP" }, - {"fd", CAN_FD_OPTION, 0, 0, "Use CAN-FD"}, + {"fd", ARGPARSE_CAN_FD_OPTION, 0, 0, "Use CAN-FD"}, {"count", 'c', "COUNT", 0, "Set count of CAN messages per Ethernet frame"}, {"can ifname", 0, 0, OPTION_DOC, "CAN interface (set to STDIN by default)"}, {"ifname", 0, 0, OPTION_DOC, "Network interface (If Ethernet)"}, @@ -106,9 +105,9 @@ static error_t parser(int key, char *arg, struct argp_state *state) use_udp = 1; break; case 'c': - multi_can_frames = atoi(arg); + num_acf_msgs = atoi(arg); break; - case CAN_FD_OPTION: + case ARGPARSE_CAN_FD_OPTION: can_variant = AVTP_CAN_FD; break; @@ -186,21 +185,20 @@ static int init_cf_pdu(uint8_t* pdu) return res; } -static int update_pdu_length(uint8_t* pdu, uint64_t length) +static int update_cf_length(uint8_t* cf_pdu, uint64_t length) { if (use_tscf) { uint64_t payloadLen = length - AVTP_TSCF_HEADER_LEN; - Avtp_Tscf_SetField((Avtp_Tscf_t*)pdu, AVTP_TSCF_FIELD_STREAM_DATA_LENGTH, payloadLen); + Avtp_Tscf_SetField((Avtp_Tscf_t*)cf_pdu, AVTP_TSCF_FIELD_STREAM_DATA_LENGTH, payloadLen); } else { uint64_t payloadLen = length - AVTP_NTSCF_HEADER_LEN; - Avtp_Ntscf_SetField((Avtp_Ntscf_t*)pdu, AVTP_NTSCF_FIELD_NTSCF_DATA_LENGTH, payloadLen); + Avtp_Ntscf_SetField((Avtp_Ntscf_t*)cf_pdu, AVTP_NTSCF_FIELD_NTSCF_DATA_LENGTH, payloadLen); } return 0; } static int prepare_acf_packet(uint8_t* acf_pdu, - uint8_t* payload, uint8_t length, - uint32_t can_frame_id) { + struct canfd_frame frame) { int processedBytes; struct timespec now; @@ -216,116 +214,54 @@ static int prepare_acf_packet(uint8_t* acf_pdu, (uint64_t)now.tv_nsec + (uint64_t)(now.tv_sec * 1e9)); Avtp_Can_SetField(pdu, AVTP_CAN_FIELD_MTV, 1U); - // Copy payload to ACF CAN PDU - processedBytes = Avtp_Can_SetPayload(pdu, can_frame_id, payload, length, can_variant); - - return processedBytes; -} - -static int get_payload(int can_socket, uint8_t* payload, uint32_t *frame_id, uint8_t *length) { - - char stdin_str[1000]; - char can_str[10]; - char can_payload[1000]; - char *token; - size_t n; - int res; - struct canfd_frame frame; - - if (can_socket == 0) { - n = read(STDIN_FILENO, stdin_str, 1000); - if (n < 0) { - return -1; - } - - res = sscanf(stdin_str, "%s %x [%hhu] %[0-9A-F ]s", can_str, frame_id, - length, can_payload); - if (res < 0) { - return -1; - } + // 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); - token = strtok(can_payload, " "); - int index = 0; - while (token != NULL) { - payload[index++] = (unsigned short)strtol(token, NULL, 16); - token = strtok(NULL, " "); - } - } else { - n = read(can_socket, &frame, sizeof(struct canfd_frame)); - if (n > 0) { - *frame_id = (uint32_t) frame.can_id; - *length = (uint8_t) frame.len; - memcpy(payload, frame.data, (size_t) *length); - } + 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); } - return n; + // Copy payload to ACF CAN PDU + processedBytes = Avtp_Can_SetPayload(pdu, frame.can_id & CAN_EFF_MASK, frame.data, + frame.len, can_variant); + + return processedBytes; } int main(int argc, char *argv[]) { - int fd, res; + int fd, res, can_socket=0; struct sockaddr_ll sk_ll_addr; struct sockaddr_in sk_udp_addr; uint8_t pdu[MAX_PDU_SIZE]; - - uint8_t payload[CAN_PAYLOAD_MAX_SIZE]; - uint8_t payload_length = 0; - uint32_t frame_id = 0; - uint8_t num_acf_msgs = 1; - uint32_t pdu_length; - - int can_socket = 0; - struct sockaddr_can can_addr; - struct ifreq ifr; + uint16_t pdu_length, cf_length; + struct canfd_frame can_frame; argp_parse(&argp, argc, argv, 0, NULL, NULL); + // Create an appropriate talker socket: UDP or Ethernet raw + // Setup the socket for sending to the destination if (use_udp) { fd = create_talker_socket_udp(priority); - } else { - fd = create_talker_socket(priority); - } - if (fd < 0) - return 1; - - num_acf_msgs = multi_can_frames; + if (fd < 0) return fd; - // Open a CAN socket for reading frames if required - if (strcmp(can_ifname, "STDIN\0")) { - can_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW); - if (can_socket < 0) - return 1; - - strcpy(ifr.ifr_name, can_ifname); - ioctl(can_socket, SIOCGIFINDEX, &ifr); - - memset(&can_addr, 0, sizeof(can_addr)); - can_addr.can_family = AF_CAN; - can_addr.can_ifindex = ifr.ifr_ifindex; - - if (can_variant == AVTP_CAN_FD) { - int enable_canfx = 1; - setsockopt(can_socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, - &enable_canfx, sizeof(enable_canfx)); - } - - if (bind(can_socket, (struct sockaddr *)&can_addr, sizeof(can_addr)) < 0) - return 1; - } - - - if (use_udp) { res = setup_udp_socket_address((struct in_addr*) ip_addr, udp_port, &sk_udp_addr); - if (res < 0) - goto err; } else { - res = setup_socket_address(fd, ifname, macaddr, ETH_P_TSN, &sk_ll_addr); - if (res < 0) - goto err; + fd = create_talker_socket(priority); + if (fd < 0) return fd; + res = setup_socket_address(fd, ifname, macaddr, + ETH_P_TSN, &sk_ll_addr); } + if (res < 0) goto err; + + // Open a CAN socket for reading frames + can_socket = setup_can_socket(can_ifname, can_variant); + if (!can_socket) goto err; // Sending loop for(;;) { @@ -333,58 +269,52 @@ int main(int argc, char *argv[]) // Pack into control formats uint8_t *cf_pdu; pdu_length = 0; + cf_length = 0; + // 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, - seq_num); - cf_pdu = &pdu[sizeof(Avtp_Udp_t)]; - } else { - cf_pdu = pdu; + udp_seq_num++); + pdu_length += sizeof(Avtp_Udp_t); } + cf_pdu = pdu + pdu_length; res = init_cf_pdu(cf_pdu); if (res < 0) goto err; pdu_length += res; + cf_length += res; int i = 0; while (i < num_acf_msgs) { // Get payload -- will 'spin' here until we get the requested number // of CAN frames. - res = get_payload(can_socket, payload, &frame_id, &payload_length); - if (!res) { - continue; - } + res = read(can_socket, &can_frame, sizeof(struct canfd_frame)); + if (!res) continue; - uint8_t* acf_pdu = cf_pdu + pdu_length; - res = prepare_acf_packet(acf_pdu, payload, payload_length, frame_id); - if (res < 0) - goto err; + uint8_t* acf_pdu = pdu + pdu_length; + res = prepare_acf_packet(acf_pdu, can_frame); + if (res < 0) goto err; pdu_length += res; - + cf_length += res; i++; } - res = update_pdu_length(cf_pdu, pdu_length); + res = update_cf_length(cf_pdu, cf_length); if (res < 0) goto err; if (use_udp) { - pdu_length += sizeof(uint32_t); res = sendto(fd, pdu, pdu_length, 0, (struct sockaddr *) &sk_udp_addr, sizeof(sk_udp_addr)); - if (res < 0) { - perror("Failed to send data"); - goto err; - } } else { res = sendto(fd, pdu, pdu_length, 0, (struct sockaddr *) &sk_ll_addr, sizeof(sk_ll_addr)); - if (res < 0) { - perror("Failed to send data"); - goto err; - } + } + if (res < 0) { + perror("Failed to send data"); + goto err; } } diff --git a/src/avtp/acf/Can.c b/src/avtp/acf/Can.c index 89406ef..1b42139 100644 --- a/src/avtp/acf/Can.c +++ b/src/avtp/acf/Can.c @@ -131,7 +131,7 @@ int Avtp_Can_Finalize(Avtp_Can_t* can_pdu, uint16_t payload_length) { uint8_t* Avtp_Can_GetPayload(Avtp_Can_t* can_pdu, uint16_t* payload_length, uint16_t *pdu_length) { uint64_t pad_len, pdu_len; - int res = Avtp_Can_GetField((Avtp_Can_t*)can_pdu, AVTP_ACF_FIELD_ACF_MSG_LENGTH, + int res = Avtp_Can_GetField((Avtp_Can_t*)can_pdu, AVTP_CAN_FIELD_ACF_MSG_LENGTH, &pdu_len); if (res < 0) { return 0;