git commit -m "first commit for v2"
This commit is contained in:
781
Devices/Libraries/Systems/can-utils/j1939cat.c
Executable file
781
Devices/Libraries/Systems/can-utils/j1939cat.c
Executable file
@@ -0,0 +1,781 @@
|
||||
/* SPDX-License-Identifier: GPL-2.0-only */
|
||||
/*
|
||||
* Copyright (c) 2018 Pengutronix, Oleksij Rempel <o.rempel@pengutronix.de>
|
||||
*/
|
||||
|
||||
#include <err.h>
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <inttypes.h>
|
||||
#include <net/if.h>
|
||||
#include <poll.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <linux/errqueue.h>
|
||||
#include <linux/net_tstamp.h>
|
||||
#include <linux/netlink.h>
|
||||
#include <linux/socket.h>
|
||||
|
||||
#include "libj1939.h"
|
||||
|
||||
#define J1939_MAX_ETP_PACKET_SIZE (7 * 0x00ffffff)
|
||||
#define JCAT_BUF_SIZE (1000 * 1024)
|
||||
|
||||
/*
|
||||
* min()/max()/clamp() macros that also do
|
||||
* strict type-checking.. See the
|
||||
* "unnecessary" pointer comparison.
|
||||
*/
|
||||
#define min(x, y) ({ \
|
||||
typeof(x) _min1 = (x); \
|
||||
typeof(y) _min2 = (y); \
|
||||
(void) (&_min1 == &_min2); \
|
||||
_min1 < _min2 ? _min1 : _min2; })
|
||||
|
||||
|
||||
struct j1939cat_stats {
|
||||
int err;
|
||||
uint32_t tskey;
|
||||
uint32_t send;
|
||||
uint32_t total;
|
||||
uint32_t pgn;
|
||||
uint8_t sa;
|
||||
uint8_t da;
|
||||
uint64_t src_name;
|
||||
uint64_t dst_name;
|
||||
};
|
||||
|
||||
struct j1939cat_priv {
|
||||
int sock;
|
||||
int infile;
|
||||
int outfile;
|
||||
size_t max_transfer;
|
||||
unsigned long repeat;
|
||||
unsigned long round;
|
||||
int todo_prio;
|
||||
|
||||
bool valid_peername;
|
||||
bool todo_recv;
|
||||
bool todo_filesize;
|
||||
bool todo_connect;
|
||||
int todo_broadcast;
|
||||
|
||||
unsigned long polltimeout;
|
||||
|
||||
struct sockaddr_can sockname;
|
||||
struct sockaddr_can peername;
|
||||
|
||||
struct sock_extended_err *serr;
|
||||
struct scm_timestamping *tss;
|
||||
struct j1939cat_stats stats;
|
||||
int32_t last_dpo;
|
||||
};
|
||||
|
||||
static const char help_msg[] =
|
||||
"j1939cat: netcat-like tool for j1939\n"
|
||||
"Usage: j1939cat [options] FROM TO\n"
|
||||
" FROM / TO - or [IFACE][:[SA][,[PGN][,NAME]]]\n"
|
||||
"Options:\n"
|
||||
" -i <infile> (default stdin)\n"
|
||||
" -s <size> Set maximal transfer size. Default: 117440505 byte\n"
|
||||
" -r Receive data\n"
|
||||
" -P <timeout> poll timeout in milliseconds before sending data.\n"
|
||||
" With this option send() will be used with MSG_DONTWAIT flag.\n"
|
||||
" -R <count> Set send repeat count. Default: 1\n"
|
||||
" -B Allow to send and receive broadcast packets.\n"
|
||||
"\n"
|
||||
"Example:\n"
|
||||
"j1939cat -i some_file_to_send can0:0x80 :0x90,0x12300\n"
|
||||
"j1939cat can0:0x90 -r > /tmp/some_file_to_receive\n"
|
||||
"\n"
|
||||
;
|
||||
|
||||
static const char optstring[] = "?hi:vs:rp:P:R:B";
|
||||
|
||||
static ssize_t j1939cat_send_one(struct j1939cat_priv *priv, int out_fd,
|
||||
const void *buf, size_t buf_size)
|
||||
{
|
||||
ssize_t num_sent;
|
||||
int flags = 0;
|
||||
|
||||
if (priv->polltimeout)
|
||||
flags |= MSG_DONTWAIT;
|
||||
|
||||
if (priv->valid_peername && !priv->todo_connect)
|
||||
num_sent = sendto(out_fd, buf, buf_size, flags,
|
||||
(struct sockaddr *)&priv->peername,
|
||||
sizeof(priv->peername));
|
||||
else
|
||||
num_sent = send(out_fd, buf, buf_size, flags);
|
||||
|
||||
if (num_sent == -1) {
|
||||
warn("%s: transfer error: %i", __func__, -errno);
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if (num_sent == 0) /* Should never happen */ {
|
||||
warn("%s: transferred 0 bytes", __func__);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
if (num_sent > (ssize_t)buf_size) /* Should never happen */ {
|
||||
warn("%s: send more then read", __func__);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
return num_sent;
|
||||
}
|
||||
|
||||
static void j1939cat_print_timestamp(struct j1939cat_priv *priv, const char *name,
|
||||
struct timespec *cur)
|
||||
{
|
||||
struct j1939cat_stats *stats = &priv->stats;
|
||||
|
||||
if (!(cur->tv_sec | cur->tv_nsec))
|
||||
return;
|
||||
|
||||
fprintf(stderr, " %s: %llu s %llu us (seq=%03u, send=%07u)",
|
||||
name, (unsigned long long)cur->tv_sec, (unsigned long long)cur->tv_nsec / 1000,
|
||||
stats->tskey, stats->send);
|
||||
|
||||
fprintf(stderr, "\n");
|
||||
}
|
||||
|
||||
static const char *j1939cat_tstype_to_str(int tstype)
|
||||
{
|
||||
switch (tstype) {
|
||||
case SCM_TSTAMP_SCHED:
|
||||
return "TX ENQ";
|
||||
case SCM_TSTAMP_SND:
|
||||
return "TX SND";
|
||||
case SCM_TSTAMP_ACK:
|
||||
return "TX ACK";
|
||||
default:
|
||||
return " unk";
|
||||
}
|
||||
}
|
||||
|
||||
/* Check the stats of SCM_TIMESTAMPING_OPT_STATS */
|
||||
static void j1939cat_scm_opt_stats(struct j1939cat_priv *priv, void *buf, int len)
|
||||
{
|
||||
struct j1939cat_stats *stats = &priv->stats;
|
||||
int offset = 0;
|
||||
|
||||
while (offset < len) {
|
||||
struct nlattr *nla = (struct nlattr *) ((char *)buf + offset);
|
||||
|
||||
switch (nla->nla_type) {
|
||||
case J1939_NLA_BYTES_ACKED:
|
||||
stats->send = *(uint32_t *)((char *)nla + NLA_HDRLEN);
|
||||
break;
|
||||
case J1939_NLA_TOTAL_SIZE:
|
||||
stats->total = *(uint32_t *)((char *)nla + NLA_HDRLEN);
|
||||
break;
|
||||
case J1939_NLA_PGN:
|
||||
stats->pgn = *(uint32_t *)((char *)nla + NLA_HDRLEN);
|
||||
break;
|
||||
case J1939_NLA_DEST_ADDR:
|
||||
stats->da = *(uint8_t *)((char *)nla + NLA_HDRLEN);
|
||||
break;
|
||||
case J1939_NLA_SRC_ADDR:
|
||||
stats->sa = *(uint8_t *)((char *)nla + NLA_HDRLEN);
|
||||
break;
|
||||
case J1939_NLA_DEST_NAME:
|
||||
stats->dst_name = *(uint64_t *)((char *)nla + NLA_HDRLEN);
|
||||
break;
|
||||
case J1939_NLA_SRC_NAME:
|
||||
stats->src_name = *(uint64_t *)((char *)nla + NLA_HDRLEN);
|
||||
break;
|
||||
default:
|
||||
warnx("not supported J1939_NLA field\n");
|
||||
}
|
||||
|
||||
offset += NLA_ALIGN(nla->nla_len);
|
||||
}
|
||||
}
|
||||
|
||||
static int j1939cat_extract_serr(struct j1939cat_priv *priv)
|
||||
{
|
||||
struct j1939cat_stats *stats = &priv->stats;
|
||||
struct sock_extended_err *serr = priv->serr;
|
||||
struct scm_timestamping *tss = priv->tss;
|
||||
|
||||
switch (serr->ee_origin) {
|
||||
case SO_EE_ORIGIN_TIMESTAMPING:
|
||||
/*
|
||||
* We expect here following patterns:
|
||||
* serr->ee_info == SCM_TSTAMP_ACK
|
||||
* Activated with SOF_TIMESTAMPING_TX_ACK
|
||||
* or
|
||||
* serr->ee_info == SCM_TSTAMP_SCHED
|
||||
* Activated with SOF_TIMESTAMPING_SCHED
|
||||
* and
|
||||
* serr->ee_data == tskey
|
||||
* session message counter which is activate
|
||||
* with SOF_TIMESTAMPING_OPT_ID
|
||||
* the serr->ee_errno should be ENOMSG
|
||||
*/
|
||||
if (serr->ee_errno != ENOMSG)
|
||||
warnx("serr: expected ENOMSG, got: %i",
|
||||
serr->ee_errno);
|
||||
stats->tskey = serr->ee_data;
|
||||
|
||||
j1939cat_print_timestamp(priv, j1939cat_tstype_to_str(serr->ee_info),
|
||||
&tss->ts[0]);
|
||||
|
||||
if (serr->ee_info == SCM_TSTAMP_SCHED)
|
||||
return -EINTR;
|
||||
return 0;
|
||||
case SO_EE_ORIGIN_LOCAL:
|
||||
/*
|
||||
* The serr->ee_origin == SO_EE_ORIGIN_LOCAL is
|
||||
* currently used to notify about locally
|
||||
* detected protocol/stack errors.
|
||||
* Following patterns are expected:
|
||||
* serr->ee_info == J1939_EE_INFO_TX_ABORT
|
||||
* is used to notify about session TX
|
||||
* abort.
|
||||
* serr->ee_data == tskey
|
||||
* session message counter which is activate
|
||||
* with SOF_TIMESTAMPING_OPT_ID
|
||||
* serr->ee_errno == actual error reason
|
||||
* error reason is converted from J1939
|
||||
* abort to linux error name space.
|
||||
*/
|
||||
switch (serr->ee_info) {
|
||||
case J1939_EE_INFO_TX_ABORT:
|
||||
j1939cat_print_timestamp(priv, "TX ABT", &tss->ts[0]);
|
||||
warnx("serr: tx error: %i, %s", serr->ee_errno,
|
||||
strerror(serr->ee_errno));
|
||||
return serr->ee_errno;
|
||||
case J1939_EE_INFO_RX_RTS:
|
||||
stats->tskey = serr->ee_data;
|
||||
j1939cat_print_timestamp(priv, "RX RTS", &tss->ts[0]);
|
||||
fprintf(stderr, " total size: %u, pgn=0x%05x, sa=0x%02x, da=0x%02x src_name=0x%08" PRIx64 ", dst_name=0x%08" PRIx64 ")\n",
|
||||
stats->total, stats->pgn, stats->sa, stats->da,
|
||||
stats->src_name, stats->dst_name);
|
||||
priv->last_dpo = -1;
|
||||
return 0;
|
||||
case J1939_EE_INFO_RX_DPO:
|
||||
stats->tskey = serr->ee_data;
|
||||
j1939cat_print_timestamp(priv, "RX DPO", &tss->ts[0]);
|
||||
if (stats->send <= (uint32_t)priv->last_dpo && priv->last_dpo != -1)
|
||||
warnx("same dpo? current: %i, last: %i",
|
||||
stats->send, priv->last_dpo);
|
||||
priv->last_dpo = stats->send;
|
||||
return 0;
|
||||
case J1939_EE_INFO_RX_ABORT:
|
||||
j1939cat_print_timestamp(priv, "RX ABT", &tss->ts[0]);
|
||||
warnx("serr: rx error: %i, %s", serr->ee_errno,
|
||||
strerror(serr->ee_errno));
|
||||
return serr->ee_errno;
|
||||
default:
|
||||
warnx("serr: unknown ee_info: %i", serr->ee_info);
|
||||
return -ENOTSUP;
|
||||
}
|
||||
|
||||
break;
|
||||
default:
|
||||
warnx("serr: wrong origin: %u", serr->ee_origin);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int j1939cat_parse_cm(struct j1939cat_priv *priv, struct cmsghdr *cm)
|
||||
{
|
||||
const size_t hdr_len = CMSG_ALIGN(sizeof(struct cmsghdr));
|
||||
|
||||
if (cm->cmsg_level == SOL_SOCKET && cm->cmsg_type == SCM_TIMESTAMPING) {
|
||||
priv->tss = (void *)CMSG_DATA(cm);
|
||||
} else if (cm->cmsg_level == SOL_SOCKET && cm->cmsg_type == SCM_TIMESTAMPING_OPT_STATS) {
|
||||
void *jstats = (void *)CMSG_DATA(cm);
|
||||
|
||||
/* Activated with SOF_TIMESTAMPING_OPT_STATS */
|
||||
j1939cat_scm_opt_stats(priv, jstats, cm->cmsg_len - hdr_len);
|
||||
} else if (cm->cmsg_level == SOL_CAN_J1939 &&
|
||||
cm->cmsg_type == SCM_J1939_ERRQUEUE) {
|
||||
priv->serr = (void *)CMSG_DATA(cm);
|
||||
} else
|
||||
warnx("serr: not supported type: %d.%d",
|
||||
cm->cmsg_level, cm->cmsg_type);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int j1939cat_recv_err(struct j1939cat_priv *priv)
|
||||
{
|
||||
char control[200];
|
||||
struct cmsghdr *cm;
|
||||
int ret;
|
||||
struct msghdr msg = {
|
||||
.msg_control = control,
|
||||
.msg_controllen = sizeof(control),
|
||||
};
|
||||
|
||||
ret = recvmsg(priv->sock, &msg, MSG_ERRQUEUE);
|
||||
if (ret == -1)
|
||||
err(EXIT_FAILURE, "recvmsg error notification: %i", errno);
|
||||
if (msg.msg_flags & MSG_CTRUNC)
|
||||
err(EXIT_FAILURE, "recvmsg error notification: truncated");
|
||||
|
||||
priv->serr = NULL;
|
||||
priv->tss = NULL;
|
||||
|
||||
for (cm = CMSG_FIRSTHDR(&msg); cm && cm->cmsg_len;
|
||||
cm = CMSG_NXTHDR(&msg, cm)) {
|
||||
j1939cat_parse_cm(priv, cm);
|
||||
if (priv->serr && priv->tss)
|
||||
return j1939cat_extract_serr(priv);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int j1939cat_send_loop(struct j1939cat_priv *priv, int out_fd, char *buf,
|
||||
size_t buf_size)
|
||||
{
|
||||
struct j1939cat_stats *stats = &priv->stats;
|
||||
ssize_t count;
|
||||
char *tmp_buf = buf;
|
||||
unsigned int events = POLLOUT | POLLERR;
|
||||
bool tx_done = false;
|
||||
|
||||
count = buf_size;
|
||||
|
||||
while (!tx_done) {
|
||||
ssize_t num_sent = 0;
|
||||
|
||||
if (priv->polltimeout) {
|
||||
struct pollfd fds = {
|
||||
.fd = priv->sock,
|
||||
.events = events,
|
||||
};
|
||||
int ret;
|
||||
|
||||
ret = poll(&fds, 1, priv->polltimeout);
|
||||
if (ret == -1) {
|
||||
if (errno == EINTR)
|
||||
continue;
|
||||
else
|
||||
return -errno;
|
||||
}
|
||||
if (!ret)
|
||||
return -ETIME;
|
||||
if (!(fds.revents & events)) {
|
||||
warn("%s: something else is wrong %x %x", __func__, fds.revents, events);
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
if (fds.revents & POLLERR) {
|
||||
ret = j1939cat_recv_err(priv);
|
||||
if (ret == -EINTR)
|
||||
continue;
|
||||
if (ret)
|
||||
return ret;
|
||||
if ((priv->repeat - 1) == stats->tskey)
|
||||
tx_done = true;
|
||||
}
|
||||
|
||||
if (fds.revents & POLLOUT) {
|
||||
num_sent = j1939cat_send_one(priv, out_fd, tmp_buf, count);
|
||||
if (num_sent < 0)
|
||||
return num_sent;
|
||||
}
|
||||
} else {
|
||||
num_sent = j1939cat_send_one(priv, out_fd, tmp_buf, count);
|
||||
if (num_sent < 0)
|
||||
return num_sent;
|
||||
}
|
||||
|
||||
count -= num_sent;
|
||||
tmp_buf += num_sent;
|
||||
if (buf + buf_size < tmp_buf + count) {
|
||||
warn("%s: send buffer is bigger than the read buffer",
|
||||
__func__);
|
||||
return -EINVAL;
|
||||
}
|
||||
if (!count) {
|
||||
if (priv->repeat == priv->round)
|
||||
events = POLLERR;
|
||||
else
|
||||
tx_done = true;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int j1939cat_sendfile(struct j1939cat_priv *priv, int out_fd, int in_fd,
|
||||
off_t *offset, size_t count)
|
||||
{
|
||||
int ret = EXIT_SUCCESS;
|
||||
off_t orig = 0;
|
||||
char *buf;
|
||||
ssize_t num_read;
|
||||
size_t to_read, buf_size;
|
||||
|
||||
buf_size = min(priv->max_transfer, count);
|
||||
buf = malloc(buf_size);
|
||||
if (!buf) {
|
||||
warn("can't allocate buf");
|
||||
ret = EXIT_FAILURE;
|
||||
goto do_nofree;
|
||||
}
|
||||
|
||||
if (offset) {
|
||||
|
||||
/* Save current file offset and set offset to value in '*offset' */
|
||||
|
||||
orig = lseek(in_fd, 0, SEEK_CUR);
|
||||
if (orig == -1) {
|
||||
ret = EXIT_FAILURE;
|
||||
goto do_free;
|
||||
}
|
||||
if (lseek(in_fd, *offset, SEEK_SET) == -1) {
|
||||
ret = EXIT_FAILURE;
|
||||
goto do_free;
|
||||
}
|
||||
}
|
||||
|
||||
while (count > 0) {
|
||||
to_read = min(buf_size, count);
|
||||
|
||||
num_read = read(in_fd, buf, to_read);
|
||||
if (num_read == -1) {
|
||||
ret = EXIT_FAILURE;
|
||||
goto do_free;
|
||||
}
|
||||
if (num_read == 0)
|
||||
break; /* EOF */
|
||||
|
||||
ret = j1939cat_send_loop(priv, out_fd, buf, num_read);
|
||||
if (ret)
|
||||
goto do_free;
|
||||
|
||||
count -= num_read;
|
||||
}
|
||||
|
||||
if (offset) {
|
||||
/* Return updated file offset in '*offset', and reset the file offset
|
||||
to the value it had when we were called. */
|
||||
|
||||
*offset = lseek(in_fd, 0, SEEK_CUR);
|
||||
if (*offset == -1) {
|
||||
ret = EXIT_FAILURE;
|
||||
goto do_free;
|
||||
}
|
||||
|
||||
if (lseek(in_fd, orig, SEEK_SET) == -1) {
|
||||
ret = EXIT_FAILURE;
|
||||
goto do_free;
|
||||
}
|
||||
}
|
||||
|
||||
do_free:
|
||||
free(buf);
|
||||
do_nofree:
|
||||
return ret;
|
||||
}
|
||||
|
||||
static size_t j1939cat_get_file_size(int fd)
|
||||
{
|
||||
off_t offset;
|
||||
|
||||
offset = lseek(fd, 0, SEEK_END);
|
||||
if (offset == -1)
|
||||
err(1, "%s lseek()\n", __func__);
|
||||
|
||||
if (lseek(fd, 0, SEEK_SET) == -1)
|
||||
err(1, "%s lseek() start\n", __func__);
|
||||
|
||||
return offset;
|
||||
}
|
||||
|
||||
static int j1939cat_send(struct j1939cat_priv *priv)
|
||||
{
|
||||
unsigned int size = 0;
|
||||
unsigned int i;
|
||||
int ret = 0;
|
||||
|
||||
if (priv->todo_filesize)
|
||||
size = j1939cat_get_file_size(priv->infile);
|
||||
|
||||
if (!size)
|
||||
return EXIT_FAILURE;
|
||||
|
||||
for (i = 0; i < priv->repeat; i++) {
|
||||
priv->round++;
|
||||
ret = j1939cat_sendfile(priv, priv->sock, priv->infile, NULL, size);
|
||||
if (ret)
|
||||
break;
|
||||
|
||||
if (lseek(priv->infile, 0, SEEK_SET) == -1)
|
||||
err(1, "%s lseek() start\n", __func__);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static int j1939cat_recv_one(struct j1939cat_priv *priv, uint8_t *buf, size_t buf_size)
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = recv(priv->sock, buf, buf_size, 0);
|
||||
if (ret < 0) {
|
||||
warn("recvf()");
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
ret = write(priv->outfile, buf, ret);
|
||||
if (ret < 0) {
|
||||
warn("write stdout()");
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
return EXIT_SUCCESS;
|
||||
}
|
||||
|
||||
static int j1939cat_recv(struct j1939cat_priv *priv)
|
||||
{
|
||||
unsigned int events = POLLIN | POLLERR;
|
||||
int ret = EXIT_SUCCESS;
|
||||
size_t buf_size;
|
||||
uint8_t *buf;
|
||||
|
||||
buf_size = priv->max_transfer;
|
||||
buf = malloc(buf_size);
|
||||
if (!buf) {
|
||||
warn("can't allocate rx buf");
|
||||
return EXIT_FAILURE;;
|
||||
}
|
||||
|
||||
priv->last_dpo = -1;
|
||||
|
||||
while (priv->todo_recv) {
|
||||
if (priv->polltimeout) {
|
||||
struct pollfd fds = {
|
||||
.fd = priv->sock,
|
||||
.events = events,
|
||||
};
|
||||
int ret;
|
||||
|
||||
ret = poll(&fds, 1, priv->polltimeout);
|
||||
if (ret == -1) {
|
||||
if (errno == EINTR)
|
||||
continue;
|
||||
else
|
||||
return -errno;
|
||||
}
|
||||
if (!ret)
|
||||
continue;
|
||||
if (!(fds.revents & events)) {
|
||||
warn("%s: something else is wrong %x %x", __func__, fds.revents, events);
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
if (fds.revents & POLLERR) {
|
||||
ret = j1939cat_recv_err(priv);
|
||||
if (ret == -EINTR)
|
||||
continue;
|
||||
if (ret)
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (fds.revents & POLLIN) {
|
||||
ret = j1939cat_recv_one(priv, buf, buf_size);
|
||||
if (ret < 0)
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
ret = j1939cat_recv_one(priv, buf, buf_size);
|
||||
if (ret)
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
free(buf);
|
||||
return ret;
|
||||
}
|
||||
|
||||
static int j1939cat_sock_prepare(struct j1939cat_priv *priv)
|
||||
{
|
||||
unsigned int sock_opt;
|
||||
int value;
|
||||
int ret;
|
||||
|
||||
/* open socket */
|
||||
priv->sock = socket(PF_CAN, SOCK_DGRAM, CAN_J1939);
|
||||
if (priv->sock < 0) {
|
||||
warn("socket(j1939)");
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
if (priv->todo_prio >= 0) {
|
||||
ret = setsockopt(priv->sock, SOL_CAN_J1939, SO_J1939_SEND_PRIO,
|
||||
&priv->todo_prio, sizeof(priv->todo_prio));
|
||||
if (ret < 0) {
|
||||
warn("set priority %i", priv->todo_prio);
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
}
|
||||
|
||||
value = 1;
|
||||
ret = setsockopt(priv->sock, SOL_CAN_J1939, SO_J1939_ERRQUEUE, &value,
|
||||
sizeof(value));
|
||||
if (ret < 0) {
|
||||
warn("set recverr");
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
sock_opt = SOF_TIMESTAMPING_SOFTWARE |
|
||||
SOF_TIMESTAMPING_OPT_CMSG |
|
||||
SOF_TIMESTAMPING_TX_ACK |
|
||||
SOF_TIMESTAMPING_TX_SCHED |
|
||||
SOF_TIMESTAMPING_OPT_STATS | SOF_TIMESTAMPING_OPT_TSONLY |
|
||||
SOF_TIMESTAMPING_OPT_ID | SOF_TIMESTAMPING_RX_SOFTWARE;
|
||||
|
||||
if (setsockopt(priv->sock, SOL_SOCKET, SO_TIMESTAMPING,
|
||||
(char *) &sock_opt, sizeof(sock_opt)))
|
||||
err(1, "setsockopt timestamping");
|
||||
|
||||
if (priv->todo_broadcast) {
|
||||
ret = setsockopt(priv->sock, SOL_SOCKET, SO_BROADCAST,
|
||||
&priv->todo_broadcast,
|
||||
sizeof(priv->todo_broadcast));
|
||||
if (ret < 0) {
|
||||
warn("setsockopt: filed to set broadcast");
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
}
|
||||
|
||||
ret = bind(priv->sock, (void *)&priv->sockname, sizeof(priv->sockname));
|
||||
if (ret < 0) {
|
||||
warn("bind()");
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
if (!priv->todo_connect)
|
||||
return EXIT_SUCCESS;
|
||||
|
||||
if (!priv->valid_peername) {
|
||||
warn("no peername supplied");
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
ret = connect(priv->sock, (void *)&priv->peername,
|
||||
sizeof(priv->peername));
|
||||
if (ret < 0) {
|
||||
warn("connect()");
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
return EXIT_SUCCESS;
|
||||
}
|
||||
|
||||
static int j1939cat_parse_args(struct j1939cat_priv *priv, int argc, char *argv[])
|
||||
{
|
||||
int opt;
|
||||
|
||||
/* argument parsing */
|
||||
while ((opt = getopt(argc, argv, optstring)) != -1)
|
||||
switch (opt) {
|
||||
case 'i':
|
||||
priv->infile = open(optarg, O_RDONLY);
|
||||
if (priv->infile == -1)
|
||||
err(EXIT_FAILURE, "can't open input file");
|
||||
priv->todo_filesize = 1;
|
||||
break;
|
||||
case 's':
|
||||
priv->max_transfer = strtoul(optarg, NULL, 0);
|
||||
if (priv->max_transfer > J1939_MAX_ETP_PACKET_SIZE)
|
||||
err(EXIT_FAILURE,
|
||||
"used value (%zu) is bigger then allowed maximal size: %u.\n",
|
||||
priv->max_transfer,
|
||||
J1939_MAX_ETP_PACKET_SIZE);
|
||||
break;
|
||||
case 'r':
|
||||
priv->todo_recv = 1;
|
||||
break;
|
||||
case 'p':
|
||||
priv->todo_prio = strtoul(optarg, NULL, 0);
|
||||
break;
|
||||
case 'P':
|
||||
priv->polltimeout = strtoul(optarg, NULL, 0);
|
||||
break;
|
||||
case 'c':
|
||||
priv->todo_connect = 1;
|
||||
break;
|
||||
case 'R':
|
||||
priv->repeat = strtoul(optarg, NULL, 0);
|
||||
if (priv->repeat < 1)
|
||||
err(EXIT_FAILURE,
|
||||
"send/repeat count can't be less then 1\n");
|
||||
break;
|
||||
case 'B':
|
||||
priv->todo_broadcast = 1;
|
||||
break;
|
||||
case 'h': /*fallthrough*/
|
||||
default:
|
||||
fputs(help_msg, stderr);
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
if (argv[optind]) {
|
||||
if (strcmp("-", argv[optind]) != 0)
|
||||
libj1939_parse_canaddr(argv[optind], &priv->sockname);
|
||||
optind++;
|
||||
}
|
||||
|
||||
if (argv[optind]) {
|
||||
if (strcmp("-", argv[optind]) != 0) {
|
||||
libj1939_parse_canaddr(argv[optind], &priv->peername);
|
||||
priv->valid_peername = 1;
|
||||
}
|
||||
optind++;
|
||||
}
|
||||
|
||||
return EXIT_SUCCESS;
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
struct j1939cat_priv *priv;
|
||||
int ret = 0;
|
||||
|
||||
priv = calloc(1, sizeof(*priv));
|
||||
if (!priv)
|
||||
err(EXIT_FAILURE, "can't allocate priv");
|
||||
|
||||
priv->todo_prio = -1;
|
||||
priv->infile = STDIN_FILENO;
|
||||
priv->outfile = STDOUT_FILENO;
|
||||
priv->max_transfer = J1939_MAX_ETP_PACKET_SIZE;
|
||||
priv->polltimeout = 100000;
|
||||
priv->repeat = 1;
|
||||
|
||||
libj1939_init_sockaddr_can(&priv->sockname, J1939_NO_PGN);
|
||||
libj1939_init_sockaddr_can(&priv->peername, J1939_NO_PGN);
|
||||
|
||||
ret = j1939cat_parse_args(priv, argc, argv);
|
||||
if (ret)
|
||||
return ret;
|
||||
|
||||
ret = j1939cat_sock_prepare(priv);
|
||||
if (ret)
|
||||
return ret;
|
||||
|
||||
if (priv->todo_recv)
|
||||
ret = j1939cat_recv(priv);
|
||||
else
|
||||
ret = j1939cat_send(priv);
|
||||
|
||||
close(priv->infile);
|
||||
close(priv->outfile);
|
||||
close(priv->sock);
|
||||
|
||||
return ret;
|
||||
}
|
||||
Reference in New Issue
Block a user