diff --git a/.gitignore b/.gitignore index 02274a2..b51f1ca 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 a75dad1..c49dd3d 100644 --- a/GNUmakefile.am +++ b/GNUmakefile.am @@ -56,6 +56,7 @@ bin_PROGRAMS = \ isotpserver \ isotpsniffer \ isotptun \ + jacd \ jspy \ jsr \ log2asc \ @@ -64,6 +65,7 @@ bin_PROGRAMS = \ slcand \ slcanpty +jacd_LDADD = libj1939.la jspy_LDADD = libj1939.la jsr_LDADD = libj1939.la diff --git a/Makefile b/Makefile index b9a5182..b8853df 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 = jacd jspy jsr 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; +} +