Feat/can fd rebased

CMakeLists.txt

@@ -52,12 +52,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)
 

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 <linux/if.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <string.h>
+#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;
+}

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);

examples/acf-can/acf-can-listener.c

@@ -48,14 +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 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 char can_ifname[IFNAMSIZ] = "STDOUT\0";
+static Avtp_CanVariant_t can_variant = AVTP_CAN_CLASSIC;
+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 \
@@ -72,6 +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", 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)" },
@@ -89,6 +93,8 @@ static error_t parser(int key, char *arg, struct argp_state *state)
     case 'u':
         use_udp = 1;
         break;
+    case ARGPARSE_CAN_FD_OPTION:
+        can_variant = AVTP_CAN_FD;
 
     case ARGP_KEY_NO_ARGS:
         break;
@@ -100,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)
@@ -154,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, &timestamp);
     Avtp_Can_GetField(pdu, AVTP_CAN_FIELD_CAN_IDENTIFIER, &can_identifier);
@@ -172,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";
-    struct can_frame frame;
-    uint64_t eff;
+    uint8_t pdu[MAX_PDU_SIZE], i;
+    uint8_t *cf_pdu, *acf_pdu, *udp_pdu, *can_payload;
+    struct canfd_frame frame;
 
+    memset(&frame, 0, sizeof(struct canfd_frame));
     res = recv(sk_fd, pdu, MAX_PDU_SIZE, 0);
 
     if (res < 0 || res > MAX_PDU_SIZE) {
@@ -191,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 {
@@ -214,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) {
@@ -234,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.can_dlc = payload_length;
-            memcpy(frame.data, can_payload, payload_length);
-            if (write(can_socket, &frame, sizeof(struct can_frame)) != sizeof(struct can_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;
@@ -292,33 +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 (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);
@@ -328,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;
         }
@@ -338,7 +317,7 @@ int main(int argc, char *argv[])
     return 0;
 
 err:
-    close(sk_fd);
+    close(fd);
     return 1;
 
 }

examples/acf-can/acf-can-talker.c

@@ -9,7 +9,7 @@
  *    * 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 
+ *    * 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.
  *
@@ -30,9 +30,7 @@
 #include <linux/if_packet.h>
 #include <linux/if.h>
 #include <linux/if_ether.h>
-#include <linux/can.h>
 #include <linux/can/raw.h>
-#include <sys/ioctl.h>
 #include <time.h>
 
 #include <arpa/inet.h>
@@ -47,23 +45,26 @@
 #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 ARGPARSE_CAN_FD_OPTION      500
 
 static char ifname[IFNAMSIZ];
 static uint8_t macaddr[ETH_ALEN];
 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 uint8_t multi_can_frames = 1;
-static char can_ifname[IFNAMSIZ] = "STDIN\0";
+static Avtp_CanVariant_t can_variant = AVTP_CAN_CLASSIC;
+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 \
@@ -79,14 +80,15 @@ static char doc[] = "\nacf-can-talker -- a program designed to send CAN messages
 
 static char args_doc[] = "[ifname] dst-mac-address/dst-nw-address:port [can ifname]";
 
-static struct argp_option options[] = {            
+static struct argp_option options[] = {
     {"tscf", 't', 0, 0, "Use TSCF"},
-    {"udp",  'u', 0, 0, "Use UDP" },
+    {"udp", 'u', 0, 0, "Use UDP" },
+    {"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)"},
     {"dst-mac-address", 0, 0, OPTION_DOC, "Stream destination MAC address (If Ethernet)"},
-    {"dst-nw-address:port", 0, 0, OPTION_DOC, "Stream destination network address and port (If UDP)"},    
+    {"dst-nw-address:port", 0, 0, OPTION_DOC, "Stream destination network address and port (If UDP)"},
     { 0 }
 };
 
@@ -103,7 +105,10 @@ 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 ARGPARSE_CAN_FD_OPTION:
+        can_variant = AVTP_CAN_FD;
         break;
 
     case ARGP_KEY_NO_ARGS:
@@ -113,22 +118,22 @@ static error_t parser(int key, char *arg, struct argp_state *state)
 
         if(state->argc < 2){
             argp_usage(state);
-        }    
+        }
 
         if(!use_udp){
 
-            strncpy(ifname, arg, sizeof(ifname) - 1);       
+            strncpy(ifname, arg, sizeof(ifname) - 1);
 
             if(state->next < state->argc)
-            {             
+            {
                 res = sscanf(state->argv[state->next], "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
                         &macaddr[0], &macaddr[1], &macaddr[2],
                         &macaddr[3], &macaddr[4], &macaddr[5]);
                 if (res != 6) {
                     fprintf(stderr, "Invalid MAC address\n\n");
                     argp_usage(state);
                 }
-                state->next += 1;   
+                state->next += 1;
             }
 
         } else {
@@ -142,13 +147,13 @@ static error_t parser(int key, char *arg, struct argp_state *state)
                 fprintf(stderr, "Invalid IP address\n\n");
                 argp_usage(state);
             }
-        } 
-          
+        }
+
         if(state->next < state->argc)
-        {                                   
-            strncpy(can_ifname, state->argv[state->next], sizeof(can_ifname) - 1);            
+        {
+            strncpy(can_ifname, state->argv[state->next], sizeof(can_ifname) - 1);
             state->next = state->argc;
-        }         
+        }
 
         break;
     }
@@ -180,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;
@@ -206,171 +210,111 @@ 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, (uint64_t)now.tv_nsec + (uint64_t)(now.tv_sec * 1e9));
+    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);
 
-    // Copy payload to ACF CAN PDU
-    processedBytes = Avtp_Can_SetPayload(pdu, can_frame_id, payload, length, CAN_CLASSIC);
+    // 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);
 
-    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 can_frame frame;
-
-    if (can_socket == 0) {
-        n = read(STDIN_FILENO, stdin_str, 1000);
-        if (n < 0) {
-            return -1;
-        }
+    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);
+    }
 
-        res = sscanf(stdin_str, "%s %x [%hhu] %[0-9A-F ]s", can_str, frame_id,
-                                                        length, can_payload);
-        if (res < 0) {
-            return -1;
-        }
+    // Copy payload to ACF CAN PDU
+    processedBytes = Avtp_Can_SetPayload(pdu, frame.can_id & CAN_EFF_MASK, frame.data, 
+                                         frame.len, can_variant);
 
-        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 can_frame));
-        if (n > 0) {
-            *frame_id = (uint32_t) frame.can_id;
-            *length = (uint8_t) frame.can_dlc;
-            memcpy(payload, frame.data, (size_t) *length);
-        }
-    }
-
-    return n;
+    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;
-
-    // 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 (bind(can_socket, (struct sockaddr *)&can_addr, sizeof(can_addr)) < 0) 
-            return 1;
-    }
+        if (fd < 0) return fd;
 
-
-    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(;;) {
 
         // 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;
         }
     }
 

include/avtp/acf/Can.h

@@ -48,9 +48,9 @@ typedef struct {
 } Avtp_Can_t;
 
 typedef enum {
-    CAN_CLASSIC = 0,
-    CAN_FD
-} Can_Variant_t;
+    AVTP_CAN_CLASSIC = 0,
+    AVTP_CAN_FD
+} Avtp_CanVariant_t;
 
 typedef enum  {
 
@@ -115,7 +115,7 @@ int Avtp_Can_SetField(Avtp_Can_t* can_pdu, Avtp_CanFields_t field, uint64_t valu
  * @returns Returns number of processed bytes (header + payload + padding)
  */
 int Avtp_Can_SetPayload(Avtp_Can_t* can_pdu, uint32_t frame_id , uint8_t* payload, 
-                        uint16_t payload_length, Can_Variant_t can_variant);
+                        uint16_t payload_length, Avtp_CanVariant_t can_variant);
 
 /**
  * Returns pointer to payload of an ACF CAN frame.

include/avtp/acf/CanBrief.h

@@ -110,7 +110,7 @@ int Avtp_CanBrief_SetField(Avtp_CanBrief_t* can_pdu, Avtp_CanBriefFields_t field
  * @returns Returns number of processed bytes (header + payload + padding)
  */
 int Avtp_CanBrief_SetPayload(Avtp_CanBrief_t* can_pdu, uint32_t frame_id , uint8_t* payload, 
-                        uint16_t payload_length, Can_Variant_t can_variant);
+                        uint16_t payload_length, Avtp_CanVariant_t can_variant);
 
 /**
  * Finalizes the ACF CAN Brief frame. This function will set the

src/avtp/acf/Can.c

@@ -81,7 +81,7 @@ int Avtp_Can_SetField(Avtp_Can_t* can_pdu,
 }
 
 int Avtp_Can_SetPayload(Avtp_Can_t* can_pdu, uint32_t frame_id , uint8_t* payload, 
-                        uint16_t payload_length, Can_Variant_t can_variant) {
+                        uint16_t payload_length, Avtp_CanVariant_t can_variant) {
 
     int ret = 0;
     int eff;
@@ -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;

src/avtp/acf/CanBrief.c

@@ -78,7 +78,7 @@ int Avtp_CanBrief_SetField(Avtp_CanBrief_t* can_pdu, Avtp_CanBriefFields_t field
 }
 
 int Avtp_CanBrief_SetPayload(Avtp_CanBrief_t* can_pdu, uint32_t frame_id , uint8_t* payload, 
-                        uint16_t payload_length, Can_Variant_t can_variant) {
+                        uint16_t payload_length, Avtp_CanVariant_t can_variant) {
 
     int ret = 0;
     int eff;

unit/test-can.c

@@ -90,15 +90,15 @@ static void can_set_payload(void **state) {
 
     // Set payload and check for EFF
     Avtp_Can_SetPayload((Avtp_Can_t*)pdu, set_frame_id, set_payload, 
-                        CAN_PAYLOAD_SIZE, CAN_CLASSIC);
+                        CAN_PAYLOAD_SIZE, AVTP_CAN_CLASSIC);
     assert_int_equal(htonl(set_frame_id), (uint32_t) *((int*)pdu+3));
     assert_memory_equal(set_payload, pdu+16, CAN_PAYLOAD_SIZE);
     assert_int_equal(0x0, *(pdu+2)&0x08);    // Check EFF
 
     // Check EFF for extended Frame IDs
     set_frame_id = 0x800;
     Avtp_Can_SetPayload((Avtp_Can_t*)pdu, set_frame_id, set_payload, 
-                        CAN_PAYLOAD_SIZE, CAN_CLASSIC);
+                        CAN_PAYLOAD_SIZE, AVTP_CAN_CLASSIC);
     assert_int_equal(htonl(set_frame_id), (uint32_t) *((int*)pdu+3));
     assert_int_equal(0x8, *(pdu+2)&0x08);    // Check EFF
 
@@ -109,7 +109,7 @@ static void can_set_payload(void **state) {
         memset(pdu, 0, MAX_PDU_SIZE);
         Avtp_Can_Init((Avtp_Can_t*)pdu);
         ret = Avtp_Can_SetPayload((Avtp_Can_t*)pdu, set_frame_id, set_payload, 
-                        i, CAN_CLASSIC);
+                        i, AVTP_CAN_CLASSIC);
         assert_int_equal(ret, AVTP_CAN_HEADER_LEN+i+((4 - i%4)&0x3));
         assert_memory_equal(set_payload, pdu+16, i);
         assert_memory_equal(zero_array, pdu+16+i, CAN_PAYLOAD_SIZE-i);