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.

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

Feat/can fd rebased #29

Merged
merged 5 commits into from
Aug 8, 2024
Merged
Changes from all commits
Commits
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
4 changes: 2 additions & 2 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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)

67 changes: 67 additions & 0 deletions examples/acf-can/acf-can-common.c
Original file line number Diff line number Diff line change
@@ -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;
}
32 changes: 32 additions & 0 deletions examples/acf-can/acf-can-common.h
Original file line number Diff line number Diff line change
@@ -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);
133 changes: 56 additions & 77 deletions examples/acf-can/acf-can-listener.c
Original file line number Diff line number Diff line change
@@ -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;

}
206 changes: 75 additions & 131 deletions examples/acf-can/acf-can-talker.c
Original file line number Diff line number Diff line change
@@ -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;
}
}

8 changes: 4 additions & 4 deletions include/avtp/acf/Can.h
Original file line number Diff line number Diff line change
@@ -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.
2 changes: 1 addition & 1 deletion include/avtp/acf/CanBrief.h
Original file line number Diff line number Diff line change
@@ -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
4 changes: 2 additions & 2 deletions src/avtp/acf/Can.c
Original file line number Diff line number Diff line change
@@ -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;
2 changes: 1 addition & 1 deletion src/avtp/acf/CanBrief.c
Original file line number Diff line number Diff line change
@@ -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;
6 changes: 3 additions & 3 deletions unit/test-can.c
Original file line number Diff line number Diff line change
@@ -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);