On Thu, Apr 28, 2011 at 09:45:30AM +0200, Marc Kleine-Budde wrote: > > +LDADD += libj1939.la > > It would be cleaner if you just link your j1939 programs with the lib: > > jacd_LDADD = libj1939.la > jsr_LDADD = libj1939.la > jspy_LDADD = libj1939.la > > (I know we link everything against libcan.la)
you're right. Thanks for the example automake syntax! --- This patch adds SAE J1939 tools & libraries to can-utils * jacd: a J1939 address claiming daemon * jspy: spy on a J1939 bus * jsr: send/receive J1939 packets * libj1939.a: conversion to/from struct sockaddr_can to string Changes with v1: * update for autotools Signed-off-by: Kurt Van Dijck <[email protected]> --- Index: can-utils/jsr.c =================================================================== --- can-utils/jsr.c (revision 0) +++ can-utils/jsr.c (revision 0) @@ -0,0 +1,229 @@ +/* + * Copyright (c) 2011 EIA Electronics + * + * Authors: + * Kurt Van Dijck <[email protected]> + * + * 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 <string.h> +#include <stdlib.h> +#include <stdio.h> +#include <errno.h> +#include <inttypes.h> + +#include <unistd.h> +#include <getopt.h> +#include <error.h> +#include <poll.h> +#include <sys/types.h> +#include <sys/socket.h> +#include <sys/ioctl.h> +#include <sys/stat.h> + +//#include <net/if.h> + +#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() */ + long 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, + }, +}; + +static uint8_t *buf; + +int main(int argc, char** argv) +{ + int ret, sock, opt; + unsigned int len, done; + struct pollfd pfd[2]; + +#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; + } + + /* prepare */ + buf = malloc(s.pkt_len); + if (!buf) + error(1, errno, "malloc %lu", 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, 2, -1); + if (ret < 0) + 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) { + //error(0, 0, "stdin EOF"); + break; + } + len = ret; + for (done = 0; done < len; ) { + ret = send(pfd[1].fd, buf, len, s.sendflags); + if (ret < 0) { + error(0, errno, "write(%s)", libj1939_addr2str(&s.src)); + if (ENOBUFS == errno) { + sleep(1); + continue; + } + exit(1); + } + done += ret; + } + } + if (pfd[1].revents) { + ret = read(pfd[1].fd, buf, sizeof(buf)); + 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; +} +//----------------------------------------------------------------------------- + Property changes on: can-utils/jsr.c ___________________________________________________________________ Name: svn:eol-style + native Index: can-utils/jacd.c =================================================================== --- can-utils/jacd.c (revision 0) +++ can-utils/jacd.c (revision 0) @@ -0,0 +1,620 @@ +/* + * Copyright (c) 2011 EIA Electronics + * + * Authors: + * Kurt Van Dijck <[email protected]> + * + * 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 <signal.h> +#include <time.h> +#include <inttypes.h> +#include <errno.h> +#include <string.h> +#include <stdlib.h> +#include <stdio.h> + +#include <unistd.h> +#include <getopt.h> +#include <error.h> +#include <sys/time.h> +#include <sys/socket.h> +#include <linux/can.h> +#include <linux/can/j1939.h> + +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; + struct sockaddr_can saddr; + int value; + + 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"); + + memset(&saddr, 0, sizeof(saddr)); + saddr.can_family = AF_CAN; + saddr.can_addr.j1939.name = name; + saddr.can_addr.j1939.addr = J1939_IDLE_ADDR; + saddr.can_addr.j1939.pgn = 0x0ee00; + + 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; + + memset(&saddr, 0, sizeof(saddr)); + saddr.can_family = AF_CAN; + saddr.can_addr.j1939.name = name; + saddr.can_addr.j1939.addr = sa; + saddr.can_addr.j1939.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; +} + Property changes on: can-utils/jacd.c ___________________________________________________________________ Name: svn:eol-style + native Index: can-utils/GNUmakefile.am =================================================================== --- can-utils/GNUmakefile.am (revision 1240) +++ can-utils/GNUmakefile.am (working copy) @@ -52,6 +52,21 @@ isotptun endif +if CONFIG_J1939 +bin_PROGRAMS += \ + jacd \ + jsr \ + jspy + +jacd_LDADD = libj1939.la +jsr_LDADD = libj1939.la +jspy_LDADD = libj1939.la + +noinst_LTLIBRARIES += libj1939.la + +libj1939_la_SOURCES = libj1939.c +endif + EXTRA_DIST = \ autogen.sh Index: can-utils/libj1939.c =================================================================== --- can-utils/libj1939.c (revision 0) +++ can-utils/libj1939.c (revision 0) @@ -0,0 +1,201 @@ +/* + * Copyright (c) 2011 EIA Electronics + * + * Authors: + * Kurt Van Dijck <[email protected]> + * + * 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 <string.h> +#include <stdio.h> +#include <stdlib.h> +#include <errno.h> +#include <inttypes.h> + +#include <error.h> +#include <unistd.h> +#include <fcntl.h> +#include <net/if.h> +#include <sys/ioctl.h> + +#include "libj1939.h" +//----------------------------------------------------------------------------- +struct ifname { + struct ifname *next;; + int ifindex; + char name[2]; +}; +//----------------------------------------------------------------------------- +static struct { + int sock; + struct ifname *names; +} s = { + .sock = -1, +}; +//----------------------------------------------------------------------------- +__attribute__((destructor)) void libj1939_cleanup(void) +{ + struct ifname *nam; + + if (s.sock >= 0) + close(s.sock); + s.sock = -1; + + while (s.names) { + nam = s.names; + s.names = nam->next; + free(nam); + } +} +//----------------------------------------------------------------------------- +static void verify_sock(void) +{ + if (s.sock >= 0) + return; + s.sock = socket(PF_CAN, SOCK_DGRAM, CAN_J1939); + if (s.sock < 0) + error(1, errno, "sock(can, dgram, j1939)"); + fcntl(s.sock, F_SETFD, fcntl(s.sock, F_GETFD) | FD_CLOEXEC); +} +//----------------------------------------------------------------------------- +static struct ifname *libj1939_add_ifnam(int ifindex, const char *str) +{ + struct ifname *nam; + nam = malloc(sizeof(*nam) + strlen(str)); + memset(nam, 0, sizeof(*nam)); + nam->ifindex = ifindex; + strcpy(nam->name, str); + nam->next = s.names; + s.names = nam; + return nam; +} +//----------------------------------------------------------------------------- +const char *libj1939_ifnam(int ifindex) +{ + int ret; + + const struct ifname *lp; + struct ifname *nam; + struct ifreq ifr; + + for (lp = s.names; lp; lp = lp->next) { + if (lp->ifindex == ifindex) + return lp->name; + } + /* find out this new ifindex */ + verify_sock(); + ifr.ifr_ifindex = ifindex; + ret = ioctl(s.sock, SIOCGIFNAME, &ifr); + if (ret < 0) + error(1, errno, "get ifname(%u)", ifindex); + nam = libj1939_add_ifnam(ifindex, ifr.ifr_name); + return nam ? nam->name : 0; +} +//----------------------------------------------------------------------------- +int libj1939_ifindex(const char *str) +{ + const struct ifname *lp; + struct ifname *nam; + char *endp; + int ret; + struct ifreq ifr; + + ret = strtol(str, &endp, 0); + if (!*endp) + // did some good parse + return ret; + + for (lp = s.names; lp; lp = lp->next) { + if (!strcmp(lp->name, str)) + return lp->ifindex; + } + /* find out this new ifindex */ + verify_sock(); + strncpy(ifr.ifr_name, str, sizeof(ifr.ifr_name)); + ret = ioctl(s.sock, SIOCGIFINDEX, &ifr); + if (ret < 0) + error(1, errno, "get ifindex(%s)", str); + nam = libj1939_add_ifnam(ifr.ifr_ifindex, str); + return nam ? nam->ifindex : 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_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); + } + 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; +} +//----------------------------------------------------------------------------- + Property changes on: can-utils/libj1939.c ___________________________________________________________________ Name: svn:eol-style + native Index: can-utils/libj1939.h =================================================================== --- can-utils/libj1939.h (revision 0) +++ can-utils/libj1939.h (revision 0) @@ -0,0 +1,28 @@ +#include <sys/socket.h> +#include <linux/can.h> +#include <linux/can/j1939.h> + +#ifndef J1939_LIB_H +#define J1939_LIB_H + +#ifdef __cplusplus +extern "C" { +#endif + +extern int libj1939_ifindex(const char *ifname); +extern const char *libj1939_ifnam(int ifindex); +/* + * cleanup held resources + * this is a __attribute__((destructor)), so not calling this + * is ok too. + */ +extern void libj1939_cleanup(void); + +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 Property changes on: can-utils/libj1939.h ___________________________________________________________________ Name: svn:eol-style + native Index: can-utils/configure.ac =================================================================== --- can-utils/configure.ac (revision 1240) +++ can-utils/configure.ac (working copy) @@ -108,6 +108,7 @@ AC_CHECK_HEADERS([ \ linux/can/gw.h \ linux/can/isotp.h \ + linux/can/j1939.h \ ],[],[], [ #ifdef HAVE_SYS_SOCKET_H @@ -117,6 +118,7 @@ AM_CONDITIONAL(CONFIG_GW, [test "${ac_cv_header_linux_can_gw_h}" = "yes"]) AM_CONDITIONAL(CONFIG_ISOTP, [test "${ac_cv_header_linux_can_isotp_h}" = "yes"]) +AM_CONDITIONAL(CONFIG_J1939, [test "${ac_cv_header_linux_can_j1939_h}" = "yes"]) AC_CHECK_DECL(SO_RXQ_OVFL,, Index: can-utils/jspy.c =================================================================== --- can-utils/jspy.c (revision 0) +++ can-utils/jspy.c (revision 0) @@ -0,0 +1,297 @@ +/* + * Copyright (c) 2011 EIA Electronics + * + * Authors: + * Kurt Van Dijck <[email protected]> + * + * 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 <string.h> +#include <time.h> +#include <stdlib.h> +#include <stdio.h> +#include <errno.h> +#include <inttypes.h> + +#include <unistd.h> +#include <getopt.h> +#include <error.h> +#include <sys/socket.h> +#include <sys/ioctl.h> +#include <sys/time.h> + +#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_0 = 0; +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]) { + 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); + + // parse args + 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) { + error(0, errno, "recvmsg(ifindex %i)", s.addr.can_ifindex); + if (ENETDOWN == errno) + continue; + exit(1); + } + 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; +} + Property changes on: can-utils/jspy.c ___________________________________________________________________ Name: svn:eol-style + native Index: can-utils/Makefile =================================================================== --- can-utils/Makefile (revision 1240) +++ can-utils/Makefile (working copy) @@ -56,26 +56,36 @@ PROGRAMS_ISOTP = isotpdump isotprecv isotpsend isotpsniffer isotptun isotpserver PROGRAMS_CANGW = cangw PROGRAMS_SLCAN = slcan_attach slcand +PROGRAMS_J1939 = jacd jspy jsr 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) +LIBRARIES = libj1939.a +all: $(PROGRAMS) $(LIBRARIES) + clean: rm -f $(PROGRAMS) *.o install: mkdir -p $(DESTDIR)$(PREFIX)/bin cp -f $(PROGRAMS) $(DESTDIR)$(PREFIX)/bin + mkdir -p $(DESTDIR)$(PREFIX)/lib + cp -f $(LIBRARIES) $(DESTDIR)$(PREFIX)/lib + distclean: - rm -f $(PROGRAMS) *.o *~ + rm -f $(PROGRAMS) $(LIBRARIES) *.o *~ +libj1939.a: libj1939.o + ar crs $@ $< + cansend.o: lib.h cangen.o: lib.h candump.o: lib.h @@ -94,3 +104,7 @@ log2long: log2long.o lib.o log2asc: log2asc.o lib.o asc2log: asc2log.o lib.o + +jspy: libj1939.a +jsr: libj1939.a + _______________________________________________ Socketcan-core mailing list [email protected] https://lists.berlios.de/mailman/listinfo/socketcan-core
