HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

446
Devices/Libraries/Systems/can-utils/cansequence.c Executable file
View File

@@ -0,0 +1,446 @@
/* SPDX-License-Identifier: GPL-2.0-only */
// Copyright (c) 2007, 2008, 2009, 2010, 2014, 2015, 2019, 2023 Pengutronix,
// Marc Kleine-Budde <kernel@pengutronix.de>
// Copyright (c) 2005 Pengutronix,
// Sascha Hauer <kernel@pengutronix.de>
#include <errno.h>
#include <getopt.h>
#include <libgen.h>
#include <limits.h>
#include <poll.h>
#include <signal.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <sys/uio.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#define CAN_ID_DEFAULT (2)
#define ANYDEV "any" /* name of interface to receive from any CAN interface */
extern int optind, opterr, optopt;
static int s = -1;
static bool running = true;
static volatile sig_atomic_t signal_num;
static bool infinite = true;
static bool canfd = false;
static bool canfd_strict = false;
static unsigned int drop_until_quit;
static unsigned int drop_count;
static bool use_poll = false;
static unsigned int loopcount = 1;
static int verbose;
static struct canfd_frame frame = {
.len = 1,
};
static struct can_filter filter[] = {
{
.can_id = CAN_ID_DEFAULT,
},
};
static void print_usage(char *prg)
{
fprintf(stderr,
"Usage: %s [<can-interface>] [Options]\n"
"\n"
"cansequence sends CAN messages with a rising sequence number as payload.\n"
"When the -r option is given, cansequence expects to receive these messages\n"
"and prints an error message if a wrong sequence number is encountered.\n"
"The main purpose of this program is to test the reliability of CAN links.\n"
"\n"
"Options:\n"
" -e, --extended send/receive extended frames\n"
" -f, --canfd send/receive CAN-FD CAN frames\n"
" -s, --strict refuse classical CAN frames in CAN-FD mode\n"
" -b, --brs send CAN-FD CAN frames with bitrate switch (BRS)\n"
" -i, --identifier=ID CAN Identifier (default = %u)\n"
" --loop=COUNT send message COUNT times\n"
" -p, --poll use poll(2) to wait for buffer space while sending\n"
" -q, --quit <num> quit if <num> wrong sequences are encountered\n"
" -r, --receive work as receiver\n"
" -v, --verbose be verbose (twice to be even more verbose\n"
" -h, --help this help\n",
prg, CAN_ID_DEFAULT);
}
static void sig_handler(int signo)
{
running = false;
signal_num = signo;
}
static void do_receive()
{
uint8_t ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(__u32))];
struct iovec iov = {
.iov_base = &frame,
};
struct msghdr msg = {
.msg_iov = &iov,
.msg_iovlen = 1,
.msg_control = &ctrlmsg,
};
const int dropmonitor_on = 1;
bool sequence_init = true;
unsigned int sequence_wrap = 0;
uint32_t sequence_mask = 0xff;
uint32_t sequence_rx = 0;
uint8_t sequence_delta = 0;
uint32_t sequence = 0;
unsigned int overflow_old = 0;
can_err_mask_t err_mask = CAN_ERR_MASK;
size_t mtu;
if (canfd)
mtu = CANFD_MTU;
else
mtu = CAN_MTU;
if (setsockopt(s, SOL_SOCKET, SO_RXQ_OVFL,
&dropmonitor_on, sizeof(dropmonitor_on)) < 0) {
perror("setsockopt() SO_RXQ_OVFL not supported by your Linux Kernel");
}
/* enable recv. of error messages */
if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask, sizeof(err_mask))) {
perror("setsockopt()");
exit(EXIT_FAILURE);
}
/* enable recv. now */
if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, filter, sizeof(filter))) {
perror("setsockopt()");
exit(EXIT_FAILURE);
}
while ((infinite || loopcount--) && running) {
ssize_t nbytes;
msg.msg_iov[0].iov_len = mtu;
msg.msg_controllen = sizeof(ctrlmsg);
msg.msg_flags = 0;
nbytes = recvmsg(s, &msg, 0);
if (nbytes < 0) {
perror("recvmsg()");
exit(EXIT_FAILURE);
}
if (frame.can_id & CAN_ERR_FLAG) {
fprintf(stderr,
"sequence CNT: %6u, ERRORFRAME %7x %02x %02x %02x %02x %02x %02x %02x %02x\n",
sequence, frame.can_id,
frame.data[0], frame.data[1], frame.data[2], frame.data[3],
frame.data[4], frame.data[5], frame.data[6], frame.data[7]);
continue;
}
sequence_rx = frame.data[0];
if (canfd_strict && nbytes == CAN_MTU) {
if (verbose > 1)
printf("sequence CNT: 0x%07x RX: 0x%02x (ignoring classical CAN frame)\n", sequence, sequence_rx);
continue;
}
if (sequence_init) {
sequence_init = false;
sequence = sequence_rx;
}
sequence_delta = sequence_rx - (uint8_t)sequence;
if (sequence_delta) {
struct cmsghdr *cmsg;
uint32_t overflow = 0;
uint32_t overflow_delta;
drop_count++;
for (cmsg = CMSG_FIRSTHDR(&msg);
cmsg && (cmsg->cmsg_level == SOL_SOCKET);
cmsg = CMSG_NXTHDR(&msg,cmsg)) {
if (cmsg->cmsg_type == SO_RXQ_OVFL) {
memcpy(&overflow, CMSG_DATA(cmsg), sizeof(overflow));
break;
}
}
overflow_delta = overflow - overflow_old;
fprintf(stderr,
"sequence CNT: 0x%07x RX: 0x%02x expected: 0x%02x missing: %4u skt overflow delta: %4u absolute: %4u hw - skt: %5d incident: %4u %s%s\n",
sequence, sequence_rx, /* CNT, RX */
sequence & sequence_mask, sequence_delta, /* expected, missing */
overflow_delta, overflow, /* skb overflow delta, absolute */
sequence_delta - overflow_delta, /* hw - skb */
drop_count, /* incident */
overflow_delta ? "[SOCKET]" : "",
overflow_delta != sequence_delta ?
((overflow_delta - sequence_delta > 0 && (overflow_delta - sequence_delta) & 0xff) ?
"[HARDWARE]" : "[HARDWARE?]") : "");
if (drop_count == drop_until_quit)
exit(EXIT_FAILURE);
sequence = sequence_rx;
overflow_old = overflow;
} else if (verbose > 1) {
printf("sequence CNT: 0x%07x RX: 0x%02x\n", sequence, sequence_rx);
}
sequence++;
if (verbose && !(sequence & sequence_mask))
printf("sequence wrap around (%d)\n", sequence_wrap++);
}
}
static void do_send()
{
unsigned int seq_wrap = 0;
uint8_t sequence = 0;
size_t mtu;
if (canfd)
mtu = CANFD_MTU;
else
mtu = CAN_MTU;
while ((infinite || loopcount--) && running) {
ssize_t len;
if (verbose > 1)
printf("sending frame. sequence number: %d\n", sequence);
again:
len = write(s, &frame, mtu);
if (len == -1) {
switch (errno) {
case ENOBUFS: {
int err;
struct pollfd fds[] = {
{
.fd = s,
.events = POLLOUT,
},
};
if (!use_poll) {
perror("write");
exit(EXIT_FAILURE);
}
err = poll(fds, 1, 1000);
if (err == 0 || (err == -1 && errno != EINTR)) {
perror("poll()");
exit(EXIT_FAILURE);
}
}
case EINTR: /* fallthrough */
goto again;
default:
perror("write");
exit(EXIT_FAILURE);
}
}
frame.data[0]++;
sequence++;
if (verbose && !sequence)
printf("sequence wrap around (%d)\n", seq_wrap++);
}
}
int main(int argc, char **argv)
{
struct sigaction act = {
.sa_handler = sig_handler,
};
struct sockaddr_can addr = {
.can_family = AF_CAN,
};
char *interface = "can0";
bool extended = false;
bool brs = false;
bool receive = false;
int opt;
sigaction(SIGINT, &act, NULL);
sigaction(SIGTERM, &act, NULL);
sigaction(SIGHUP, &act, NULL);
struct option long_options[] = {
{ "extended", no_argument, 0, 'e' },
{ "canfd", no_argument, 0, 'f' },
{ "strict", no_argument, 0, 's' },
{ "brs", no_argument, 0, 'b' },
{ "identifier", required_argument, 0, 'i' },
{ "loop", required_argument, 0, 'l' },
{ "poll", no_argument, 0, 'p' },
{ "quit", optional_argument, 0, 'q' },
{ "receive", no_argument, 0, 'r' },
{ "verbose", no_argument, 0, 'v' },
{ "help", no_argument, 0, 'h' },
{ 0, 0, 0, 0 },
};
while ((opt = getopt_long(argc, argv, "efsbi:pq::rvh?", long_options, NULL)) != -1) {
switch (opt) {
case 'e':
extended = true;
break;
case 'f':
canfd = true;
break;
case 's':
canfd_strict = true;
break;
case 'b':
brs = true; /* bitrate switch implies CAN-FD */
canfd = true;
break;
case 'i':
filter->can_id = strtoul(optarg, NULL, 0);
break;
case 'r':
receive = true;
break;
case 'l':
if (optarg) {
loopcount = strtoul(optarg, NULL, 0);
infinite = false;
} else {
infinite = true;
}
break;
case 'p':
use_poll = true;
break;
case 'q':
if (optarg)
drop_until_quit = strtoul(optarg, NULL, 0);
else
drop_until_quit = 1;
break;
case 'v':
verbose++;
break;
case 'h':
case '?':
print_usage(basename(argv[0]));
exit(EXIT_SUCCESS);
break;
default:
print_usage(basename(argv[0]));
exit(EXIT_FAILURE);
break;
}
}
if (argv[optind] != NULL)
interface = argv[optind];
if (extended) {
filter->can_mask = CAN_EFF_MASK;
filter->can_id &= CAN_EFF_MASK;
filter->can_id |= CAN_EFF_FLAG;
} else {
filter->can_mask = CAN_SFF_MASK;
filter->can_id &= CAN_SFF_MASK;
}
frame.can_id = filter->can_id;
filter->can_mask |= CAN_EFF_FLAG;
printf("interface = %s\n", interface);
s = socket(AF_CAN, SOCK_RAW, CAN_RAW);
if (s < 0) {
perror("socket()");
exit(EXIT_FAILURE);
}
if (strcmp(ANYDEV, interface)) {
addr.can_ifindex = if_nametoindex(interface);
if (!addr.can_ifindex) {
perror("if_nametoindex()");
exit(EXIT_FAILURE);
}
}
/* first don't recv. any msgs */
if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0)) {
perror("setsockopt()");
exit(EXIT_FAILURE);
}
if (canfd) {
const int enable_canfd = 1;
struct ifreq ifr;
strncpy(ifr.ifr_name, interface, sizeof(ifr.ifr_name));
/* check if the frame fits into the CAN netdevice */
if (ioctl(s, SIOCGIFMTU, &ifr) < 0) {
perror("SIOCGIFMTU");
exit(EXIT_FAILURE);
}
if (ifr.ifr_mtu != CANFD_MTU && ifr.ifr_mtu != CANXL_MTU) {
printf("CAN interface is only Classical CAN capable - sorry.\n");
exit(EXIT_FAILURE);
}
/* interface is ok - try to switch the socket into CAN FD mode */
if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, sizeof(enable_canfd))) {
printf("error when enabling CAN FD support\n");
exit(EXIT_FAILURE);
}
} else {
canfd_strict = false;
}
if (brs)
frame.flags |= CANFD_BRS;
if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
perror("bind()");
exit(EXIT_FAILURE);
}
if (receive)
do_receive();
else
do_send();
if (signal_num)
return 128 + signal_num;
exit(EXIT_SUCCESS);
}