diff --git a/.gitignore b/.gitignore index c2291bb..fb2edb5 100644 --- a/.gitignore +++ b/.gitignore @@ -32,6 +32,7 @@ GNUmakefile.in /canlogserver /canplayer /cansend +/cansequence /cansniffer /isotpdump /isotpperf diff --git a/Android.mk b/Android.mk index ee9fa5c..fbdf2ae 100644 --- a/Android.mk +++ b/Android.mk @@ -146,6 +146,35 @@ LOCAL_VENDOR_MODULE := true include $(BUILD_EXECUTABLE) +# +# cansequence +# + +include $(CLEAR_VARS) + +LOCAL_SRC_FILES := cansequence.c +LOCAL_MODULE := cansequence +LOCAL_MODULE_TAGS := optional +LOCAL_C_INCLUDES := $(LOCAL_PATH)/include/ +LOCAL_CFLAGS := $(PRIVATE_LOCAL_CFLAGS) + +include $(BUILD_EXECUTABLE) + +# +# cansequence +# + +include $(CLEAR_VARS) + +LOCAL_SRC_FILES := cansequence.c +LOCAL_MODULE := cansequence +LOCAL_MODULE_TAGS := optional +LOCAL_STATIC_LIBRARIES := libcan +LOCAL_C_INCLUDES := $(LOCAL_PATH)/include/ +LOCAL_CFLAGS := $(PRIVATE_LOCAL_CFLAGS) + +include $(BUILD_EXECUTABLE) + # # bcmserver # diff --git a/CMakeLists.txt b/CMakeLists.txt index 6c619df..33e0b0b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -33,6 +33,7 @@ set(PROGRAMS_CANLIB canlogserver canplayer cansend + cansequence log2asc log2long ) diff --git a/GNUmakefile.am b/GNUmakefile.am index f47ddd3..10f8584 100644 --- a/GNUmakefile.am +++ b/GNUmakefile.am @@ -50,6 +50,7 @@ bin_PROGRAMS = \ canlogserver \ canplayer \ cansend \ + cansequence \ cansniffer \ isotpdump \ isotpperf \ diff --git a/Makefile b/Makefile index 4712471..2810d9e 100644 --- a/Makefile +++ b/Makefile @@ -89,6 +89,7 @@ PROGRAMS := \ candump \ canfdtest \ cangen \ + cansequence \ canlogserver \ canplayer \ cansend \ @@ -116,6 +117,7 @@ cangen.o: lib.h canlogserver.o: lib.h canplayer.o: lib.h cansend.o: lib.h +cansequence.o: lib.h log2asc.o: lib.h log2long.o: lib.h j1939acd.o: libj1939.h @@ -131,6 +133,7 @@ cangen: cangen.o lib.o canlogserver: canlogserver.o lib.o canplayer: canplayer.o lib.o cansend: cansend.o lib.o +cansequence: cansequence.o lib.o log2asc: log2asc.o lib.o log2long: log2long.o lib.o j1939acd: j1939acd.o libj1939.o diff --git a/README.md b/README.md index af18ea2..4b3037b 100644 --- a/README.md +++ b/README.md @@ -13,6 +13,7 @@ subsystem (aka SocketCAN): * canplayer : replay CAN logfiles * cansend : send a single frame * cangen : generate (random) CAN traffic +* cansequence : send and check sequence of CAN frames with incrementing payload * cansniffer : display CAN data content differences #### CAN access via IP sockets diff --git a/cansequence.c b/cansequence.c new file mode 100644 index 0000000..3f72aae --- /dev/null +++ b/cansequence.c @@ -0,0 +1,367 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +// Copyright (c) 2007, 2008, 2009, 2010, 2014, 2015, 2019 Pengutronix, +// Marc Kleine-Budde +// Copyright (c) 2005 Pengutronix, +// Sascha Hauer + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include + +#define CAN_ID_DEFAULT (2) + +extern int optind, opterr, optopt; + +static int s = -1; +static bool running = true; +static bool infinite = true; +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 can_frame frame = { + .can_dlc = 1, +}; +static struct can_filter filter[] = { + { .can_id = CAN_ID_DEFAULT, }, +}; + +static void print_usage(char *prg) +{ + fprintf(stderr, "Usage: %s [] [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 extended frame\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 quit if 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" + " --version print version information and exit\n", + prg, CAN_ID_DEFAULT); +} + +static void sig_handler(int signo) +{ + running = false; +} + + +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; + uint32_t sequence_delta = 0; + uint32_t sequence = 0; + unsigned int overflow_old = 0; + can_err_mask_t err_mask = CAN_ERR_MASK; + + 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 = sizeof(frame); + msg.msg_controllen = sizeof(ctrlmsg); + msg.msg_flags = 0; + + nbytes = recvmsg(s, &msg, 0); + if (nbytes < 0) { + perror("read()"); + 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 (sequence_init) { + sequence_init = false; + sequence = sequence_rx; + } + + sequence_delta = (sequence_rx - sequence) & sequence_mask; + 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: %6u, RX: %6u expected: %3u missing: %4u skt overfl d: %4u a: %4u delta: %3u incident: %u\n", + sequence, sequence_rx, + sequence & sequence_mask, sequence_delta, + overflow_delta, overflow, + sequence_delta - overflow_delta, + drop_count); + + if (drop_count == drop_until_quit) + exit(EXIT_FAILURE); + + sequence = sequence_rx; + overflow_old = overflow; + } else if (verbose > 1) { + printf("sequence CNT: %6u, RX: %6u\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; + + while ((infinite || loopcount--) && running) { + ssize_t len; + + if (verbose > 1) + printf("sending frame. sequence number: %d\n", sequence); + + again: + len = write(s, &frame, sizeof(frame)); + 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 == -1 && errno != -EINTR) { + perror("poll()"); + exit(EXIT_FAILURE); + } + } + case EINTR: /* fallthrough */ + goto again; + default: + perror("write"); + exit(EXIT_FAILURE); + } + } + + (unsigned char)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 ifreq ifr; + struct sockaddr_can addr; + char *interface = "can0"; + int family = PF_CAN, type = SOCK_RAW, proto = CAN_RAW; + int extended = 0; + int receive = 0; + int opt; + + sigaction(SIGINT, &act, NULL); + sigaction(SIGTERM, &act, NULL); + sigaction(SIGHUP, &act, NULL); + + struct option long_options[] = { + { "extended", no_argument, 0, 'e' }, + { "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, "ei:pq::rvh", long_options, NULL)) != -1) { + switch (opt) { + case 'e': + extended = 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': + 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, family = %d, type = %d, proto = %d\n", + interface, family, type, proto); + + s = socket(family, type, proto); + if (s < 0) { + perror("socket()"); + exit(EXIT_FAILURE); + } + + addr.can_family = family; + strncpy(ifr.ifr_name, interface, sizeof(ifr.ifr_name)); + if (ioctl(s, SIOCGIFINDEX, &ifr)) { + perror("ioctl()"); + exit(EXIT_FAILURE); + } + addr.can_ifindex = ifr.ifr_ifindex; + + /* first don't recv. any msgs */ + if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0)) { + perror("setsockopt()"); + exit(EXIT_FAILURE); + } + + if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("bind()"); + exit(EXIT_FAILURE); + } + + if (receive) + do_receive(); + else + do_send(); + + exit(EXIT_SUCCESS); +}