From ac181f305fbc8ba9ad6b8312f032133414360fea Mon Sep 17 00:00:00 2001 From: Marc Kleine-Budde Date: Thu, 18 Jan 2018 10:33:48 +0100 Subject: [PATCH 01/10] travis: include config file in tarball Signed-off-by: Marc Kleine-Budde --- GNUmakefile.am | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/GNUmakefile.am b/GNUmakefile.am index e403846..c824f55 100644 --- a/GNUmakefile.am +++ b/GNUmakefile.am @@ -58,7 +58,8 @@ bin_PROGRAMS = \ slcanpty EXTRA_DIST = \ - autogen.sh + autogen.sh \ + .travis.yml MAINTAINERCLEANFILES = \ configure \ From eabe3a69f03a0bdc7f79f031f72d5f3ede133e3e Mon Sep 17 00:00:00 2001 From: Marc Kleine-Budde Date: Wed, 17 Jan 2018 18:00:46 +0100 Subject: [PATCH 02/10] travis: we don't need sudo --- .travis.yml | 3 --- 1 file changed, 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 4bf3a9e..de7d6f9 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,21 +5,18 @@ matrix: - env: test="x64 4.8.4 (make)" os: linux dist: trusty - sudo: required compiler: gcc script: make - env: test="x64 4.8.4 (autotools)" os: linux dist: trusty - sudo: required compiler: gcc script: ./autogen.sh && ./configure && make distcheck - env: test="x64 5.0 (autotools)" os: linux dist: trusty - sudo: required compiler: clang script: ./autogen.sh && ./configure && make distcheck From c234ebc4a5d0f7c9b623afdddc17e7d8d9602dc3 Mon Sep 17 00:00:00 2001 From: Marc Kleine-Budde Date: Wed, 17 Jan 2018 18:20:11 +0100 Subject: [PATCH 03/10] travis: build add precise env Signed-off-by: Marc Kleine-Budde --- .travis.yml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/.travis.yml b/.travis.yml index de7d6f9..9ad61cc 100644 --- a/.travis.yml +++ b/.travis.yml @@ -8,6 +8,13 @@ matrix: compiler: gcc script: make + - env: test="x64 precise (autotools)" + os: linux + dist: precise + sudo: required + compiler: gcc + script: ./autogen.sh && ./configure && make distcheck + - env: test="x64 4.8.4 (autotools)" os: linux dist: trusty From 6e22cc49a7ebc676de754fab70f8f1b59391b3b5 Mon Sep 17 00:00:00 2001 From: Kurt Van Dijck Date: Wed, 13 Mar 2013 21:05:55 +0100 Subject: [PATCH 04/10] can-j1939: update headers with j1939 Signed-off-by: Kurt Van Dijck Signed-off-by: Marc Kleine-Budde --- GNUmakefile.am | 1 + include/linux/can.h | 20 +++++++++- include/linux/can/j1939.h | 81 +++++++++++++++++++++++++++++++++++++++ 3 files changed, 101 insertions(+), 1 deletion(-) create mode 100644 include/linux/can/j1939.h diff --git a/GNUmakefile.am b/GNUmakefile.am index c824f55..db81487 100644 --- a/GNUmakefile.am +++ b/GNUmakefile.am @@ -18,6 +18,7 @@ noinst_HEADERS = \ include/linux/can/gw.h \ include/linux/can.h \ include/linux/can/isotp.h \ + include/linux/can/j1939.h \ include/linux/can/netlink.h \ include/linux/can/raw.h \ include/linux/can/vxcan.h diff --git a/include/linux/can.h b/include/linux/can.h index 984915c..3ed2f17 100644 --- a/include/linux/can.h +++ b/include/linux/can.h @@ -157,7 +157,8 @@ struct canfd_frame { #define CAN_TP20 4 /* VAG Transport Protocol v2.0 */ #define CAN_MCNET 5 /* Bosch MCNet */ #define CAN_ISOTP 6 /* ISO 15765-2 Transport Protocol */ -#define CAN_NPROTO 7 +#define CAN_J1939 7 /* SAE J1939 */ +#define CAN_NPROTO 8 #define SOL_CAN_BASE 100 @@ -185,6 +186,23 @@ struct sockaddr_can { canid_t tx_id; } tp; + /* J1939 address information */ + struct { + /* 8 byte name when using dynamic addressing */ + __u64 name; + /* + * pgn: + * 8bit: PS in PDU2 case, else 0 + * 8bit: PF + * 1bit: DP + * 1bit: reserved + */ + __u32 pgn; + + /* 1byte address */ + __u8 addr; + } j1939; + /* reserved for future CAN protocols address information */ } can_addr; }; diff --git a/include/linux/can/j1939.h b/include/linux/can/j1939.h new file mode 100644 index 0000000..cd4661a --- /dev/null +++ b/include/linux/can/j1939.h @@ -0,0 +1,81 @@ +/* + * j1939.h + * + * Copyright (c) 2010-2011 EIA Electronics + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + */ + +#ifndef _UAPI_CAN_J1939_H_ +#define _UAPI_CAN_J1939_H_ + +#include +#include +#include + +#define J1939_IDLE_ADDR 0xfe +#define J1939_NO_ADDR 0xff +#define J1939_NO_NAME 0 +#define J1939_NO_PGN 0x40000 + +/* J1939 Parameter Group Number + * + * bit 0-7 : PDU Specific (PS) + * bit 8-15 : PDU Format (PF) + * bit 16 : Data Page (DP) + * bit 17 : Reserved (R) + * bit 19-31 : set to zero + */ +typedef __u32 pgn_t; + +/* J1939 Priority + * + * bit 0-2 : Priority (P) + * bit 3-7 : set to zero + */ +typedef __u8 priority_t; + +/* J1939 NAME + * + * bit 0-20 : Identity Number + * bit 21-31 : Manufacturer Code + * bit 32-34 : ECU Instance + * bit 35-39 : Function Instance + * bit 40-47 : Function + * bit 48 : Reserved + * bit 49-55 : Vehicle System + * bit 56-59 : Vehicle System Instance + * bit 60-62 : Industry Group + * bit 63 : Arbitrary Address Capable + */ +typedef __u64 name_t; + +/* J1939 socket options */ +#define SOL_CAN_J1939 (SOL_CAN_BASE + CAN_J1939) +enum { + SO_J1939_FILTER = 1, /* set filters */ + SO_J1939_PROMISC = 2, /* set/clr promiscuous mode */ + SO_J1939_RECV_OWN = 3, + SO_J1939_SEND_PRIO = 4, +}; + +enum { + SCM_J1939_DEST_ADDR = 1, + SCM_J1939_DEST_NAME = 2, + SCM_J1939_PRIO = 3, +}; + +struct j1939_filter { + name_t name; + name_t name_mask; + __u8 addr; + __u8 addr_mask; + pgn_t pgn; + pgn_t pgn_mask; +}; + +#define J1939_FILTER_MAX 512 /* maximum number of j1939_filter set via setsockopt() */ + +#endif /* !_UAPI_CAN_J1939_H_ */ From f66e18aed0ed12e8febe90f346bcd012e2077f0c Mon Sep 17 00:00:00 2001 From: Kurt Van Dijck Date: Wed, 13 Mar 2013 20:28:44 +0100 Subject: [PATCH 05/10] can-j1939: add libj1939 libj1939 provides a parser for struct sockaddr_can with j1939 info Signed-off-by: Kurt Van Dijck Signed-off-by: Marc Kleine-Budde --- GNUmakefile.am | 6 +- Makefile | 2 +- libj1939.c | 176 +++++++++++++++++++++++++++++++++++++++++++++++++ libj1939.h | 19 ++++++ 4 files changed, 201 insertions(+), 2 deletions(-) create mode 100644 libj1939.c create mode 100644 libj1939.h diff --git a/GNUmakefile.am b/GNUmakefile.am index db81487..3543955 100644 --- a/GNUmakefile.am +++ b/GNUmakefile.am @@ -12,6 +12,7 @@ LDADD = \ noinst_HEADERS = \ canframelen.h \ lib.h \ + libj1939.h \ terminal.h \ include/linux/can/bcm.h \ include/linux/can/error.h \ @@ -25,12 +26,15 @@ noinst_HEADERS = \ noinst_LTLIBRARIES = \ - libcan.la + libcan.la \ + libj1939.la libcan_la_SOURCES = \ lib.c \ canframelen.c +libj1939_la_SOURCES = \ + libj1939.c bin_PROGRAMS = \ asc2log \ diff --git a/Makefile b/Makefile index 50d568d..46eaa7b 100644 --- a/Makefile +++ b/Makefile @@ -73,7 +73,7 @@ install: cp -f $(PROGRAMS) $(DESTDIR)$(PREFIX)/bin distclean: - rm -f $(PROGRAMS) *.o *~ + rm -f $(PROGRAMS) $(LIBRARIES) *.o *~ cansend.o: lib.h cangen.o: lib.h diff --git a/libj1939.c b/libj1939.c new file mode 100644 index 0000000..ec3c108 --- /dev/null +++ b/libj1939.c @@ -0,0 +1,176 @@ +/* + * Copyright (c) 2011 EIA Electronics + * + * Authors: + * Kurt Van Dijck + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include "libj1939.h" + +/* static data */ +static struct if_nameindex *saved; + +__attribute__((destructor)) +static void libj1939_cleanup(void) +{ + if (saved) + if_freenameindex(saved); + saved = 0; +} + +static inline void fetch_names(void) +{ + if (!saved) { + saved = if_nameindex(); + if (!saved) + error(1, errno, "if_nameindex()"); + } +} + +/* retrieve name */ +const char *libj1939_ifnam(int ifindex) +{ + const struct if_nameindex *lp, *cached = saved; + + fetch_names(); + + for (lp = saved; lp->if_index; ++lp) { + if (lp->if_index == ifindex) + return lp->if_name; + } + if (cached) { + /* + * the list was not recent + * iterate twice, but force a refresh now + * recursion stops since the 'saved' pointer is cleaned + */ + libj1939_cleanup(); + return libj1939_ifnam(ifindex); + } else + return NULL; +} + +/* retrieve index */ +int libj1939_ifindex(const char *str) +{ + const struct if_nameindex *lp, *cached = saved; + char *endp; + int ret; + + ret = strtol(str, &endp, 0); + if (!*endp) + /* did some good parse */ + return ret; + + fetch_names(); + for (lp = saved; lp->if_index; ++lp) { + if (!strcmp(lp->if_name, str)) + return lp->if_index; + } + if (cached) { + libj1939_cleanup(); + return libj1939_ifindex(str); + } else + return 0; +} + +int libj1939_str2addr(const char *str, char **endp, struct sockaddr_can *can) +{ + char *p; + const char *pstr; + uint64_t tmp64; + unsigned long tmp; + + if (!endp) + endp = &p; + memset(can, 0, sizeof(*can)); + can->can_family = AF_CAN; + can->can_addr.j1939.name = J1939_NO_NAME; + can->can_addr.j1939.addr = J1939_NO_ADDR; + can->can_addr.j1939.pgn = J1939_NO_PGN; + + pstr = strchr(str, ':'); + if (pstr) { + char tmp[IFNAMSIZ]; + if ((pstr - str) >= IFNAMSIZ) + return -1; + strncpy(tmp, str, pstr - str); + tmp[pstr - str] = 0; + can->can_ifindex = libj1939_ifindex(tmp); + } else { + can->can_ifindex = libj1939_ifindex(str); + if (can->can_ifindex) { + if (endp) + *endp = (char *)&str[strlen(str)]; + return 0; + } + } + if (pstr) + ++pstr; + else + pstr = str; + + + tmp64 = strtoull(pstr, endp, 16); + if (*endp <= pstr) + return 0; + if ((*endp - pstr) == 2) + can->can_addr.j1939.addr = tmp64; + else + can->can_addr.j1939.name = tmp64; + if (!**endp) + return 0; + + str = *endp + 1; + tmp = strtoul(str, endp, 16); + if (*endp > str) + can->can_addr.j1939.pgn = tmp; + return 0; +} + +const char *libj1939_addr2str(const struct sockaddr_can *can) +{ + char *str; + static char buf[128]; + + str = buf; + if (can->can_ifindex) { + const char *ifname; + ifname = libj1939_ifnam(can->can_ifindex); + if (!ifname) + str += sprintf(str, "#%i:", can->can_ifindex); + else + str += sprintf(str, "%s:", ifname); + } + if (can->can_addr.j1939.name) { + str += sprintf(str, "%016llx", (unsigned long long)can->can_addr.j1939.name); + if (can->can_addr.j1939.pgn == 0x0ee00) + str += sprintf(str, ".%02x", can->can_addr.j1939.addr); + } else if (can->can_addr.j1939.addr <= 0xfe) + str += sprintf(str, "%02x", can->can_addr.j1939.addr); + else + str += sprintf(str, "-"); + if (can->can_addr.j1939.pgn <= 0x3ffff) + str += sprintf(str, ",%05x", can->can_addr.j1939.pgn); + + return buf; +} + diff --git a/libj1939.h b/libj1939.h new file mode 100644 index 0000000..46e2277 --- /dev/null +++ b/libj1939.h @@ -0,0 +1,19 @@ +#include +#include +#include + +#ifndef J1939_LIB_H +#define J1939_LIB_H + +#ifdef __cplusplus +extern "C" { +#endif + +extern int libj1939_str2addr(const char *str, char **endp, struct sockaddr_can *can); +extern const char *libj1939_addr2str(const struct sockaddr_can *can); + +#ifdef __cplusplus +} +#endif + +#endif From 6e8753e4d2d62a0078f385ff49fb69000e620159 Mon Sep 17 00:00:00 2001 From: Kurt Van Dijck Date: Wed, 13 Mar 2013 20:33:40 +0100 Subject: [PATCH 06/10] can-j1939: add jspy 'sniffer' program Signed-off-by: Kurt Van Dijck Signed-off-by: Marc Kleine-Budde --- .gitignore | 1 + GNUmakefile.am | 3 + Makefile | 5 + jspy.c | 305 +++++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 314 insertions(+) create mode 100644 jspy.c diff --git a/.gitignore b/.gitignore index b7d9cbd..c3cfde9 100644 --- a/.gitignore +++ b/.gitignore @@ -40,6 +40,7 @@ GNUmakefile.in /isotpserver /isotpsniffer /isotptun +/jspy /log2asc /log2long /slcan_attach diff --git a/GNUmakefile.am b/GNUmakefile.am index 3543955..7aec699 100644 --- a/GNUmakefile.am +++ b/GNUmakefile.am @@ -56,12 +56,15 @@ bin_PROGRAMS = \ isotpserver \ isotpsniffer \ isotptun \ + jspy \ log2asc \ log2long \ slcan_attach \ slcand \ slcanpty +jspy_LDADD = libj1939.la + EXTRA_DIST = \ autogen.sh \ .travis.yml diff --git a/Makefile b/Makefile index 46eaa7b..01a59c2 100644 --- a/Makefile +++ b/Makefile @@ -55,12 +55,14 @@ CPPFLAGS += -Iinclude \ PROGRAMS_ISOTP = isotpdump isotprecv isotpsend isotpsniffer isotptun isotpserver isotpperf PROGRAMS_CANGW = cangw PROGRAMS_SLCAN = slcan_attach slcand +PROGRAMS_J1939 = jspy PROGRAMS = can-calc-bit-timing candump cansniffer cansend canplayer cangen canbusload\ log2long log2asc asc2log\ canlogserver bcmserver\ $(PROGRAMS_ISOTP)\ $(PROGRAMS_CANGW)\ $(PROGRAMS_SLCAN)\ + $(PROGRAMS_J1939)\ slcanpty canfdtest all: $(PROGRAMS) @@ -84,6 +86,7 @@ canbusload.o: lib.h log2long.o: lib.h log2asc.o: lib.h asc2log.o: lib.h +jspy.o: libj1939.h canframelen.o: canframelen.h cansend: cansend.o lib.o @@ -95,3 +98,5 @@ log2long: log2long.o lib.o log2asc: log2asc.o lib.o asc2log: asc2log.o lib.o canbusload: canbusload.o canframelen.o + +jspy: jspy.o libj1939.o diff --git a/jspy.c b/jspy.c new file mode 100644 index 0000000..55ddeb7 --- /dev/null +++ b/jspy.c @@ -0,0 +1,305 @@ +/* + * Copyright (c) 2011 EIA Electronics + * + * Authors: + * Kurt Van Dijck + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "libj1939.h" + +/* + * getopt + */ +static const char help_msg[] = + "jspy: An SAE J1939 spy utility" "\n" + "Usage: jspy [OPTION...] [[IFACE:][NAME|SA][,PGN]]" "\n" + "\n" + " -v, --verbose Increase verbosity" "\n" + " -P, --promisc Run in promiscuous mode" "\n" + " (= receive traffic not for this ECU)" "\n" + " -b, --block=SIZE Use a receive buffer of SIZE (default 1024)" "\n" + " -t, --time[=a|d|z|A] Show time: (a)bsolute, (d)elta, (z)ero, (A)bsolute w date" "\n" + ; + +#ifdef _GNU_SOURCE +static struct option long_opts[] = { + { "help", no_argument, NULL, '?', }, + { "verbose", no_argument, NULL, 'v', }, + + { "promisc", no_argument, NULL, 'P', }, + { "block", required_argument, NULL, 'b', }, + { "time", optional_argument, NULL, 't', }, + { }, +}; +#else +#define getopt_long(argc, argv, optstring, longopts, longindex) \ + getopt((argc), (argv), (optstring)) +#endif +static const char optstring[] = "vPb:t::?"; + +/* + * static variables + */ +static struct { + int verbose; + struct sockaddr_can addr; + int promisc; + int time; + int pkt_len; +} s = { + .pkt_len = 1024, + .addr.can_addr.j1939 = { + .name = J1939_NO_NAME, + .addr = J1939_NO_ADDR, + .pgn = J1939_NO_PGN, + }, +}; + +/* + * usefull buffers + */ +static const int ival_1 = 1; + +static char ctrlmsg[ + CMSG_SPACE(sizeof(struct timeval)) + + CMSG_SPACE(sizeof(uint8_t)) /* dest addr */ + + CMSG_SPACE(sizeof(uint64_t)) /* dest name */ + + CMSG_SPACE(sizeof(uint8_t)) /* priority */ + ]; +static struct iovec iov; +static struct msghdr msg; +static struct cmsghdr *cmsg; +static uint8_t *buf; + +/* + * program + */ +int main(int argc, char **argv) +{ + int ret, sock, j, opt; + unsigned int len; + struct timeval tref, tdut, ttmp; + struct sockaddr_can src; + struct j1939_filter filt; + int filter = 0; + uint8_t priority, dst_addr; + uint64_t dst_name; + long recvflags; + +#ifdef _GNU_SOURCE + program_invocation_name = program_invocation_short_name; +#endif + /* argument parsing */ + while ((opt = getopt_long(argc, argv, optstring, long_opts, NULL)) != -1) + switch (opt) { + case 'v': + ++s.verbose; + break; + case 'b': + s.pkt_len = strtoul(optarg, 0, 0); + break; + case 'P': + ++s.promisc; + break; + case 't': + if (optarg) { + if (!strchr("adzA", optarg[0])) + error(1, 0, "unknown time option '%c'", optarg[0]); + s.time = optarg[0]; + } else { + s.time = 'z'; + } + break; + default: + fputs(help_msg, stderr); + exit(1); + break; + } + if (argv[optind]) { + optarg = argv[optind]; + ret = libj1939_str2addr(optarg, 0, &s.addr); + if (ret < 0) { + error(0, 0, "bad URI %s", optarg); + return 1; + } + } + + buf = malloc(s.pkt_len); + if (!buf) + error(1, errno, "malloc %u", s.pkt_len); + + /* setup socket */ + sock = socket(PF_CAN, SOCK_DGRAM, CAN_J1939); + if (sock < 0) + error(1, errno, "socket(can, dgram, j1939)"); + + memset(&filt, 0, sizeof(filt)); + if (s.addr.can_addr.j1939.name) { + filt.name = s.addr.can_addr.j1939.name; + filt.name_mask = ~0ULL; + ++filter; + } + if (s.addr.can_addr.j1939.addr < 0xff) { + filt.addr = s.addr.can_addr.j1939.addr; + filt.addr_mask = ~0; + ++filter; + } + if (s.addr.can_addr.j1939.pgn <= 0x3ffff) { + filt.pgn = s.addr.can_addr.j1939.pgn; + filt.pgn_mask = ~0; + ++filter; + } + if (filter) { + ret = setsockopt(sock, SOL_CAN_J1939, SO_J1939_FILTER, &filt, sizeof(filt)); + if (ret < 0) + error(1, errno, "setsockopt filter"); + } + + if (s.promisc) { + ret = setsockopt(sock, SOL_CAN_J1939, SO_J1939_PROMISC, &ival_1, sizeof(ival_1)); + if (ret < 0) + error(1, errno, "setsockopt promisc"); + } + + if (s.time) { + ret = setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &ival_1, sizeof(ival_1)); + if (ret < 0) + error(1, errno, "setsockopt timestamp"); + } + ret = setsockopt(sock, SOL_SOCKET, SO_RCVBUF, &s.pkt_len, sizeof(s.pkt_len)); + if (ret < 0) + error(1, errno, "setsockopt rcvbuf %u", s.pkt_len); + + /* bind(): to default, only ifindex is used. */ + memset(&src, 0, sizeof(src)); + src.can_ifindex = s.addr.can_ifindex; + src.can_family = AF_CAN; + src.can_addr.j1939.name = J1939_NO_NAME; + src.can_addr.j1939.addr = J1939_NO_ADDR; + src.can_addr.j1939.pgn = J1939_NO_PGN; + ret = bind(sock, (void *)&src, sizeof(src)); + if (ret < 0) + error(1, errno, "bind(%s)", argv[1]); + + /* these settings are static and can be held out of the hot path */ + iov.iov_base = &buf[0]; + msg.msg_name = &src; + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = &ctrlmsg; + + memset(&tref, 0, sizeof(tref)); + if (s.verbose) + error(0, 0, "listening"); + while (1) { + /* these settings may be modified by recvmsg() */ + iov.iov_len = s.pkt_len; + msg.msg_namelen = sizeof(src); + msg.msg_controllen = sizeof(ctrlmsg); + msg.msg_flags = 0; + + ret = recvmsg(sock, &msg, 0); + //ret = recvfrom(buf, s.pkt_len, 0, (void *)&addr, &len); + if (ret < 0) { + switch (errno) { + case ENETDOWN: + error(0, errno, "ifindex %i", s.addr.can_ifindex); + continue; + case EINTR: + continue; + default: + error(1, errno, "recvmsg(ifindex %i)", s.addr.can_ifindex); + break; + } + } + len = ret; + recvflags = 0; + dst_addr = 0; + priority = 0; + for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) { + switch (cmsg->cmsg_level) { + case SOL_SOCKET: + if (cmsg->cmsg_type == SCM_TIMESTAMP) { + memcpy(&tdut, CMSG_DATA(cmsg), sizeof(tdut)); + recvflags |= 1 << cmsg->cmsg_type; + } + break; + case SOL_CAN_J1939: + recvflags |= 1 << cmsg->cmsg_type; + if (cmsg->cmsg_type == SCM_J1939_DEST_ADDR) + dst_addr = *CMSG_DATA(cmsg); + else if (cmsg->cmsg_type == SCM_J1939_DEST_NAME) + memcpy(&dst_name, CMSG_DATA(cmsg), cmsg->cmsg_len - CMSG_LEN(0)); + else if (cmsg->cmsg_type == SCM_J1939_PRIO) + priority = *CMSG_DATA(cmsg); + break; + } + + } + if (recvflags & (1 << SCM_TIMESTAMP)) { + if ('z' == s.time) { + if (!tref.tv_sec) + tref = tdut; + timersub(&tdut, &tref, &ttmp); + tdut = ttmp; + goto abs_time; + } else if ('d' == s.time) { + timersub(&tdut, &tref, &ttmp); + tref = tdut; + tdut = ttmp; + goto abs_time; + } else if ('a' == s.time) { + abs_time: + printf("(%lu.%04lu)", tdut.tv_sec, tdut.tv_usec / 100); + } else if ('A' == s.time) { + struct tm tm; + tm = *localtime(&tdut.tv_sec); + printf("(%04u%02u%02uT%02u%02u%02u.%04lu)", + tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday, + tm.tm_hour, tm.tm_min, tm.tm_sec, + tdut.tv_usec/100); + } + } + printf(" %s ", libj1939_addr2str(&src)); + if (recvflags & (1 << SCM_J1939_DEST_NAME)) + printf("%016llx ", (unsigned long long)dst_name); + else if (recvflags & (1 << SCM_J1939_DEST_ADDR)) + printf("%02x ", dst_addr); + else + printf("- "); + printf("!%u ", priority); + + printf("[%i%s]", len, (msg.msg_flags & MSG_TRUNC) ? "..." : ""); + for (j = 0; j < len; ) { + int end = j + 4; + if (end > len) + end = len; + printf(" "); + for (; j < end; ++j) + printf("%02x", buf[j]); + } + printf("\n"); + } + + free(buf); + return 0; +} + From 126896e1734256eadc52718a80731e7e7df3f198 Mon Sep 17 00:00:00 2001 From: Kurt Van Dijck Date: Wed, 13 Mar 2013 20:44:57 +0100 Subject: [PATCH 07/10] can-j1939: add jsr program jsr sends data on to j1939, and received j1939 data is put on . Signed-off-by: Kurt Van Dijck Signed-off-by: Marc Kleine-Budde --- .gitignore | 1 + GNUmakefile.am | 2 + Makefile | 4 +- jsr.c | 221 +++++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 227 insertions(+), 1 deletion(-) create mode 100644 jsr.c diff --git a/.gitignore b/.gitignore index c3cfde9..02274a2 100644 --- a/.gitignore +++ b/.gitignore @@ -41,6 +41,7 @@ GNUmakefile.in /isotpsniffer /isotptun /jspy +/jsr /log2asc /log2long /slcan_attach diff --git a/GNUmakefile.am b/GNUmakefile.am index 7aec699..a75dad1 100644 --- a/GNUmakefile.am +++ b/GNUmakefile.am @@ -57,6 +57,7 @@ bin_PROGRAMS = \ isotpsniffer \ isotptun \ jspy \ + jsr \ log2asc \ log2long \ slcan_attach \ @@ -64,6 +65,7 @@ bin_PROGRAMS = \ slcanpty jspy_LDADD = libj1939.la +jsr_LDADD = libj1939.la EXTRA_DIST = \ autogen.sh \ diff --git a/Makefile b/Makefile index 01a59c2..b9a5182 100644 --- a/Makefile +++ b/Makefile @@ -55,7 +55,7 @@ CPPFLAGS += -Iinclude \ PROGRAMS_ISOTP = isotpdump isotprecv isotpsend isotpsniffer isotptun isotpserver isotpperf PROGRAMS_CANGW = cangw PROGRAMS_SLCAN = slcan_attach slcand -PROGRAMS_J1939 = jspy +PROGRAMS_J1939 = jspy jsr PROGRAMS = can-calc-bit-timing candump cansniffer cansend canplayer cangen canbusload\ log2long log2asc asc2log\ canlogserver bcmserver\ @@ -87,6 +87,7 @@ log2long.o: lib.h log2asc.o: lib.h asc2log.o: lib.h jspy.o: libj1939.h +jsr.o: libj1939.h canframelen.o: canframelen.h cansend: cansend.o lib.o @@ -100,3 +101,4 @@ asc2log: asc2log.o lib.o canbusload: canbusload.o canframelen.o jspy: jspy.o libj1939.o +jsr: jsr.o libj1939.o diff --git a/jsr.c b/jsr.c new file mode 100644 index 0000000..0303af0 --- /dev/null +++ b/jsr.c @@ -0,0 +1,221 @@ +/* + * Copyright (c) 2011 EIA Electronics + * + * Authors: + * Kurt Van Dijck + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "libj1939.h" + +/* + * getopt + */ +static const char help_msg[] = + "jsr: An SAE J1939 send/recv utility" "\n" + "Usage: jsr [OPTION...] SOURCE [DEST]" "\n" + "\n" + " -v, --verbose Increase verbosity" "\n" + " -p, --priority=VAL J1939 priority (0..7, default 6)" "\n" + " -S, --serialize Strictly serialize outgoing packets" "\n" + " -s, --size Packet size, default autodetected" "\n" + "\n" + " SOURCE [IFACE:][NAME|SA][,PGN]" "\n" + " DEST [NAME|SA]" "\n" + ; + +#ifdef _GNU_SOURCE +static struct option long_opts[] = { + { "help", no_argument, NULL, '?', }, + { "verbose", no_argument, NULL, 'v', }, + + { "priority", required_argument, NULL, 'p', }, + { "size", required_argument, NULL, 's', }, + { "serialize", no_argument, NULL, 'S', }, + { }, +}; +#else +#define getopt_long(argc, argv, optstring, longopts, longindex) \ + getopt((argc), (argv), (optstring)) +#endif +static const char optstring[] = "vp:s:S?"; + +/* + * static variables: configurations + */ +static struct { + int verbose; + int sendflags; /* flags for sendto() */ + int pkt_len; + int priority; + int defined; + #define DEF_SRC 1 + #define DEF_DST 2 + #define DEF_PRIO 4 + struct sockaddr_can src, dst; +} s = { + .priority = 6, + .src.can_addr.j1939 = { + .name = J1939_NO_NAME, + .addr = J1939_NO_ADDR, + .pgn = J1939_NO_PGN, + }, + .dst.can_addr.j1939 = { + .name = J1939_NO_NAME, + .addr = J1939_NO_ADDR, + .pgn = J1939_NO_PGN, + }, +}; + +int main(int argc, char **argv) +{ + + int ret, sock, opt; + unsigned int len; + struct pollfd pfd[2]; + uint8_t *buf; + +#ifdef _GNU_SOURCE + program_invocation_name = program_invocation_short_name; +#endif + /* argument parsing */ + while ((opt = getopt_long(argc, argv, optstring, long_opts, NULL)) != -1) + switch (opt) { + case 'v': + ++s.verbose; + break; + case 's': + s.pkt_len = strtoul(optarg, 0, 0); + if (!s.pkt_len) + error(1, EINVAL, "packet size of %s", optarg); + break; + case 'p': + s.priority = strtoul(optarg, 0, 0); + s.defined |= DEF_PRIO; + break; + case 'S': + s.sendflags |= MSG_SYN; + break; + default: + fputs(help_msg, stderr); + exit(1); + break; + } + + if (argv[optind]) { + optarg = argv[optind++]; + ret = libj1939_str2addr(optarg, 0, &s.src); + if (ret < 0) + error(1, 0, "bad address spec [%s]", optarg); + s.defined |= DEF_SRC; + } + if (argv[optind]) { + optarg = argv[optind++]; + ret = libj1939_str2addr(optarg, 0, &s.dst); + if (ret < 0) + error(1, 0, "bad address spec [%s]", optarg); + s.defined |= DEF_DST; + } + + if (!s.pkt_len) { + struct stat st; + + if (fstat(STDIN_FILENO, &st) < 0) + error(1, errno, "stat stdin, could not determine buffer size"); + s.pkt_len = st.st_size ?: 1024; + } + + /* prepare */ + buf = malloc(s.pkt_len); + if (!buf) + error(1, errno, "malloc %u", s.pkt_len); + + sock = socket(PF_CAN, SOCK_DGRAM, CAN_J1939); + if (sock < 0) + error(1, errno, "socket(can, dgram, j1939)"); + + if (s.defined & DEF_PRIO) { + ret = setsockopt(sock, SOL_CAN_J1939, SO_J1939_SEND_PRIO, &s.priority, sizeof(s.priority)); + if (ret < 0) + error(1, errno, "setsockopt priority"); + } + if (s.defined & DEF_SRC) { + s.src.can_family = AF_CAN; + ret = bind(sock, (void *)&s.src, sizeof(s.src)); + if (ret < 0) + error(1, errno, "bind(%s), %i", libj1939_addr2str(&s.src), -errno); + } + + if (s.defined & DEF_DST) { + s.dst.can_family = AF_CAN; + ret = connect(sock, (void *)&s.dst, sizeof(s.dst)); + if (ret < 0) + error(1, errno, "connect(%s), %i", libj1939_addr2str(&s.dst), -errno); + } + + pfd[0].fd = STDIN_FILENO; + pfd[0].events = POLLIN; + pfd[1].fd = sock; + pfd[1].events = POLLIN; + + /* run */ + while (1) { + ret = poll(pfd, sizeof(pfd)/sizeof(pfd[0]), -1); + if (ret < 0) { + if (errno == EINTR) + continue; + error(1, errno, "poll()"); + } + if (pfd[0].revents) { + ret = read(pfd[0].fd, buf, s.pkt_len); + if (ret < 0) + error(1, errno, "read(stdin)"); + if (!ret) + break; + len = ret; + do { + ret = send(pfd[1].fd, buf, len, s.sendflags); + if (ret < 0) + error(errno != ENOBUFS, errno, "write(%s)", + libj1939_addr2str(&s.src)); + } while (ret < 0); + } + if (pfd[1].revents) { + ret = read(pfd[1].fd, buf, s.pkt_len); + if (ret < 0) { + ret = errno; + error(0, errno, "read(%s)", libj1939_addr2str(&s.dst)); + switch (ret) { + case EHOSTDOWN: + break; + default: + exit(1); + } + } else { + write(STDOUT_FILENO, buf, ret); + } + } + } + + free(buf); + return 0; +} + From 99e1d052363b31b8f87620b5d043346c64d933d7 Mon Sep 17 00:00:00 2001 From: Marc Kleine-Budde Date: Tue, 9 Jan 2018 14:51:47 +0100 Subject: [PATCH 08/10] testj1939: import from Kurt's repo Import from: 1ba47899048c Merge pull request #1 from linux-can/cross-compile-fixes Signed-off-by: Marc Kleine-Budde --- .gitignore | 1 + GNUmakefile.am | 3 +- Makefile | 2 +- testj1939.c | 290 +++++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 294 insertions(+), 2 deletions(-) create mode 100644 testj1939.c diff --git a/.gitignore b/.gitignore index 02274a2..6fd80d4 100644 --- a/.gitignore +++ b/.gitignore @@ -47,6 +47,7 @@ GNUmakefile.in /slcan_attach /slcand /slcanpty +/testj1939 /can-utils-*.tar.bz2 /can-utils-*.tar.gz diff --git a/GNUmakefile.am b/GNUmakefile.am index a75dad1..066a12d 100644 --- a/GNUmakefile.am +++ b/GNUmakefile.am @@ -62,7 +62,8 @@ bin_PROGRAMS = \ log2long \ slcan_attach \ slcand \ - slcanpty + slcanpty \ + testj1939 jspy_LDADD = libj1939.la jsr_LDADD = libj1939.la diff --git a/Makefile b/Makefile index b9a5182..743ba77 100644 --- a/Makefile +++ b/Makefile @@ -55,7 +55,7 @@ CPPFLAGS += -Iinclude \ PROGRAMS_ISOTP = isotpdump isotprecv isotpsend isotpsniffer isotptun isotpserver isotpperf PROGRAMS_CANGW = cangw PROGRAMS_SLCAN = slcan_attach slcand -PROGRAMS_J1939 = jspy jsr +PROGRAMS_J1939 = jspy jsr testj1939 PROGRAMS = can-calc-bit-timing candump cansniffer cansend canplayer cangen canbusload\ log2long log2asc asc2log\ canlogserver bcmserver\ diff --git a/testj1939.c b/testj1939.c new file mode 100644 index 0000000..e602477 --- /dev/null +++ b/testj1939.c @@ -0,0 +1,290 @@ +/* + * Copyright (c) 2013 EIA Electronics + * + * Authors: + * Kurt Van Dijck + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +static const char help_msg[] = + "testj1939: demonstrate j1939 use\n" + "Usage: testj1939 FROM TO\n" + " FROM / TO - or [IFACE][:[SA][,[PGN][,NAME]]]\n" + "Options:\n" + " -v Print relevant API calls\n" + " -s[=LEN] Initial send of LEN bytes dummy data\n" + " -r Receive (and print) data\n" + " -e Echo incoming packets back\n" + " This atually receives packets\n" + " -c Issue connect()\n" + " -p=PRIO Set priority to PRIO\n" + " -n Emit 64bit NAMEs in output\n" + " -w[TIME] Return after TIME (default 1) seconds\n" + "\n" + "Example:\n" + "testj1939 can1 20\n" + "\n" + ; + +static const char optstring[] = "?vs::rep:cnw::"; + +static void parse_canaddr(char *spec, struct sockaddr_can *paddr) +{ + char *str; + + str = strsep(&spec, ":"); + if (strlen(str)) + paddr->can_ifindex = if_nametoindex(str); + + str = strsep(&spec, ","); + if (str && strlen(str)) + paddr->can_addr.j1939.addr = strtoul(str, NULL, 0); + + str = strsep(&spec, ","); + if (str && strlen(str)) + paddr->can_addr.j1939.pgn = strtoul(str, NULL, 0); + + str = strsep(&spec, ","); + if (str && strlen(str)) + paddr->can_addr.j1939.name = strtoul(str, NULL, 0); +} + +static const char *canaddr2str(const struct sockaddr_can *paddr) +{ + static char buf[128]; + char *str = buf; + char ifname[IF_NAMESIZE]; + + if (paddr->can_ifindex) + str += sprintf(str, "%s", if_indextoname(paddr->can_ifindex, ifname)); + *str++ = ':'; + + if (paddr->can_addr.j1939.addr != J1939_NO_ADDR) + str += sprintf(str, "%02x", paddr->can_addr.j1939.addr); + *str++ = ','; + if (paddr->can_addr.j1939.pgn != J1939_NO_PGN) + str += sprintf(str, "%05x", paddr->can_addr.j1939.pgn); + *str++ = ','; + if (paddr->can_addr.j1939.name != J1939_NO_NAME) + str += sprintf(str, "%016llx", paddr->can_addr.j1939.name); + *str++ = 0; + return buf; +} + +static void onsigalrm(int sig) +{ + error(0, 0, "exit as requested"); + exit(0); +} + +static void schedule_oneshot_itimer(double delay) +{ + struct itimerval it = {}; + + it.it_value.tv_sec = delay; + it.it_value.tv_usec = (long)(delay * 1e6) % 1000000; + if (setitimer(ITIMER_REAL, &it, NULL) < 0) + error(1, errno, "schedule itimer %.3lfs", delay); +} + +/* main */ +int main(int argc, char *argv[]) +{ + int ret, sock, opt, j, verbose; + socklen_t peernamelen; + struct sockaddr_can sockname = { + .can_family = AF_CAN, + .can_addr.j1939 = { + .addr = J1939_NO_ADDR, + .name = J1939_NO_NAME, + .pgn = J1939_NO_PGN, + }, + }, peername = { + .can_family = AF_CAN, + .can_addr.j1939 = { + .addr = J1939_NO_ADDR, + .name = J1939_NO_NAME, + .pgn = J1939_NO_PGN, + }, + }; + uint8_t dat[128]; + int valid_peername = 0; + int todo_send = 0, todo_recv = 0, todo_echo = 0, todo_prio = -1; + int todo_connect = 0, todo_names = 0, todo_wait = 0; + + /* argument parsing */ + while ((opt = getopt(argc, argv, optstring)) != -1) + switch (opt) { + case 'v': + verbose = 1; + break; + case 's': + todo_send = strtoul(optarg ?: "8", NULL, 0); + break; + case 'r': + todo_recv = 1; + break; + case 'e': + todo_echo = 1; + break; + case 'p': + todo_prio = strtoul(optarg, NULL, 0); + break; + case 'c': + todo_connect = 1; + break; + case 'n': + todo_names = 1; + break; + case 'w': + schedule_oneshot_itimer(strtod(optarg ?: "1", NULL)); + signal(SIGALRM, onsigalrm); + todo_wait = 1; + break; + default: + fputs(help_msg, stderr); + exit(1); + break; + } + + if (argv[optind]) { + if (strcmp("-", argv[optind])) + parse_canaddr(argv[optind], &sockname); + ++optind; + } + + if (argv[optind]) { + if (strcmp("-", argv[optind])) { + parse_canaddr(argv[optind], &peername); + valid_peername = 1; + } + ++optind; + } + + /* open socket */ + if (verbose) + fprintf(stderr, "- socket(PF_CAN, SOCK_DGRAM, CAN_J1939);\n"); + sock = ret = socket(PF_CAN, SOCK_DGRAM, CAN_J1939); + if (ret < 0) + error(1, errno, "socket(j1939)"); + + if (todo_prio >= 0) { + if (verbose) + fprintf(stderr, "- setsockopt(, SOL_CAN_J1939, SO_J1939_SEND_PRIO, &%i);\n", todo_prio); + ret = setsockopt(sock, SOL_CAN_J1939, SO_J1939_SEND_PRIO, + &todo_prio, sizeof(todo_prio)); + if (ret < 0) + error(1, errno, "set priority %i", todo_prio); + } + + if (verbose) + fprintf(stderr, "- bind(, %s, %zi);\n", canaddr2str(&sockname), sizeof(sockname)); + ret = bind(sock, (void *)&sockname, sizeof(sockname)); + if (ret < 0) + error(1, errno, "bind()"); + + if (todo_connect) { + if (!valid_peername) + error(1, 0, "no peername supplied"); + if (verbose) + fprintf(stderr, "- connect(, %s, %zi);\n", canaddr2str(&peername), sizeof(peername)); + ret = connect(sock, (void *)&peername, sizeof(peername)); + if (ret < 0) + error(1, errno, "connect()"); + } + + if (todo_send) { + /* initialize test vector */ + for (j = 0; j < sizeof(dat); ++j) + dat[j] = ((2*j) << 4) + ((2*j+1) & 0xf); + + /* send data */ + /* + * when using connect, do not provide additional + * destination information and use send() + */ + if (valid_peername && !todo_connect) { + if (verbose) + fprintf(stderr, "- sendto(, , %i, 0, %s, %zi);\n", todo_send, canaddr2str(&peername), sizeof(peername)); + ret = sendto(sock, dat, todo_send, 0, + (void *)&peername, sizeof(peername)); + } else { + /* + * we may do sendto(sock, dat, todo_send, 0, NULL, 0) + * as well, but using send() demonstrates the API better + */ + if (verbose) + fprintf(stderr, "- send(, , %i, 0);\n", todo_send); + ret = send(sock, dat, todo_send, 0); + } + + if (ret < 0) + error(1, errno, "sendto"); + } + + /* main loop */ + if ((todo_echo || todo_recv) && verbose) + fprintf(stderr, "- while (1)\n"); + while (todo_echo || todo_recv) { + /* + * re-use peername for storing the sender's peername of + * received packets + */ + if (verbose) + fprintf(stderr, "- recvfrom(, , %zi, 0, &, %zi);\n", sizeof(peername), sizeof(peername)); + peernamelen = sizeof(peername); + ret = recvfrom(sock, dat, sizeof(dat), 0, + (void *)&peername, &peernamelen); + if (ret < 0) { + if (EINTR == errno) { + if (verbose) + fprintf(stderr, "-\t\n"); + continue; + } + error(1, errno, "recvfrom()"); + } + + if (todo_echo) { + if (verbose) + fprintf(stderr, "- sendto(, , %i, 0, %s, %i);\n", ret, canaddr2str(&peername), peernamelen); + ret = sendto(sock, dat, ret, 0, + (void *)&peername, peernamelen); + if (ret < 0) + error(1, errno, "sendto"); + } + if (todo_recv) { + if (todo_names && peername.can_addr.j1939.name) + printf("%016llx ", peername.can_addr.j1939.name); + printf("%02x %05x:", peername.can_addr.j1939.addr, + peername.can_addr.j1939.pgn); + for (j = 0; j < ret; ++j) + printf(" %02x", dat[j]); + printf("\n"); + } + } + if (todo_wait) + for (;;) + sleep(1); + return 0; +} + From 186656f960ed95a658ca2902525dc1a4223d7234 Mon Sep 17 00:00:00 2001 From: Kurt Van Dijck Date: Wed, 13 Mar 2013 20:46:56 +0100 Subject: [PATCH 09/10] can-j1939: add jacd address claim daemon The can-j1939 kernel part only follows & validates inbound & outbound address claim packets. Jacd implements the part that chooses an address and emits the address claim packet. Signed-off-by: Kurt Van Dijck Signed-off-by: Marc Kleine-Budde --- .gitignore | 1 + GNUmakefile.am | 2 + Makefile | 2 +- jacd.c | 626 +++++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 630 insertions(+), 1 deletion(-) create mode 100644 jacd.c diff --git a/.gitignore b/.gitignore index 6fd80d4..7f41b3d 100644 --- a/.gitignore +++ b/.gitignore @@ -40,6 +40,7 @@ GNUmakefile.in /isotpserver /isotpsniffer /isotptun +/jacd /jspy /jsr /log2asc diff --git a/GNUmakefile.am b/GNUmakefile.am index 066a12d..c48d543 100644 --- a/GNUmakefile.am +++ b/GNUmakefile.am @@ -56,6 +56,7 @@ bin_PROGRAMS = \ isotpserver \ isotpsniffer \ isotptun \ + jacd \ jspy \ jsr \ log2asc \ @@ -65,6 +66,7 @@ bin_PROGRAMS = \ slcanpty \ testj1939 +jacd_LDADD = libj1939.la jspy_LDADD = libj1939.la jsr_LDADD = libj1939.la diff --git a/Makefile b/Makefile index 743ba77..a63f72a 100644 --- a/Makefile +++ b/Makefile @@ -55,7 +55,7 @@ CPPFLAGS += -Iinclude \ PROGRAMS_ISOTP = isotpdump isotprecv isotpsend isotpsniffer isotptun isotpserver isotpperf PROGRAMS_CANGW = cangw PROGRAMS_SLCAN = slcan_attach slcand -PROGRAMS_J1939 = jspy jsr testj1939 +PROGRAMS_J1939 = jacd jspy jsr testj1939 PROGRAMS = can-calc-bit-timing candump cansniffer cansend canplayer cangen canbusload\ log2long log2asc asc2log\ canlogserver bcmserver\ diff --git a/jacd.c b/jacd.c new file mode 100644 index 0000000..1358918 --- /dev/null +++ b/jacd.c @@ -0,0 +1,626 @@ +/* + * Copyright (c) 2011 EIA Electronics + * + * Authors: + * Kurt Van Dijck + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +static const char help_msg[] = + "jacd: An SAE J1939 address claiming daemon" "\n" + "Usage: jacd [options] NAME [INTF]" "\n" + "\n" + " -v, --verbose Increase verbosity" "\n" + " -r, --range=RANGE Ranges of source addresses" "\n" + " e.g. 80,50-100,200-210 (defaults to 0-253)" "\n" + " -c, --cache=FILE Cache file to save/restore the source address" "\n" + " -a, --address=ADDRESS Start with Source Address ADDRESS" "\n" + " -p, --prefix=STR Prefix to use when logging" "\n" + "\n" + "NAME is the 64bit nodename" "\n" + "\n" + "Example:" "\n" + "jacd -r 100,80-120 -c /tmp/1122334455667788.jacd 1122334455667788" "\n" + ; + +#ifdef _GNU_SOURCE +static struct option long_opts[] = { + { "help", no_argument, NULL, '?', }, + { "verbose", no_argument, NULL, 'v', }, + { "range", required_argument, NULL, 'r', }, + { "cache", required_argument, NULL, 'c', }, + { "address", required_argument, NULL, 'a', }, + { "prefix", required_argument, NULL, 'p', }, + { }, +}; +#else +#define getopt_long(argc, argv, optstring, longopts, longindex) \ + getopt((argc), (argv), (optstring)) +#endif +static const char optstring[] = "vr:c:a:p:?"; + +/* byte swap functions */ +static inline int host_is_little_endian(void) +{ + static const uint16_t endian_test = 1; + return *(const uint8_t *)&endian_test; +} + +static __attribute__((unused)) void bswap(void *vptr, int size) +{ + uint8_t *p0, *pe; + uint8_t tmp; + + p0 = vptr; + pe = &p0[size-1]; + for (; p0 < pe; ++p0, --pe) { + tmp = *p0; + *p0 = *pe; + *pe = tmp; + } +} + +/* rate-limiting for errors */ +static inline int must_warn(int ret) +{ + if (ret >= 0) + return 0; + switch (errno) { + case EINTR: + case ENOBUFS: + return 0; + } + return 1; +} + +/* global variables */ +static char default_range[] = "0x80-0xfd"; +static const char default_intf[] = "can0"; + +static struct { + int verbose; + const char *cachefile; + + const char *intf; + char *ranges; + uint64_t name; + uint8_t current_sa; + uint8_t last_sa; + int sig_term; + int sig_alrm; + int sig_usr1; + int state; + #define STATE_INITIAL 0 + #define STATE_REQ_SENT 1 + #define STATE_REQ_PENDING 2 /* wait 1250 msec for first claim */ + #define STATE_OPERATIONAL 3 +} s = { + .intf = default_intf, + .ranges = default_range, + .current_sa = J1939_IDLE_ADDR, + .last_sa = J1939_NO_ADDR, +}; + +struct { + uint64_t name; + int flags; + #define F_USE 0x01 + #define F_SEEN 0x02 +} addr[J1939_IDLE_ADDR /* =254 */]; + +/* lookup by name */ +static int lookup_name(uint64_t name) +{ + int j; + + for (j = 0; j < J1939_IDLE_ADDR; ++j) { + if (addr[j].name == name) + return j; + } + return J1939_IDLE_ADDR; + +} + +/* parse address range */ +static int parse_range(char *str) +{ + char *tok, *endp; + int a0, ae; + int j, cnt; + + cnt = 0; + for (tok = strtok(str, ",;"); tok; tok = strtok(NULL, ",;")) { + a0 = ae = strtoul(tok, &endp, 0); + if (endp <= tok) + error(1, 0, "parsing range '%s'", tok); + if (*endp == '-') { + tok = endp+1; + ae = strtoul(tok, &endp, 0); + if (endp <= tok) + error(1, 0, "parsing addr '%s'", tok); + if (ae < a0) + ae = a0; + } + for (j = a0; j <= ae; ++j, ++cnt) { + if (j == J1939_IDLE_ADDR) + break; + addr[j].flags |= F_USE; + } + } + return cnt; +} + +/* j1939 socket */ +static const struct j1939_filter filt[] = { + { + .pgn = 0x0ee00, + .pgn_mask = 0x3ff00, + }, { + .pgn = 0x0ea00, + .pgn_mask = 0x3ff00, + }, { + .pgn = 0x0fed8, + .pgn_mask = 0x3ffff, + }, +}; + +static int open_socket(const char *device, uint64_t name) +{ + int ret, sock; + int value; + struct sockaddr_can saddr = { + .can_family = AF_CAN, + .can_addr.j1939 = { + .name = name, + .addr = J1939_IDLE_ADDR, + .pgn = 0x0ee00, + }, + }; + + sock = ret = socket(PF_CAN, SOCK_DGRAM, CAN_J1939); + if (ret < 0) + error(1, errno, "socket(j1939)"); + + ret = setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, + device, strlen(device)); + if (ret < 0) + error(1, errno, "bindtodevice %s", device); + + ret = setsockopt(sock, SOL_CAN_J1939, SO_J1939_FILTER, + &filt, sizeof(filt)); + if (ret < 0) + error(1, errno, "setsockopt filter"); + + value = 1; + ret = setsockopt(sock, SOL_CAN_J1939, SO_J1939_RECV_OWN, + &value, sizeof(value)); + if (ret < 0) + error(1, errno, "setsockopt receive own msgs"); + + ret = bind(sock, (void *)&saddr, sizeof(saddr)); + if (ret < 0) + error(1, errno, "bind()"); + return sock; +} + +/* real IO function */ +static int repeat_address(int sock, uint64_t name) +{ + int ret; + uint8_t dat[8]; + + memcpy(dat, &name, 8); + if (!host_is_little_endian()) + bswap(dat, 8); + ret = send(sock, dat, 8, 0); + if (must_warn(ret)) + error(1, errno, "send address claim for 0x%02x", s.last_sa); + return ret; +} +static int claim_address(int sock, uint64_t name, int sa) +{ + int ret; + struct sockaddr_can saddr = { + .can_family = AF_CAN, + .can_addr.j1939 = { + .name = name, + .addr = sa, + .pgn = 0x0ee00, + }, + }; + + ret = bind(sock, (void *)&saddr, sizeof(saddr)); + if (ret < 0) + error(1, errno, "rebind with sa 0x%02x", sa); + s.last_sa = sa; + return repeat_address(sock, name); +} + +static int request_addresses(int sock) +{ + static const uint8_t dat[3] = { 0, 0xee, 0, }; + int ret; + static const struct sockaddr_can saddr = { + .can_family = AF_CAN, + .can_addr.j1939.pgn = 0x0ea00, + .can_addr.j1939.addr = J1939_NO_ADDR, + }; + + ret = sendto(sock, dat, sizeof(dat), 0, (void *)&saddr, sizeof(saddr)); + if (must_warn(ret)) + error(1, errno, "send request for address claims"); + return ret; +} + +/* real policy */ +static int choose_new_sa(uint64_t name, int sa) +{ + int j, cnt; + + /* test current entry */ + if ((sa < J1939_IDLE_ADDR) && (addr[sa].flags & F_USE)) { + j = sa; + if (!addr[j].name || (addr[j].name == name) || (addr[j].name > name)) + return j; + } + /* take first empty spot */ + for (j = 0; j < J1939_IDLE_ADDR; ++j) { + if (!(addr[j].flags & F_USE)) + continue; + if (!addr[j].name || (addr[j].name == name)) + return j; + } + + /* + * no empty spot found + * take next (relative to @sa) spot that we can + * successfully contest + */ + j = sa + 1; + for (cnt = 0; cnt < J1939_IDLE_ADDR; ++j, ++cnt) { + if (j >= J1939_IDLE_ADDR) + j = 0; + if (!(addr[j].flags & F_USE)) + continue; + if (name < addr[j].name) + return j; + } + return J1939_IDLE_ADDR; +} + +/* signa handling */ +static void sighandler(int sig, siginfo_t *info, void *vp) +{ + switch (sig) { + case SIGINT: + case SIGTERM: + s.sig_term = 1; + break; + case SIGALRM: + s.sig_alrm = 1; + break; + case SIGUSR1: + s.sig_usr1 = 1; + break; + } +} + +static void install_signal(int sig) +{ + int ret; + struct sigaction sigact = { + .sa_sigaction = sighandler, + .sa_flags = SA_SIGINFO, + }; + + sigfillset(&sigact.sa_mask); + ret = sigaction(sig, &sigact, NULL); + if (ret < 0) + error(1, errno, "sigaction for signal %i", sig); +} + +static void schedule_itimer(int msec) +{ + int ret; + struct itimerval val = {}; + + val.it_value.tv_sec = msec / 1000; + val.it_value.tv_usec = (msec % 1000) * 1000; + + s.sig_alrm = 0; + do { + ret = setitimer(ITIMER_REAL, &val, NULL); + } while ((ret < 0) && (errno == EINTR)); + if (ret < 0) + error(1, errno, "setitimer %i msec", msec); +} + +/* dump status */ +static inline int addr_status_mine(int sa) +{ + if (sa == s.current_sa) + return '*'; + else if (addr[sa].flags & F_USE) + return '+'; + else + return '-'; +} + +static void dump_status(void) +{ + int j; + + for (j = 0; j < J1939_IDLE_ADDR; ++j) { + if (!addr[j].flags && !addr[j].name) + continue; + fprintf(stdout, "%02x: %c", j, addr_status_mine(j)); + if (addr[j].name) + fprintf(stdout, " %016llx", (long long)addr[j].name); + else + fprintf(stdout, " -"); + fprintf(stdout, "\n"); + } + fflush(stdout); +} + +/* cache file */ +static void save_cache(void) +{ + FILE *fp; + time_t t; + + if (!s.cachefile) + return; + fp = fopen(s.cachefile, "w"); + if (!fp) + error(1, errno, "fopen %s, w", s.cachefile); + + time(&t); + fprintf(fp, "# saved on %s\n", ctime(&t)); + fprintf(fp, "\n"); + fprintf(fp, "0x%02x\n", s.current_sa); + fclose(fp); +} + +static void restore_cache(void) +{ + FILE *fp; + int ret; + char *endp; + char *line = 0; + size_t sz = 0; + + if (!s.cachefile) + return; + fp = fopen(s.cachefile, "r"); + if (!fp) { + if (ENOENT == errno) + return; + error(1, errno, "fopen %s, r", s.cachefile); + } + while (!feof(fp)) { + ret = getline(&line, &sz, fp); + if (ret <= 0) + continue; + if (line[0] == '#') + continue; + ret = strtoul(line, &endp, 0); + if ((endp > line) && (ret >= 0) && (ret <= J1939_IDLE_ADDR)) { + s.current_sa = ret; + break; + } + } + fclose(fp); + if (line) + free(line); +} + +/* main */ +int main(int argc, char *argv[]) +{ + int ret, sock, pgn, sa, opt; + socklen_t slen; + uint8_t dat[9]; + struct sockaddr_can saddr; + uint64_t cmd_name; + +#ifdef _GNU_SOURCE + program_invocation_name = program_invocation_short_name; +#endif + /* argument parsing */ + while ((opt = getopt_long(argc, argv, optstring, long_opts, NULL)) != -1) + switch (opt) { + case 'v': + ++s.verbose; + break; + case 'c': + s.cachefile = optarg; + break; + case 'r': + s.ranges = optarg; + break; + case 'a': + s.current_sa = strtoul(optarg, 0, 0); + break; + case 'p': +#ifdef _GNU_SOURCE + asprintf(&program_invocation_name, "%s.%s", program_invocation_short_name, optarg); +#else + error(0, 0, "compile with -D_GNU_SOURCE to use -p"); +#endif + break; + default: + fputs(help_msg, stderr); + exit(1); + break; + } + if (argv[optind]) + s.name = strtoull(argv[optind++], 0, 16); + if (argv[optind]) + s.intf = argv[optind++]; + + /* args done */ + + restore_cache(); + + ret = parse_range(s.ranges); + if (!ret) + error(1, 0, "no addresses in range"); + + if ((s.current_sa < J1939_IDLE_ADDR) && !(addr[s.current_sa].flags & F_USE)) { + if (s.verbose) + error(0, 0, "forget saved address 0x%02x", s.current_sa); + s.current_sa = J1939_IDLE_ADDR; + } + + if (s.verbose) + error(0, 0, "ready for %s:%016llx", s.intf, (long long)s.name); + if (!s.intf || !s.name) + error(1, 0, "bad arguments"); + ret = sock = open_socket(s.intf, s.name); + + install_signal(SIGTERM); + install_signal(SIGINT); + install_signal(SIGALRM); + install_signal(SIGUSR1); + install_signal(SIGUSR2); + + while (!s.sig_term) { + if (s.sig_usr1) { + s.sig_usr1 = 0; + dump_status(); + } + switch (s.state) { + case STATE_INITIAL: + ret = request_addresses(sock); + if (ret < 0) + error(1, errno, "could not sent initial request"); + s.state = STATE_REQ_SENT; + break; + case STATE_REQ_PENDING: + if (!s.sig_alrm) + break; + s.sig_alrm = 0; + /* claim addr */ + sa = choose_new_sa(s.name, s.current_sa); + if (sa == J1939_IDLE_ADDR) + error(1, 0, "no free address to use"); + ret = claim_address(sock, s.name, sa); + if (ret < 0) + schedule_itimer(50); + s.state = STATE_OPERATIONAL; + break; + case STATE_OPERATIONAL: + if (s.sig_alrm) { + s.sig_alrm = 0; + ret = repeat_address(sock, s.name); + if (ret < 0) + schedule_itimer(50); + } + break; + } + + slen = sizeof(saddr); + ret = recvfrom(sock, dat, sizeof(dat), 0, (void *)&saddr, &slen); + if (ret < 0) { + if (EINTR == errno) + continue; + error(1, errno, "recvfrom()"); + } + switch (saddr.can_addr.j1939.pgn) { + case 0x0ea00: + if (ret < 3) + break; + pgn = dat[0] + (dat[1] << 8) + ((dat[2] & 0x03) << 16); + if (pgn != 0x0ee00) + /* not interested */ + break; + if (s.state == STATE_REQ_SENT) { + if (s.verbose) + error(0, 0, "request sent, pending for 1250 ms"); + schedule_itimer(1250); + s.state = STATE_REQ_PENDING; + } else if (s.state == STATE_OPERATIONAL) { + ret = claim_address(sock, s.name, s.current_sa); + if (ret < 0) + schedule_itimer(50); + } + break; + case 0x0ee00: + if (saddr.can_addr.j1939.addr >= J1939_IDLE_ADDR) { + sa = lookup_name(saddr.can_addr.j1939.name); + if (sa < J1939_IDLE_ADDR) + addr[sa].name = 0; + break; + } + sa = lookup_name(saddr.can_addr.j1939.name); + if ((sa != saddr.can_addr.j1939.addr) && (sa < J1939_IDLE_ADDR)) + /* update cache */ + addr[sa].name = 0; + + /* shortcut */ + sa = saddr.can_addr.j1939.addr; + addr[sa].name = saddr.can_addr.j1939.name; + addr[sa].flags |= F_SEEN; + + if (s.name == saddr.can_addr.j1939.name) { + /* ourselve, disable itimer */ + s.current_sa = sa; + if (s.verbose) + error(0, 0, "claimed 0x%02x", sa); + } else if (sa == s.current_sa) { + if (s.verbose) + error(0, 0, "address collision for 0x%02x", sa); + if (s.name > saddr.can_addr.j1939.name) { + sa = choose_new_sa(s.name, sa); + if (sa == J1939_IDLE_ADDR) { + error(0, 0, "no address left"); + /* put J1939_IDLE_ADDR in cache file */ + s.current_sa = sa; + goto done; + } + } + ret = claim_address(sock, s.name, sa); + if (ret < 0) + schedule_itimer(50); + } + break; + case 0x0fed8: + if (!host_is_little_endian()) + bswap(dat, 8); + memcpy(&cmd_name, dat, 8); + if (cmd_name == s.name) { + ret = claim_address(sock, s.name, dat[8]); + if (ret < 0) + schedule_itimer(50); + } + break; + } + } +done: + if (s.verbose) + error(0, 0, "shutdown"); + claim_address(sock, s.name, J1939_IDLE_ADDR); + save_cache(); + return 0; +} + From 206906ccd38e304d985ab6942d8053dddaf69168 Mon Sep 17 00:00:00 2001 From: Bastian Stender Date: Thu, 11 Jan 2018 15:44:44 +0100 Subject: [PATCH 10/10] testj1939: initialize verbose variable Signed-off-by: Bastian Stender --- testj1939.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/testj1939.c b/testj1939.c index e602477..d76a52d 100644 --- a/testj1939.c +++ b/testj1939.c @@ -110,7 +110,8 @@ static void schedule_oneshot_itimer(double delay) /* main */ int main(int argc, char *argv[]) { - int ret, sock, opt, j, verbose; + int ret, sock, opt, j; + int verbose = 0; socklen_t peernamelen; struct sockaddr_can sockname = { .can_family = AF_CAN,