sven
/
can-utils
mirror of https://github.com/linux-can/can-utils.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
can-utils/isobusfs/isobusfs_cli.c

689 lines
16 KiB
C
Raw Blame History

// SPDX-License-Identifier: LGPL-2.0-only
// SPDX-FileCopyrightText: 2023 Oleksij Rempel <linux@rempel-privat.de>
#include <err.h>
#include <errno.h>
#include <fcntl.h>
#include <getopt.h>
#include <net/if.h>
#include <poll.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <unistd.h>
#include "isobusfs_cli.h"
/* Maximal number of events that can be registered. The number is
* based on feeling not on any real data.
*/
#define ISOBUSFS_CLI_MAX_EVENTS 10
int isobusfs_cli_register_event(struct isobusfs_priv *priv,
const struct isobusfs_event *new_event)
{
if (!priv->events) {
priv->events = malloc(sizeof(*new_event) *
ISOBUSFS_CLI_MAX_EVENTS);
if (!priv->events)
return -ENOMEM;
priv->max_events = ISOBUSFS_CLI_MAX_EVENTS;
} else if (priv->num_events >= priv->max_events) {
return -1;
}
memcpy(&priv->events[priv->num_events], new_event,
sizeof(*new_event));
priv->num_events++;
return 0;
}
int isobusfs_cli_remove_event(struct isobusfs_priv *priv,
struct isobusfs_event *event_to_remove)
{
size_t i;
for (i = 0; i < priv->num_events; ++i) {
if (&priv->events[i] == event_to_remove) {
memmove(&priv->events[i], &priv->events[i + 1],
(priv->num_events - i - 1) *
sizeof(*priv->events));
priv->num_events--;
return 0;
}
}
return -ENOENT;
}
struct timespec ms_to_timespec(unsigned int timeout_ms)
{
struct timespec ts;
ts.tv_sec = timeout_ms / 1000;
ts.tv_nsec = (timeout_ms % 1000) * 1000000;
return ts;
}
void isobusfs_cli_prepare_response_event(struct isobusfs_event *event, int sock,
uint8_t fs_function)
{
struct timespec current_time, timeout_timespec;
event->fd = sock;
event->fs_function = fs_function;
/* Calculate the timeout */
clock_gettime(CLOCK_REALTIME, &current_time);
timeout_timespec = ms_to_timespec(ISOBUSFS_CLI_DEFAULT_WAIT_TIMEOUT_MS);
event->timeout.tv_sec = current_time.tv_sec + timeout_timespec.tv_sec;
event->timeout.tv_nsec = current_time.tv_nsec + timeout_timespec.tv_nsec;
/* Adjust for nanosecond overflow */
if (event->timeout.tv_nsec >= 1000000000) {
event->timeout.tv_nsec -= 1000000000;
event->timeout.tv_sec++;
}
event->one_shot = true;
}
static bool isobusfs_cli_has_event_expired(const struct timespec *timeout)
{
struct timespec now;
clock_gettime(CLOCK_REALTIME, &now);
return now.tv_sec > timeout->tv_sec ||
(now.tv_sec == timeout->tv_sec &&
now.tv_nsec > timeout->tv_nsec);
}
static void isobusfs_cli_process_expired_events(struct isobusfs_priv *priv)
{
for (size_t i = 0; i < priv->num_events; ++i) {
struct isobusfs_event *event = &priv->events[i];
if (isobusfs_cli_has_event_expired(&event->timeout)) {
event->cb(priv, NULL, event->ctx, -ETIME);
isobusfs_cli_remove_event(priv, event);
i--;
}
}
}
static int isobusfs_cli_rx_event(struct isobusfs_priv *priv, int sock,
struct isobusfs_msg *msg, bool *found)
{
struct isobusfs_event *event = NULL;
int ret = 0;
*found = false;
for (size_t i = 0; i < priv->num_events; ++i) {
event = &priv->events[i];
if (event->fd == sock && event->fs_function == msg->buf[0]) {
if (event->cb)
ret = event->cb(priv, msg, event->ctx, 0);
*found = true;
break;
}
}
if (*found && event->one_shot)
isobusfs_cli_remove_event(priv, event);
return ret;
}
static int isobusfs_cli_rx(struct isobusfs_priv *priv, struct isobusfs_msg *msg)
{
int cmd = isobusfs_buf_to_cmd(msg->buf);
int ret = 0;
switch (cmd) {
case ISOBUSFS_CG_CONNECTION_MANAGMENT:
ret = isobusfs_cli_rx_cg_cm(priv, msg);
break;
case ISOBUSFS_CG_DIRECTORY_HANDLING:
ret = isobusfs_cli_rx_cg_dh(priv, msg);
break;
case ISOBUSFS_CG_FILE_ACCESS: /* fall through */
ret = isobusfs_cli_rx_cg_fa(priv, msg);
break;
case ISOBUSFS_CG_FILE_HANDLING: /* fall through */
case ISOBUSFS_CG_VOLUME_HANDLING: /* fall through */
default:
isobusfs_send_nack(priv->sock_nack, msg);
pr_warn("unsupported command group: %i", cmd);
/* Not a critical error */
return 0;
}
return ret;
}
static int isobusfs_cli_rx_ack(struct isobusfs_priv *priv, struct isobusfs_msg *msg)
{
enum isobusfs_ack_ctrl ctrl = msg->buf[0];
switch (ctrl) {
case ISOBUS_ACK_CTRL_ACK:
/* received ACK unexpectedly an ACK, no idea what to do */
pr_debug("< rx: ACK?????");
break;
case ISOBUS_ACK_CTRL_NACK:
/* we did something wrong */
pr_debug("< rx: NACK!!!!!!");
/* try to provide some usable information with a trace of
* the TX history
*/
isobusfs_dump_tx_data(&priv->tx_buf_log);
priv->state = ISOBUSFS_CLI_STATE_IDLE;
break;
default:
pr_warn("%s: unsupported ACK control: %i", __func__, ctrl);
}
/* Not a critical error */
return 0;
}
static int isobusfs_cli_rx_buf(struct isobusfs_priv *priv, struct isobusfs_msg *msg)
{
pgn_t pgn = msg->peername.can_addr.j1939.pgn;
int ret;
switch (pgn) {
case ISOBUSFS_PGN_FS_TO_CL:
ret = isobusfs_cli_rx(priv, msg);
break;
case ISOBUS_PGN_ACK:
ret = isobusfs_cli_rx_ack(priv, msg);
break;
default:
pr_warn("%s: unsupported PGN: %x", __func__, pgn);
/* Not a critical error */
ret = 0;
break;
}
return ret;
}
static int isobusfs_cli_rx_one(struct isobusfs_priv *priv, int sock)
{
struct isobusfs_msg *msg;
int flags = 0;
bool found;
int ret;
msg = malloc(sizeof(*msg));
if (!msg) {
pr_err("can't allocate rx msg struct\n");
return -ENOMEM;
}
msg->buf_size = ISOBUSFS_MAX_TRANSFER_LENGH;
msg->peer_addr_len = sizeof(msg->peername);
msg->sock = sock;
ret = recvfrom(sock, &msg->buf[0], msg->buf_size, flags,
(struct sockaddr *)&msg->peername, &msg->peer_addr_len);
if (ret < 0) {
ret = -errno;
pr_warn("recvfrom() failed: %i %s", ret, strerror(-ret));
return ret;
}
if (ret < ISOBUSFS_MIN_TRANSFER_LENGH) {
pr_warn("buf is less then min transfer: %i\n", ret);
isobusfs_send_nack(priv->sock_nack, msg);
return -EINVAL;
}
msg->len = ret;
ret = isobusfs_cli_rx_event(priv, sock, msg, &found);
if (ret < 0) {
pr_warn("failed to process rx event: %i (%s)\n", ret, strerror(ret));
return ret;
} else if (found)
return 0;
ret = isobusfs_cli_rx_buf(priv, msg);
if (ret < 0) {
pr_warn("failed to process rx buf: %i (%s)\n", ret, strerror(ret));
return ret;
}
return 0;
}
static int isobusfs_cli_handle_events(struct isobusfs_priv *priv, unsigned int nfds)
{
int ret;
unsigned int n;
for (n = 0; n < nfds && n < priv->cmn.epoll_events_size; ++n) {
struct epoll_event *ev = &priv->cmn.epoll_events[n];
if (!ev->events) {
warn("no events");
continue;
}
if (ev->data.fd == priv->sock_ccm) {
if (ev->events & POLLERR) {
struct isobusfs_err_msg emsg = {
.stats = &priv->stats,
};
ret = isobusfs_recv_err(priv->sock_ccm, &emsg);
if (ret)
pr_warn("error queue reported error: %i", ret);
}
} else if (ev->data.fd == STDIN_FILENO) {
if (!priv->interactive) {
warn("got POLLIN on stdin, but interactive mode is disabled");
continue;
}
if (ev->events & POLLIN) {
ret = isobusfs_cli_interactive(priv);
if (ret)
return ret;
} else
warn("got not POLLIN on stdin");
} else if (ev->events & POLLIN) {
ret = isobusfs_cli_rx_one(priv, ev->data.fd);
if (ret) {
warn("recv one");
return ret;
}
}
}
return 0;
}
static int isobusfs_cli_handle_periodic_tasks(struct isobusfs_priv *priv)
{
/* detect FS timeout */
isobusfs_cli_fs_detect_timeout(priv);
isobusfs_cli_run_self_tests(priv);
isobusfs_cli_process_expired_events(priv);
/* this function will send status only if it is proper time to do so */
return isobusfs_cli_ccm_send(priv);
}
int isobusfs_cli_process_events_and_tasks(struct isobusfs_priv *priv)
{
bool dont_wait = false;
int nfds = 0;
int ret;
if (priv->state == ISOBUSFS_CLI_STATE_SELFTEST)
dont_wait = true;
ret = isobusfs_cmn_prepare_for_events(&priv->cmn, &nfds, dont_wait);
if (ret)
return ret;
if (nfds > 0) {
ret = isobusfs_cli_handle_events(priv, (unsigned int)nfds);
if (ret)
return ret;
}
return isobusfs_cli_handle_periodic_tasks(priv);
}
static int isobusfs_cli_sock_main_prepare(struct isobusfs_priv *priv)
{
struct sockaddr_can addr = priv->sockname;
int ret;
ret = isobusfs_cmn_open_socket();
if (ret < 0)
return ret;
priv->sock_main = ret;
/* TODO: this is TX only socket */
addr.can_addr.j1939.pgn = ISOBUSFS_PGN_FS_TO_CL;
ret = isobusfs_cmn_bind_socket(priv->sock_main, &addr);
if (ret < 0)
return ret;
ret = isobusfs_cmn_set_linger(priv->sock_main);
if (ret < 0)
return ret;
ret = isobusfs_cmn_socket_prio(priv->sock_main, ISOBUSFS_PRIO_DEFAULT);
if (ret < 0)
return ret;
ret = isobusfs_cmn_connect_socket(priv->sock_main, &priv->peername);
if (ret < 0)
return ret;
return isobusfs_cmn_add_socket_to_epoll(priv->cmn.epoll_fd,
priv->sock_main, EPOLLIN);
}
/* isobusfs_cli_sock_int_prepare() is used to prepare stdin for interactive
* mode.
*/
static int isobusfs_cli_sock_int_prepare(struct isobusfs_priv *priv)
{
int ret;
if (!priv->interactive)
return 0;
isobusfs_set_interactive(true);
ret = fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);
if (ret < 0)
return ret;
return isobusfs_cmn_add_socket_to_epoll(priv->cmn.epoll_fd,
STDIN_FILENO, EPOLLIN);
}
static int isobusfs_cli_sock_ccm_prepare(struct isobusfs_priv *priv)
{
struct sockaddr_can addr = priv->sockname;
int ret;
ret = isobusfs_cmn_open_socket();
if (ret < 0)
return ret;
priv->sock_ccm = ret;
ret = isobusfs_cmn_configure_error_queue(priv->sock_ccm);
if (ret < 0)
return ret;
/* TODO: this is TX only socket */
addr.can_addr.j1939.pgn = J1939_NO_PGN;
ret = isobusfs_cmn_bind_socket(priv->sock_ccm, &addr);
if (ret < 0)
return ret;
ret = isobusfs_cmn_set_linger(priv->sock_ccm);
if (ret < 0)
return ret;
ret = isobusfs_cmn_socket_prio(priv->sock_ccm, ISOBUSFS_PRIO_DEFAULT);
if (ret < 0)
return ret;
ret = isobusfs_cmn_connect_socket(priv->sock_ccm, &priv->peername);
if (ret < 0)
return ret;
/* poll for errors to get confirmation if our packets are send */
return isobusfs_cmn_add_socket_to_epoll(priv->cmn.epoll_fd, priv->sock_ccm,
EPOLLERR);
}
static int isobusfs_cli_sock_nack_prepare(struct isobusfs_priv *priv)
{
struct sockaddr_can addr = priv->sockname;
int ret;
ret = isobusfs_cmn_open_socket();
if (ret < 0)
return ret;
priv->sock_nack = ret;
addr.can_addr.j1939.pgn = ISOBUS_PGN_ACK;
ret = isobusfs_cmn_bind_socket(priv->sock_nack, &addr);
if (ret < 0)
return ret;
ret = isobusfs_cmn_socket_prio(priv->sock_nack, ISOBUSFS_PRIO_ACK);
if (ret < 0)
return ret;
/* poll for errors to get confirmation if our packets are send */
return isobusfs_cmn_add_socket_to_epoll(priv->cmn.epoll_fd,
priv->sock_nack, EPOLLIN);
}
/* rx socket for fss and volume status announcements */
static int isobusfs_cli_sock_bcast_prepare(struct isobusfs_priv *priv)
{
struct sockaddr_can addr = priv->sockname;
int ret;
ret = isobusfs_cmn_open_socket();
if (ret < 0)
return ret;
priv->sock_bcast_rx = ret;
/* keep address and name and overwrite PGN */
addr.can_addr.j1939.name = J1939_NO_NAME;
addr.can_addr.j1939.addr = J1939_NO_ADDR;
addr.can_addr.j1939.pgn = ISOBUSFS_PGN_FS_TO_CL;
ret = isobusfs_cmn_bind_socket(priv->sock_bcast_rx, &addr);
if (ret < 0)
return ret;
ret = isobusfs_cmn_set_broadcast(priv->sock_bcast_rx);
if (ret < 0)
return ret;
ret = isobusfs_cmn_connect_socket(priv->sock_bcast_rx, &priv->peername);
if (ret < 0)
return ret;
return isobusfs_cmn_add_socket_to_epoll(priv->cmn.epoll_fd, priv->sock_bcast_rx,
EPOLLIN);
}
static int isobusfs_cli_sock_prepare(struct isobusfs_priv *priv)
{
int ret;
ret = isobusfs_cmn_create_epoll();
if (ret < 0)
return ret;
priv->cmn.epoll_fd = ret;
priv->cmn.epoll_events = calloc(ISOBUSFS_CLI_MAX_EPOLL_EVENTS,
sizeof(struct epoll_event));
if (!priv->cmn.epoll_events)
return -ENOMEM;
priv->cmn.epoll_events_size = ISOBUSFS_CLI_MAX_EPOLL_EVENTS;
ret = isobusfs_cli_sock_int_prepare(priv);
if (ret < 0)
return ret;
ret = isobusfs_cli_sock_ccm_prepare(priv);
if (ret < 0)
return ret;
ret = isobusfs_cli_sock_bcast_prepare(priv);
if (ret < 0)
return ret;
ret = isobusfs_cli_sock_main_prepare(priv);
if (ret < 0)
return ret;
return isobusfs_cli_sock_nack_prepare(priv);
}
static void isobusfs_cli_print_help(void)
{
printf("Usage: isobusfs-cli [options]\n");
printf("Options:\n");
printf(" --interactive or -I (Default)\n");
printf(" --interface <interface_name> or -i <interface_name>\n");
printf(" --local-address <local_address_hex> or -a <local_address_hex>\n");
printf(" --local-name <local_name_hex> or -n <local_name_hex>\n");
printf(" --log-level <logging_level> or -l <logging_level> (Default %d)\n",
LOG_LEVEL_INFO);
printf(" --remote-address <remote_address_hex> or -r <remote_address_hex>\n");
printf(" --remote-name <remote_name_hex> or -m <remote_name_hex>\n");
printf("Note: Local address and local name are mutually exclusive\n");
printf("Note: Remote address and remote name are mutually exclusive\n");
}
static int isobusfs_cli_parse_args(struct isobusfs_priv *priv, int argc, char *argv[])
{
struct sockaddr_can *remote = &priv->peername;
struct sockaddr_can *local = &priv->sockname;
bool local_address_set = false;
bool local_name_set = false;
bool remote_address_set = false;
bool remote_name_set = false;
bool interface_set = false;
int long_index = 0;
int level;
int opt;
static struct option long_options[] = {
{"interface", required_argument, 0, 'i'},
{"interactive", no_argument, 0, 'I'},
{"local-address", required_argument, 0, 'a'},
{"local-name", required_argument, 0, 'n'},
{"log-level", required_argument, 0, 'l'},
{"remote-address", required_argument, 0, 'r'},
{"remote-name", required_argument, 0, 'm'},
{0, 0, 0, 0}
};
/* active by default */
priv->interactive = true;
while ((opt = getopt_long(argc, argv, "a:n:r:m:Ii:l:", long_options, &long_index)) != -1) {
switch (opt) {
case 'a':
local->can_addr.j1939.addr = strtoul(optarg, NULL, 16);
local_address_set = true;
break;
case 'n':
local->can_addr.j1939.name = strtoull(optarg, NULL, 16);
local_name_set = true;
break;
case 'r':
remote->can_addr.j1939.addr = strtoul(optarg, NULL, 16);
remote_address_set = true;
break;
case 'm':
remote->can_addr.j1939.name = strtoull(optarg, NULL, 16);
remote_name_set = true;
break;
case 'i':
local->can_ifindex = if_nametoindex(optarg);
if (!local->can_ifindex) {
pr_err("Interface %s not found. Error: %d (%s)\n",
optarg, errno, strerror(errno));
return -EINVAL;
}
remote->can_ifindex = local->can_ifindex;
interface_set = true;
break;
case 'I':
priv->interactive = true;
break;
case 'l':
level = strtoul(optarg, NULL, 0);
if (level < LOG_LEVEL_ERROR || level > LOG_LEVEL_DEBUG)
pr_err("invalid debug level %d", level);
isobusfs_log_level_set(level);
break;
default:
isobusfs_cli_print_help();
return -EINVAL;
}
}
if (!interface_set) {
pr_err("interface not specified");
isobusfs_cli_print_help();
return -EINVAL;
}
if ((local_address_set && local_name_set) ||
(remote_address_set && remote_name_set)) {
pr_err("local address and local name or remote address and remote name are mutually exclusive");
isobusfs_cli_print_help();
return -EINVAL;
}
return 0;
}
int main(int argc, char *argv[])
{
struct isobusfs_priv *priv;
struct timespec ts;
int ret;
priv = malloc(sizeof(*priv));
if (!priv)
err(EXIT_FAILURE, "can't allocate priv");
bzero(priv, sizeof(*priv));
isobusfs_init_sockaddr_can(&priv->sockname, J1939_NO_PGN);
isobusfs_init_sockaddr_can(&priv->peername, ISOBUSFS_PGN_CL_TO_FS);
ret = isobusfs_cli_parse_args(priv, argc, argv);
if (ret)
return ret;
ret = isobusfs_cli_sock_prepare(priv);
if (ret)
return ret;
isobusfs_cli_ccm_init(priv);
/* Init next st_next_send_time value to avoid warnings */
clock_gettime(CLOCK_MONOTONIC, &ts);
priv->cmn.next_send_time = ts;
if (priv->interactive)
isobusfs_cli_int_start(priv);
else
pr_debug("starting client\n");
while (1) {
ret = isobusfs_cli_process_events_and_tasks(priv);
if (ret)
break;
}
close(priv->cmn.epoll_fd);
free(priv->cmn.epoll_events);
close(priv->sock_main);
close(priv->sock_nack);
close(priv->sock_ccm);
close(priv->sock_bcast_rx);
return ret;
}
Reference in New Issue View Git Blame Copy Permalink