From 69c1e8289d447ec3b058c2a9fb54434937ec862b Mon Sep 17 00:00:00 2001 From: Oleksij Rempel Date: Wed, 15 May 2024 20:01:48 +0200 Subject: [PATCH 1/3] add pr_err and pr_warn macros This variant will be used in the j19393-timeday code. Signed-off-by: Oleksij Rempel --- include/linux/kernel.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/linux/kernel.h b/include/linux/kernel.h index b7cbdb3..f9e5fb3 100644 --- a/include/linux/kernel.h +++ b/include/linux/kernel.h @@ -22,6 +22,8 @@ struct regmap { }; #define pr_info(...) fprintf(stdout, ## __VA_ARGS__) +#define pr_err(...) fprintf(stderr, ## __VA_ARGS__) +#define pr_warn(...) fprintf(stderr, ## __VA_ARGS__) #define pr_cont(...) fprintf(stdout, ## __VA_ARGS__) #define netdev_info(ndev, ...) fprintf(stdout, ## __VA_ARGS__) #define BUILD_BUG_ON(...) From af95ee0c6d37cb07ed4ffdd6b5f57d9b9ab5d601 Mon Sep 17 00:00:00 2001 From: Oleksij Rempel Date: Wed, 15 May 2024 17:00:27 +0200 Subject: [PATCH 2/3] move part of isobusfs code to the libj1939 Move part of isobusfs which can be reused by other applications to the libj1939. By the way, reuse some of new libj1939 code in the j1939cat. Signed-off-by: Oleksij Rempel --- CMakeLists.txt | 10 +- isobusfs/isobusfs_cli.c | 42 +++---- isobusfs/isobusfs_cli.h | 2 +- isobusfs/isobusfs_cmn.c | 185 ---------------------------- isobusfs/isobusfs_cmn.h | 19 --- isobusfs/isobusfs_srv.c | 34 +++--- isobusfs/isobusfs_srv.h | 2 +- isobusfs/isobusfs_srv_cm.c | 6 +- j1939cat.c | 13 +- libj1939.c | 244 ++++++++++++++++++++++++++++++++++++- libj1939.h | 24 +++- 11 files changed, 314 insertions(+), 267 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c7991ab..a5e5c30 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -105,6 +105,10 @@ if(NOT ANDROID) libj1939.c ) + target_link_libraries(j1939 + PRIVATE can + ) + add_library(isobusfs SHARED isobusfs/isobusfs_cmn.c isobusfs/isobusfs_cmn_dh.c @@ -134,7 +138,7 @@ if(NOT ANDROID) ) target_link_libraries(isobusfs-cli - PRIVATE isobusfs can + PRIVATE isobusfs can j1939 ) add_executable(isobusfs-srv @@ -148,7 +152,7 @@ if(NOT ANDROID) ) target_link_libraries(isobusfs-srv - PRIVATE isobusfs can + PRIVATE isobusfs can j1939 ) install(TARGETS @@ -167,7 +171,7 @@ foreach(name ${PROGRAMS}) if("${name}" IN_LIST PROGRAMS_J1939) target_link_libraries(${name} - PRIVATE j1939 + PRIVATE j1939 can ) elseif("${name}" IN_LIST PROGRAMS_CANLIB) target_link_libraries(${name} diff --git a/isobusfs/isobusfs_cli.c b/isobusfs/isobusfs_cli.c index b6bcd77..6d74bba 100644 --- a/isobusfs/isobusfs_cli.c +++ b/isobusfs/isobusfs_cli.c @@ -336,7 +336,7 @@ int isobusfs_cli_process_events_and_tasks(struct isobusfs_priv *priv) if (priv->state == ISOBUSFS_CLI_STATE_SELFTEST) dont_wait = true; - ret = isobusfs_cmn_prepare_for_events(&priv->cmn, &nfds, dont_wait); + ret = libj1939_prepare_for_events(&priv->cmn, &nfds, dont_wait); if (ret) return ret; @@ -354,7 +354,7 @@ static int isobusfs_cli_sock_main_prepare(struct isobusfs_priv *priv) struct sockaddr_can addr = priv->sockname; int ret; - ret = isobusfs_cmn_open_socket(); + ret = libj1939_open_socket(); if (ret < 0) return ret; @@ -362,7 +362,7 @@ static int isobusfs_cli_sock_main_prepare(struct isobusfs_priv *priv) /* 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); + ret = libj1939_bind_socket(priv->sock_main, &addr); if (ret < 0) return ret; @@ -370,7 +370,7 @@ static int isobusfs_cli_sock_main_prepare(struct isobusfs_priv *priv) if (ret < 0) return ret; - ret = isobusfs_cmn_socket_prio(priv->sock_main, ISOBUSFS_PRIO_DEFAULT); + ret = libj1939_socket_prio(priv->sock_main, ISOBUSFS_PRIO_DEFAULT); if (ret < 0) return ret; @@ -378,7 +378,7 @@ static int isobusfs_cli_sock_main_prepare(struct isobusfs_priv *priv) if (ret < 0) return ret; - return isobusfs_cmn_add_socket_to_epoll(priv->cmn.epoll_fd, + return libj1939_add_socket_to_epoll(priv->cmn.epoll_fd, priv->sock_main, EPOLLIN); } @@ -398,7 +398,7 @@ static int isobusfs_cli_sock_int_prepare(struct isobusfs_priv *priv) if (ret < 0) return ret; - return isobusfs_cmn_add_socket_to_epoll(priv->cmn.epoll_fd, + return libj1939_add_socket_to_epoll(priv->cmn.epoll_fd, STDIN_FILENO, EPOLLIN); } @@ -407,7 +407,7 @@ static int isobusfs_cli_sock_ccm_prepare(struct isobusfs_priv *priv) struct sockaddr_can addr = priv->sockname; int ret; - ret = isobusfs_cmn_open_socket(); + ret = libj1939_open_socket(); if (ret < 0) return ret; @@ -419,7 +419,7 @@ static int isobusfs_cli_sock_ccm_prepare(struct isobusfs_priv *priv) /* TODO: this is TX only socket */ addr.can_addr.j1939.pgn = J1939_NO_PGN; - ret = isobusfs_cmn_bind_socket(priv->sock_ccm, &addr); + ret = libj1939_bind_socket(priv->sock_ccm, &addr); if (ret < 0) return ret; @@ -427,7 +427,7 @@ static int isobusfs_cli_sock_ccm_prepare(struct isobusfs_priv *priv) if (ret < 0) return ret; - ret = isobusfs_cmn_socket_prio(priv->sock_ccm, ISOBUSFS_PRIO_DEFAULT); + ret = libj1939_socket_prio(priv->sock_ccm, ISOBUSFS_PRIO_DEFAULT); if (ret < 0) return ret; @@ -436,7 +436,7 @@ static int isobusfs_cli_sock_ccm_prepare(struct isobusfs_priv *priv) 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, + return libj1939_add_socket_to_epoll(priv->cmn.epoll_fd, priv->sock_ccm, EPOLLERR); } @@ -445,23 +445,23 @@ static int isobusfs_cli_sock_nack_prepare(struct isobusfs_priv *priv) struct sockaddr_can addr = priv->sockname; int ret; - ret = isobusfs_cmn_open_socket(); + ret = libj1939_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); + ret = libj1939_bind_socket(priv->sock_nack, &addr); if (ret < 0) return ret; - ret = isobusfs_cmn_socket_prio(priv->sock_nack, ISOBUSFS_PRIO_ACK); + ret = libj1939_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, + return libj1939_add_socket_to_epoll(priv->cmn.epoll_fd, priv->sock_nack, EPOLLIN); } @@ -471,7 +471,7 @@ static int isobusfs_cli_sock_bcast_prepare(struct isobusfs_priv *priv) struct sockaddr_can addr = priv->sockname; int ret; - ret = isobusfs_cmn_open_socket(); + ret = libj1939_open_socket(); if (ret < 0) return ret; @@ -481,11 +481,11 @@ static int isobusfs_cli_sock_bcast_prepare(struct isobusfs_priv *priv) 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); + ret = libj1939_bind_socket(priv->sock_bcast_rx, &addr); if (ret < 0) return ret; - ret = isobusfs_cmn_set_broadcast(priv->sock_bcast_rx); + ret = libj1939_set_broadcast(priv->sock_bcast_rx); if (ret < 0) return ret; @@ -493,7 +493,7 @@ static int isobusfs_cli_sock_bcast_prepare(struct isobusfs_priv *priv) if (ret < 0) return ret; - return isobusfs_cmn_add_socket_to_epoll(priv->cmn.epoll_fd, priv->sock_bcast_rx, + return libj1939_add_socket_to_epoll(priv->cmn.epoll_fd, priv->sock_bcast_rx, EPOLLIN); } @@ -501,7 +501,7 @@ static int isobusfs_cli_sock_prepare(struct isobusfs_priv *priv) { int ret; - ret = isobusfs_cmn_create_epoll(); + ret = libj1939_create_epoll(); if (ret < 0) return ret; @@ -647,8 +647,8 @@ int main(int argc, char *argv[]) bzero(priv, sizeof(*priv)); - isobusfs_init_sockaddr_can(&priv->sockname, J1939_NO_PGN); - isobusfs_init_sockaddr_can(&priv->peername, ISOBUSFS_PGN_CL_TO_FS); + libj1939_init_sockaddr_can(&priv->sockname, J1939_NO_PGN); + libj1939_init_sockaddr_can(&priv->peername, ISOBUSFS_PGN_CL_TO_FS); ret = isobusfs_cli_parse_args(priv, argc, argv); if (ret) diff --git a/isobusfs/isobusfs_cli.h b/isobusfs/isobusfs_cli.h index 71d375d..6689c6c 100644 --- a/isobusfs/isobusfs_cli.h +++ b/isobusfs/isobusfs_cli.h @@ -103,7 +103,7 @@ struct isobusfs_priv { struct isobusfs_buf_log tx_buf_log; enum isobusfs_cli_state state; - struct isobusfs_cmn cmn; + struct libj1939_cmn cmn; uint8_t handle; uint32_t read_offset; diff --git a/isobusfs/isobusfs_cmn.c b/isobusfs/isobusfs_cmn.c index a6c7c96..4481407 100644 --- a/isobusfs/isobusfs_cmn.c +++ b/isobusfs/isobusfs_cmn.c @@ -125,29 +125,6 @@ void isobusfs_log_level_set(log_level_t level) log_level = level; } -int isobusfs_get_timeout_ms(struct timespec *ts) -{ - struct timespec curr_time; - int64_t time_diff; - int timeout_ms; - - clock_gettime(CLOCK_MONOTONIC, &curr_time); - time_diff = timespec_diff_ms(ts, &curr_time); - if (time_diff < 0) { - /* Too late to send next message. Send it now */ - timeout_ms = 0; - } else { - if (time_diff > INT_MAX) { - warn("timeout too long: %" PRId64 " ms", time_diff); - time_diff = INT_MAX; - } - - timeout_ms = time_diff; - } - - return timeout_ms; -} - const char *isobusfs_error_to_str(enum isobusfs_error err) { switch (err) { @@ -238,15 +215,6 @@ enum isobusfs_error linux_error_to_isobusfs_error(int linux_err) } } - -void isobusfs_init_sockaddr_can(struct sockaddr_can *sac, uint32_t pgn) -{ - sac->can_family = AF_CAN; - sac->can_addr.j1939.addr = J1939_NO_ADDR; - sac->can_addr.j1939.name = J1939_NO_NAME; - sac->can_addr.j1939.pgn = pgn; -} - static void isobusfs_print_timestamp(struct isobusfs_err_msg *emsg, const char *name, struct timespec *cur) { @@ -520,28 +488,6 @@ int isobusfs_send(int sock, const void *data, size_t len, return 0; } -/** - * isobusfs_cmn_open_socket - Open a CAN J1939 socket - * - * This function opens a CAN J1939 socket and returns the file descriptor - * on success. In case of an error, the function returns the negative - * error code. - */ -int isobusfs_cmn_open_socket(void) -{ - int ret; - - /* Create a new CAN J1939 socket */ - ret = socket(PF_CAN, SOCK_DGRAM, CAN_J1939); - if (ret < 0) { - /* Get the error code and print an error message */ - ret = -errno; - pr_err("socket(j1939): %d (%s)", ret, strerror(ret)); - return ret; - } - return ret; -} - /** * isobusfs_cmn_configure_socket_filter - Configure a J1939 socket filter * @sock: Socket file descriptor @@ -650,47 +596,6 @@ int isobusfs_cmn_configure_error_queue(int sock) return 0; } -/** - * isobusfs_cmn_bind_socket - Bind a J1939 socket to a given address - * @sock: socket file descriptor - * @addr: pointer to a sockaddr_can structure containing the address - * information to bind the socket to - * - * This function binds a J1939 socket to the specified address. It returns - * 0 on successful binding or a negative error code on failure. - * - * Return: 0 on success, or a negative error code on failure. - */ -int isobusfs_cmn_bind_socket(int sock, struct sockaddr_can *addr) -{ - int ret; - - ret = bind(sock, (void *)addr, sizeof(*addr)); - if (ret < 0) { - ret = -errno; - pr_err("failed to bind: %d (%s)", ret, strerror(ret)); - return ret; - } - - return 0; -} - -int isobusfs_cmn_socket_prio(int sock, int prio) -{ - int ret; - - ret = setsockopt(sock, SOL_CAN_J1939, SO_J1939_SEND_PRIO, - &prio, sizeof(prio)); - if (ret < 0) { - ret = -errno; - pr_warn("Failed to set priority %i. Error %i (%s)", prio, ret, - strerror(ret)); - return ret; - } - - return 0; -} - int isobusfs_cmn_connect_socket(int sock, struct sockaddr_can *addr) { int ret; @@ -705,32 +610,6 @@ int isobusfs_cmn_connect_socket(int sock, struct sockaddr_can *addr) return 0; } -/** - * isobusfs_cmn_set_broadcast - Enable broadcast option for a socket - * @sock: socket file descriptor - * - * This function enables the SO_BROADCAST option for the given socket, - * allowing it to send and receive broadcast messages. It returns 0 on success - * or a negative error code on failure. - * - * Return: 0 on success, or a negative error code on failure. - */ -int isobusfs_cmn_set_broadcast(int sock) -{ - int broadcast = true; - int ret; - - ret = setsockopt(sock, SOL_SOCKET, SO_BROADCAST, &broadcast, - sizeof(broadcast)); - if (ret < 0) { - ret = -errno; - pr_err("setsockopt(SO_BROADCAST): %d (%s)", ret, strerror(ret)); - return ret; - } - - return 0; -} - /* FIXME: linger is currently not supported by the kernel J1939 stack * but it would be nice to have it. Especially if we wont to stop sending * messages on a socket when the connection is lost. @@ -753,70 +632,6 @@ int isobusfs_cmn_set_linger(int sock) return 0; } -int isobusfs_cmn_add_socket_to_epoll(int epoll_fd, int sock, uint32_t events) -{ - struct epoll_event ev = {0}; - int ret; - - ev.events = events; - ev.data.fd = sock; - - ret = epoll_ctl(epoll_fd, EPOLL_CTL_ADD, sock, &ev); - if (ret < 0) { - ret = errno; - pr_err("epoll_ctl(EPOLL_CTL_ADD): %d (%s)", ret, strerror(ret)); - return ret; - } - - return 0; -} - -int isobusfs_cmn_create_epoll(void) -{ - int ret, epoll_fd; - - epoll_fd = epoll_create1(0); - if (epoll_fd < 0) { - ret = -errno; - pr_err("epoll_create1: %d (%s)", ret, strerror(ret)); - return ret; - } - - return epoll_fd; -} - -int isobusfs_cmn_prepare_for_events(struct isobusfs_cmn *cmn, int *nfds, - bool dont_wait) -{ - int ret, timeout_ms; - - if (dont_wait) - timeout_ms = 0; - else - timeout_ms = isobusfs_get_timeout_ms(&cmn->next_send_time); - - ret = epoll_wait(cmn->epoll_fd, cmn->epoll_events, - cmn->epoll_events_size, timeout_ms); - if (ret < 0) { - ret = -errno; - if (ret != -EINTR) { - *nfds = 0; - return ret; - } - } - - *nfds = ret; - - ret = clock_gettime(CLOCK_MONOTONIC, &cmn->last_time); - if (ret < 0) { - ret = -errno; - pr_err("failed to get time: %i (%s)", ret, strerror(ret)); - return ret; - } - - return 0; -} - void isobusfs_cmn_dump_last_x_bytes(const uint8_t *buffer, size_t buffer_size, size_t x) { diff --git a/isobusfs/isobusfs_cmn.h b/isobusfs/isobusfs_cmn.h index 7373ea3..23fa582 100644 --- a/isobusfs/isobusfs_cmn.h +++ b/isobusfs/isobusfs_cmn.h @@ -257,15 +257,6 @@ struct isobusfs_err_msg { struct isobusfs_stats *stats; }; -struct isobusfs_cmn { - int epoll_fd; - struct epoll_event *epoll_events; - size_t epoll_events_size; - struct timespec next_send_time; - struct timespec last_time; -}; - -void isobusfs_init_sockaddr_can(struct sockaddr_can *sac, uint32_t pgn); int isobusfs_recv_err(int sock, struct isobusfs_err_msg *emsg); /* @@ -300,7 +291,6 @@ static inline uint8_t isobusfs_cg_function_to_buf(enum isobusfs_cg cg, const char *isobusfs_error_to_str(enum isobusfs_error err); enum isobusfs_error linux_error_to_isobusfs_error(int linux_err); -int isobusfs_get_timeout_ms(struct timespec *ts); void isobusfs_send_nack(int sock, struct isobusfs_msg *msg); void isobufs_store_tx_data(struct isobusfs_buf_log *buffer, uint8_t *data); void isobusfs_dump_tx_data(const struct isobusfs_buf_log *buffer); @@ -313,20 +303,11 @@ int isobusfs_send(int sock, const void *data, size_t len, void isobusfs_cmn_dump_last_x_bytes(const uint8_t *buffer, size_t buffer_size, size_t x); -int isobusfs_cmn_open_socket(void); int isobusfs_cmn_configure_socket_filter(int sock, pgn_t pgn); int isobusfs_cmn_configure_error_queue(int sock); -int isobusfs_cmn_bind_socket(int sock, struct sockaddr_can *addr); int isobusfs_cmn_connect_socket(int sock, struct sockaddr_can *addr); -int isobusfs_cmn_set_broadcast(int sock); -int isobusfs_cmn_add_socket_to_epoll(int epoll_fd, int sock, uint32_t events); -int isobusfs_cmn_create_epoll(void); -int isobusfs_cmn_socket_prio(int sock, int prio); int isobusfs_cmn_set_linger(int sock); -int isobusfs_cmn_prepare_for_events(struct isobusfs_cmn *cmn, int *nfds, - bool dont_wait); - /* ============ directory handling ============ */ int isobusfs_cmn_dh_validate_dir_path(const char *path, bool writable); diff --git a/isobusfs/isobusfs_srv.c b/isobusfs/isobusfs_srv.c index 9429dbb..479182e 100644 --- a/isobusfs/isobusfs_srv.c +++ b/isobusfs/isobusfs_srv.c @@ -250,7 +250,7 @@ static int isobusfs_srv_process_events_and_tasks(struct isobusfs_srv_priv *priv) { int ret, nfds; - ret = isobusfs_cmn_prepare_for_events(&priv->cmn, &nfds, false); + ret = libj1939_prepare_for_events(&priv->cmn, &nfds, false); if (ret) return ret; @@ -268,7 +268,7 @@ static int isobusfs_srv_sock_fss_prepare(struct isobusfs_srv_priv *priv) struct sockaddr_can addr = priv->addr; int ret; - ret = isobusfs_cmn_open_socket(); + ret = libj1939_open_socket(); if (ret < 0) return ret; @@ -283,11 +283,11 @@ static int isobusfs_srv_sock_fss_prepare(struct isobusfs_srv_priv *priv) * PGN? */ addr.can_addr.j1939.pgn = ISOBUSFS_PGN_CL_TO_FS; - ret = isobusfs_cmn_bind_socket(priv->sock_fss, &addr); + ret = libj1939_bind_socket(priv->sock_fss, &addr); if (ret < 0) return ret; - ret = isobusfs_cmn_set_broadcast(priv->sock_fss); + ret = libj1939_set_broadcast(priv->sock_fss); if (ret < 0) return ret; @@ -295,7 +295,7 @@ static int isobusfs_srv_sock_fss_prepare(struct isobusfs_srv_priv *priv) if (ret < 0) return ret; - ret = isobusfs_cmn_socket_prio(priv->sock_fss, ISOBUSFS_PRIO_FSS); + ret = libj1939_socket_prio(priv->sock_fss, ISOBUSFS_PRIO_FSS); if (ret < 0) return ret; @@ -308,7 +308,7 @@ static int isobusfs_srv_sock_fss_prepare(struct isobusfs_srv_priv *priv) 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_fss, + return libj1939_add_socket_to_epoll(priv->cmn.epoll_fd, priv->sock_fss, EPOLLERR); } @@ -317,7 +317,7 @@ static int isobusfs_srv_sock_in_prepare(struct isobusfs_srv_priv *priv) struct sockaddr_can addr = priv->addr; int ret; - ret = isobusfs_cmn_open_socket(); + ret = libj1939_open_socket(); if (ret < 0) return ret; @@ -325,12 +325,12 @@ static int isobusfs_srv_sock_in_prepare(struct isobusfs_srv_priv *priv) /* keep address and name and overwrite PGN */ addr.can_addr.j1939.pgn = ISOBUSFS_PGN_CL_TO_FS; - ret = isobusfs_cmn_bind_socket(priv->sock_in, &addr); + ret = libj1939_bind_socket(priv->sock_in, &addr); if (ret < 0) return ret; - return isobusfs_cmn_add_socket_to_epoll(priv->cmn.epoll_fd, priv->sock_in, - EPOLLIN); + return libj1939_add_socket_to_epoll(priv->cmn.epoll_fd, priv->sock_in, + EPOLLIN); } static int isobusfs_srv_sock_nack_prepare(struct isobusfs_srv_priv *priv) @@ -338,24 +338,24 @@ static int isobusfs_srv_sock_nack_prepare(struct isobusfs_srv_priv *priv) struct sockaddr_can addr = priv->addr; int ret; - ret = isobusfs_cmn_open_socket(); + ret = libj1939_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); + ret = libj1939_bind_socket(priv->sock_nack, &addr); if (ret < 0) return ret; - ret = isobusfs_cmn_socket_prio(priv->sock_nack, ISOBUSFS_PRIO_ACK); + ret = libj1939_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); + return libj1939_add_socket_to_epoll(priv->cmn.epoll_fd, + priv->sock_nack, EPOLLIN); } /** @@ -370,7 +370,7 @@ static int isobusfs_srv_sock_prepare(struct isobusfs_srv_priv *priv) { int ret; - ret = isobusfs_cmn_create_epoll(); + ret = libj1939_create_epoll(); if (ret < 0) return ret; @@ -762,7 +762,7 @@ int main(int argc, char *argv[]) memset(priv, 0, sizeof(*priv)); /* Initialize sockaddr_can with a non-configurable PGN */ - isobusfs_init_sockaddr_can(&priv->addr, J1939_NO_PGN); + libj1939_init_sockaddr_can(&priv->addr, J1939_NO_PGN); priv->server_version = ISOBUSFS_SRV_VERSION; /* Parse command line arguments */ diff --git a/isobusfs/isobusfs_srv.h b/isobusfs/isobusfs_srv.h index 7389afb..557348e 100644 --- a/isobusfs/isobusfs_srv.h +++ b/isobusfs/isobusfs_srv.h @@ -103,7 +103,7 @@ struct isobusfs_srv_priv { int clients_count; struct isobusfs_buf_log tx_buf_log; - struct isobusfs_cmn cmn; + struct libj1939_cmn cmn; struct isobusfs_srv_volume volumes[ISOBUSFS_SRV_MAX_VOLUMES]; int volume_count; diff --git a/isobusfs/isobusfs_srv_cm.c b/isobusfs/isobusfs_srv_cm.c index 9a9fac8..b5b8d1e 100644 --- a/isobusfs/isobusfs_srv_cm.c +++ b/isobusfs/isobusfs_srv_cm.c @@ -213,14 +213,14 @@ static int isobusfs_srv_init_client(struct isobusfs_srv_priv *priv, return -EINVAL; } - ret = isobusfs_cmn_open_socket(); + ret = libj1939_open_socket(); if (ret < 0) return ret; client->sock = ret; addr.can_addr.j1939.pgn = ISOBUSFS_PGN_CL_TO_FS; - ret = isobusfs_cmn_bind_socket(client->sock, &addr); + ret = libj1939_bind_socket(client->sock, &addr); if (ret < 0) return ret; @@ -243,7 +243,7 @@ static int isobusfs_srv_init_client(struct isobusfs_srv_priv *priv, goto close_socket; } - ret = isobusfs_cmn_socket_prio(client->sock, ISOBUSFS_PRIO_DEFAULT); + ret = libj1939_socket_prio(client->sock, ISOBUSFS_PRIO_DEFAULT); if (ret < 0) return ret; diff --git a/j1939cat.c b/j1939cat.c index 50fe5da..23715ea 100644 --- a/j1939cat.c +++ b/j1939cat.c @@ -97,15 +97,6 @@ static const char help_msg[] = static const char optstring[] = "?hi:vs:rp:P:R:B"; - -static void j1939cat_init_sockaddr_can(struct sockaddr_can *sac) -{ - sac->can_family = AF_CAN; - sac->can_addr.j1939.addr = J1939_NO_ADDR; - sac->can_addr.j1939.name = J1939_NO_NAME; - sac->can_addr.j1939.pgn = J1939_NO_PGN; -} - static ssize_t j1939cat_send_one(struct j1939cat_priv *priv, int out_fd, const void *buf, size_t buf_size) { @@ -766,8 +757,8 @@ int main(int argc, char *argv[]) priv->polltimeout = 100000; priv->repeat = 1; - j1939cat_init_sockaddr_can(&priv->sockname); - j1939cat_init_sockaddr_can(&priv->peername); + 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) diff --git a/libj1939.c b/libj1939.c index 506b525..ed9a554 100644 --- a/libj1939.c +++ b/libj1939.c @@ -11,19 +11,24 @@ */ #include +#include +#include #include +#include +#include +#include +#include #include #include #include - -#include -#include +#include #include +#include +#include #include -#include - #include "libj1939.h" +#include "lib.h" /* static data */ static struct if_nameindex *saved; @@ -195,3 +200,232 @@ const char *libj1939_addr2str(const struct sockaddr_can *can) return buf; } +void libj1939_init_sockaddr_can(struct sockaddr_can *sac, uint32_t pgn) +{ + sac->can_family = AF_CAN; + sac->can_addr.j1939.addr = J1939_NO_ADDR; + sac->can_addr.j1939.name = J1939_NO_NAME; + sac->can_addr.j1939.pgn = pgn; +} + +/** + * libj1939_open_socket - Open a new J1939 socket + * + * This function opens a new J1939 socket. + * + * Return: The file descriptor of the new socket, or a negative error code. + */ +int libj1939_open_socket(void) +{ + int ret; + + /* Create a new CAN J1939 socket */ + ret = socket(PF_CAN, SOCK_DGRAM, CAN_J1939); + if (ret < 0) { + /* Get the error code and print an error message */ + ret = -errno; + pr_err("socket(j1939): %d (%s)", ret, strerror(ret)); + return ret; + } + return ret; +} + +/** + * libj1939_bind_socket - Bind a J1939 socket to a specific address + * @sock: The file descriptor of the socket + * @addr: The address to bind to + * + * This function binds a J1939 socket to a specific address. + * + * Return: 0 on success, or a negative error code. + */ +int libj1939_bind_socket(int sock, struct sockaddr_can *addr) +{ + int ret; + + ret = bind(sock, (void *)addr, sizeof(*addr)); + if (ret < 0) { + ret = -errno; + pr_err("failed to bind: %d (%s)", ret, strerror(ret)); + return ret; + } + + return 0; +} + +/** + * libj1939_socket_prio - Set the priority of a J1939 socket + * @sock: The file descriptor of the socket + * @prio: The priority to set + * + * This function sets the priority of a J1939 socket. + * + * Return: 0 on success, or a negative error code. + */ +int libj1939_socket_prio(int sock, int prio) +{ + int ret; + + ret = setsockopt(sock, SOL_CAN_J1939, SO_J1939_SEND_PRIO, + &prio, sizeof(prio)); + if (ret < 0) { + ret = -errno; + pr_warn("Failed to set priority %i. Error %i (%s)", prio, ret, + strerror(ret)); + return ret; + } + + return 0; +} + +/** + * libj1939_set_broadcast - Enable broadcast on a J1939 socket + * @sock: The file descriptor of the socket + * + * This function enables broadcast on a J1939 socket. + * + * Return: 0 on success, or a negative error code. + */ +int libj1939_set_broadcast(int sock) +{ + int broadcast = true; + int ret; + + ret = setsockopt(sock, SOL_SOCKET, SO_BROADCAST, &broadcast, + sizeof(broadcast)); + if (ret < 0) { + ret = -errno; + pr_err("setsockopt(SO_BROADCAST): %d (%s)", ret, strerror(ret)); + return ret; + } + + return 0; +} + +/** + * libj1939_add_socket_to_epoll - Add a socket to an epoll instance + * @epoll_fd: The file descriptor of the epoll instance + * @sock: The file descriptor of the socket + * @events: The events to monitor + * + * This function adds a socket to an epoll instance. + * + * Return: 0 on success, or a negative error code. + */ +int libj1939_add_socket_to_epoll(int epoll_fd, int sock, uint32_t events) +{ + struct epoll_event ev = {0}; + int ret; + + ev.events = events; + ev.data.fd = sock; + + ret = epoll_ctl(epoll_fd, EPOLL_CTL_ADD, sock, &ev); + if (ret < 0) { + ret = errno; + pr_err("epoll_ctl(EPOLL_CTL_ADD): %d (%s)", ret, strerror(ret)); + return ret; + } + + return 0; +} + +/** + * libj1939_create_epoll - Create a new epoll instance + * + * This function creates a new epoll instance. + * + * Return: The file descriptor of the new epoll instance, or a negative error + * code. + */ +int libj1939_create_epoll(void) +{ + int ret, epoll_fd; + + epoll_fd = epoll_create1(0); + if (epoll_fd < 0) { + ret = -errno; + pr_err("epoll_create1: %d (%s)", ret, strerror(ret)); + return ret; + } + + return epoll_fd; +} + +/** + * libj1939_get_timeout_ms - Get the timeout in milliseconds until a specific + * time + * @ts: The time to wait for + * @return: The timeout in milliseconds until the specified time + * + * This function calculates the timeout in milliseconds until a specific time. + * + * Return: The timeout in milliseconds until the specified time. + */ +static int libj1939_get_timeout_ms(struct timespec *ts) +{ + struct timespec curr_time; + int64_t time_diff; + int timeout_ms; + + clock_gettime(CLOCK_MONOTONIC, &curr_time); + time_diff = timespec_diff_ms(ts, &curr_time); + if (time_diff < 0) { + /* Too late to send next message. Send it now */ + timeout_ms = 0; + } else { + if (time_diff > INT_MAX) { + pr_warn("timeout too long: %" PRId64 " ms", time_diff); + time_diff = INT_MAX; + } + + timeout_ms = time_diff; + } + + return timeout_ms; +} + +/** + * libj1939_prepare_for_events - Prepare and wait for events on an epoll + * @cmn: The common J1939 instance data + * @nfds: The number of file descriptors that are ready + * @dont_wait: Don't wait for events, just check if there are any + * + * This function calculates the timeout until the next message should be sent + * or any other event should be handled, prepares the epoll instance for events + * by waiting for the specified timeout or until an event occurs, and waits for + * events on the epoll instance. + * + * Return: 0 on success, or a negative error code. + */ +int libj1939_prepare_for_events(struct libj1939_cmn *cmn, int *nfds, + bool dont_wait) +{ + int ret, timeout_ms; + + if (dont_wait) + timeout_ms = 0; + else + timeout_ms = libj1939_get_timeout_ms(&cmn->next_send_time); + + ret = epoll_wait(cmn->epoll_fd, cmn->epoll_events, + cmn->epoll_events_size, timeout_ms); + if (ret < 0) { + ret = -errno; + if (ret != -EINTR) { + *nfds = 0; + return ret; + } + } + + *nfds = ret; + + ret = clock_gettime(CLOCK_MONOTONIC, &cmn->last_time); + if (ret < 0) { + ret = -errno; + pr_err("failed to get time: %i (%s)", ret, strerror(ret)); + return ret; + } + + return 0; +} diff --git a/libj1939.h b/libj1939.h index b6832cf..44393a2 100644 --- a/libj1939.h +++ b/libj1939.h @@ -13,9 +13,11 @@ /* needed on some 64 bit platforms to get consistent 64-bit types */ #define __SANE_USERSPACE_TYPES__ -#include #include #include +#include +#include +#include #ifndef J1939_LIB_H #define J1939_LIB_H @@ -24,10 +26,30 @@ extern "C" { #endif +struct libj1939_cmn { + int epoll_fd; + struct epoll_event *epoll_events; + size_t epoll_events_size; + struct timespec next_send_time; + struct timespec last_time; +}; + void libj1939_parse_canaddr(char *spec, struct sockaddr_can *paddr); extern int libj1939_str2addr(const char *str, char **endp, struct sockaddr_can *can); extern const char *libj1939_addr2str(const struct sockaddr_can *can); +void libj1939_init_sockaddr_can(struct sockaddr_can *sac, uint32_t pgn); + +int libj1939_open_socket(void); +int libj1939_bind_socket(int sock, struct sockaddr_can *addr); +int libj1939_socket_prio(int sock, int prio); +int libj1939_set_broadcast(int sock); +int libj1939_add_socket_to_epoll(int epoll_fd, int sock, uint32_t events); +int libj1939_create_epoll(void); + +int libj1939_prepare_for_events(struct libj1939_cmn *cmn, int *nfds, + bool dont_wait); + #ifdef __cplusplus } #endif From d0b04bd45633f258ae7b71fa1753867110032b07 Mon Sep 17 00:00:00 2001 From: Oleksij Rempel Date: Wed, 15 May 2024 19:43:23 +0200 Subject: [PATCH 3/3] add j1939 datetime Implement client and server side for SAE J1939-71:2002 - 5.3 pgn65254 - Time/Date - TD. Testing: ./j1939-timedate-srv -i vcan0 -a 0x70 & ./j1939-timedate-cli -i vcan0 -a 0x80 -r 0xff Signed-off-by: Oleksij Rempel --- CMakeLists.txt | 31 ++ j1939_timedate/j1939_timedate_cli.c | 500 ++++++++++++++++++++++++++++ j1939_timedate/j1939_timedate_cmn.h | 84 +++++ j1939_timedate/j1939_timedate_srv.c | 450 +++++++++++++++++++++++++ 4 files changed, 1065 insertions(+) create mode 100644 j1939_timedate/j1939_timedate_cli.c create mode 100644 j1939_timedate/j1939_timedate_cmn.h create mode 100644 j1939_timedate/j1939_timedate_srv.c diff --git a/CMakeLists.txt b/CMakeLists.txt index a5e5c30..497f91f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,6 +62,11 @@ set(PROGRAMS_J1939 testj1939 ) +set(PROGRAMS_J1939_TIMEDATE + j1939-timedate-srv + j1939-timedate-cli +) + set(PROGRAMS_ISOBUSFS isobusfs-srv isobusfs-cli @@ -159,6 +164,32 @@ if(NOT ANDROID) isobusfs-cli isobusfs-srv DESTINATION ${CMAKE_INSTALL_BINDIR}) + + set(PUBLIC_HEADER_j1939_TIMEDATE + j1939_timedate/j1939_timedate_cmn.h + ) + + add_executable(j1939-timedate-cli + j1939_timedate/j1939_timedate_cli.c + ) + + target_link_libraries(j1939-timedate-cli + PRIVATE can j1939 + ) + + add_executable(j1939-timedate-srv + j1939_timedate/j1939_timedate_srv.c + ) + + target_link_libraries(j1939-timedate-srv + PRIVATE can j1939 + ) + + install(TARGETS + j1939-timedate-cli + j1939-timedate-srv + DESTINATION ${CMAKE_INSTALL_BINDIR}) + endif() add_library(can STATIC diff --git a/j1939_timedate/j1939_timedate_cli.c b/j1939_timedate/j1939_timedate_cli.c new file mode 100644 index 0000000..7e4b80f --- /dev/null +++ b/j1939_timedate/j1939_timedate_cli.c @@ -0,0 +1,500 @@ +// SPDX-License-Identifier: LGPL-2.0-only +// SPDX-FileCopyrightText: 2024 Oleksij Rempel + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "j1939_timedate_cmn.h" + +#define J1939_TIMEDATE_CLI_MAX_EPOLL_EVENTS 10 + +struct j1939_timedate_cli_priv { + int sock_nack; + int sock_main; + + struct sockaddr_can sockname; + struct sockaddr_can peername; + + struct j1939_timedate_stats stats; + + struct libj1939_cmn cmn; + struct timespec wait_until_time; + + bool utc; + bool broadcast; + bool done; +}; + +static void print_time_date_packet(struct j1939_timedate_cli_priv *priv, + const struct j1939_timedate_msg *msg) +{ + const struct j1939_time_date_packet *tdp = + (const struct j1939_time_date_packet *)msg->buf; + char timezone_offset[] = "+00:00 (Local Time)"; + char time_buffer[64]; + int actual_hour, actual_minute; + int actual_year, actual_month; + double actual_seconds; + double actual_day; + + if (msg->len < sizeof(*tdp)) { + pr_warn("received too short time and date packet: %zi", + msg->len); + return; + } + + actual_year = 1985 + tdp->year; + actual_month = tdp->month; + actual_day = tdp->day * 0.25; + actual_hour = tdp->hours; + actual_minute = tdp->minutes; + actual_seconds = tdp->seconds * 0.25; + + if (tdp->local_hour_offset == 125) { + snprintf(timezone_offset, sizeof(timezone_offset), + "+00:00 (Local Time)"); + } else if (!priv->utc) { + actual_hour += tdp->local_hour_offset; + actual_minute += tdp->local_minute_offset; + + /* Wraparound for hours and minutes if needed */ + if (actual_minute >= 60) { + actual_minute -= 60; + actual_hour++; + } else if (actual_minute < 0) { + actual_minute += 60; + actual_hour--; + } + + if (actual_hour >= 24) { + actual_hour -= 24; + actual_day++; + } else if (actual_hour < 0) { + actual_hour += 24; + actual_day--; + } + + snprintf(timezone_offset, sizeof(timezone_offset), "%+03d:%02d", + tdp->local_hour_offset, abs(tdp->local_minute_offset)); + } else { + snprintf(timezone_offset, sizeof(timezone_offset), + "+00:00 (UTC)"); + } + + snprintf(time_buffer, sizeof(time_buffer), + "%d-%02d-%02.0f %02d:%02d:%05.2f%.20s", + actual_year, actual_month, actual_day, actual_hour, + actual_minute, actual_seconds, timezone_offset); + + printf("SA: 0x%02X, NAME: 0x%016llX, Time: %s\n", + msg->peername.can_addr.j1939.addr, + msg->peername.can_addr.j1939.name, time_buffer); + + if (!priv->broadcast) + priv->done = true; +} + +static int j1939_timedate_cli_rx_buf(struct j1939_timedate_cli_priv *priv, + struct j1939_timedate_msg *msg) +{ + pgn_t pgn = msg->peername.can_addr.j1939.pgn; + int ret = 0; + + switch (pgn) { + case J1939_PGN_TD: + print_time_date_packet(priv, msg); + break; + default: + pr_warn("%s: unsupported PGN: %x", __func__, pgn); + /* Not a critical error */ + break; + } + + return ret; +} + +static int j1939_timedate_cli_rx_one(struct j1939_timedate_cli_priv *priv, + int sock) +{ + struct j1939_timedate_msg *msg; + int flags = 0; + int ret; + + msg = malloc(sizeof(*msg)); + if (!msg) { + pr_err("can't allocate rx msg struct\n"); + return -ENOMEM; + } + msg->buf_size = J1939_TIMEDATE_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 < 3) { + pr_warn("received too short message: %i", ret); + return -EINVAL; + } + + msg->len = ret; + + ret = j1939_timedate_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 j1939_timedate_cli_handle_events(struct j1939_timedate_cli_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->events & POLLIN) { + ret = j1939_timedate_cli_rx_one(priv, ev->data.fd); + if (ret) { + warn("recv one"); + return ret; + } + } + } + return 0; +} + +static int j1939_timedate_cli_process_events_and_tasks(struct j1939_timedate_cli_priv *priv) +{ + int ret, nfds; + + ret = libj1939_prepare_for_events(&priv->cmn, &nfds, false); + if (ret) + return ret; + + if (nfds > 0) { + ret = j1939_timedate_cli_handle_events(priv, nfds); + if (ret) + return ret; + } + + return 0; +} + +static int j1939_timedate_cli_send_req(struct j1939_timedate_cli_priv *priv) +{ + struct sockaddr_can addr = priv->peername; + uint8_t data[3] = {0}; + int ret; + + addr.can_addr.j1939.pgn = J1939_PGN_REQUEST_PGN; + + data[0] = J1939_PGN_TD & 0xff; + data[1] = (J1939_PGN_TD >> 8) & 0xff; + data[2] = (J1939_PGN_TD >> 16) & 0xff; + + ret = sendto(priv->sock_main, data, sizeof(data), 0, + (struct sockaddr *)&addr, sizeof(addr)); + if (ret == -1) { + ret = -errno; + pr_warn("failed to send data: %i (%s)", ret, strerror(ret)); + return ret; + } + + return 0; +} + +static int j1939_timedate_cli_sock_main_prepare(struct j1939_timedate_cli_priv *priv) +{ + struct sockaddr_can addr = priv->sockname; + int ret; + + ret = libj1939_open_socket(); + if (ret < 0) + return ret; + + priv->sock_main = ret; + + ret = libj1939_bind_socket(priv->sock_main, &addr); + if (ret < 0) + return ret; + + ret = libj1939_socket_prio(priv->sock_main, + J1939_TIMEDATE_PRIO_DEFAULT); + if (ret < 0) + return ret; + + ret = libj1939_set_broadcast(priv->sock_main); + if (ret < 0) + return ret; + + return libj1939_add_socket_to_epoll(priv->cmn.epoll_fd, + priv->sock_main, EPOLLIN); +} + +static int j1939_timedate_cli_sock_nack_prepare(struct j1939_timedate_cli_priv *priv) +{ + struct sockaddr_can addr = priv->sockname; + int ret; + + ret = libj1939_open_socket(); + if (ret < 0) + return ret; + + priv->sock_nack = ret; + + addr.can_addr.j1939.pgn = ISOBUS_PGN_ACK; + ret = libj1939_bind_socket(priv->sock_nack, &addr); + if (ret < 0) + return ret; + + return libj1939_add_socket_to_epoll(priv->cmn.epoll_fd, + priv->sock_nack, EPOLLIN); +} + +static int j1939_timedate_cli_sock_prepare(struct j1939_timedate_cli_priv *priv) +{ + int ret; + + ret = libj1939_create_epoll(); + if (ret < 0) + return ret; + + priv->cmn.epoll_fd = ret; + + priv->cmn.epoll_events = calloc(J1939_TIMEDATE_CLI_MAX_EPOLL_EVENTS, + sizeof(struct epoll_event)); + if (!priv->cmn.epoll_events) + return -ENOMEM; + + priv->cmn.epoll_events_size = J1939_TIMEDATE_CLI_MAX_EPOLL_EVENTS; + + ret = j1939_timedate_cli_sock_main_prepare(priv); + if (ret < 0) + return ret; + + return j1939_timedate_cli_sock_nack_prepare(priv); +} + +static void j1939_timedate_cli_print_help(void) +{ + printf("Usage: j1939_timedate-cli [options]\n"); + printf("Options:\n"); + printf(" --interface or -i \n"); + printf(" Specifies the CAN interface to use (mandatory).\n"); + printf(" --local-address or -a \n"); + printf(" Specifies the local address in hexadecimal (mandatory if local name is not provided).\n"); + printf(" --local-name or -n \n"); + printf(" Specifies the local NAME in hexadecimal (mandatory if local address is not provided).\n"); + printf(" --remote-address or -r \n"); + printf(" Specifies the remote address in hexadecimal (optional).\n"); + printf(" --remote-name or -m \n"); + printf(" Specifies the remote NAME in hexadecimal (optional).\n"); + printf(" --utc or -u\n"); + printf(" Outputs the time in UTC format.\n"); + printf("\n"); + printf("Note:\n"); + printf(" Local address and local name are mutually exclusive and one must be provided.\n"); + printf(" Remote address and remote name are mutually exclusive. \n"); + printf(" If no remote property is provided, the broadcast address will be used.\n"); + printf("\n"); + printf("Behavior:\n"); + printf(" In unicast mode (remote address or remote name provided),\n"); + printf(" the client will send a request and wait for the first response, then exit.\n"); + printf(" In broadcast mode (no remote address or remote name provided),\n"); + printf(" the program will wait 1000 milliseconds to collect responses, then exit.\n"); + printf("\n"); + printf("Time Output Formats:\n"); + printf(" YYYY-MM-DD HH:MM:SS.SS+00:00 (Local Time) - when no time zone information is received.\n"); + printf(" YYYY-MM-DD HH:MM:SS.SS+00:00 (UTC) - when the --utc option is used.\n"); + printf(" YYYY-MM-DD HH:MM:SS.SS+00:00 - default response with time zone offset automatically calculated.\n"); + printf("\n"); + printf("Complete Message Format:\n"); + printf(" The message will include the Source Address (SA) and J1939 NAME, formatted as follows:\n"); + printf(" SA: 0x60, NAME: 0x0000000000000000, Time: 2024-05-16 20:23:40.00+02:00\n"); + printf(" If the NAME is known, it will have a non-zero value.\n"); + printf("\n"); + printf("Usage Examples:\n"); + printf(" j1939acd -r 64-95 -c /tmp/1122334455667788.jacd 1122334455667788 vcan0 &\n"); + printf("\n"); + printf(" Broadcast mode:\n"); + printf(" j1939-timedate-cli -i vcan0 -a 0x80\n"); + printf("\n"); + printf(" Unicast mode:\n"); + printf(" j1939-timedate-cli -i vcan0 -a 0x80 -r 0x90\n"); + printf("\n"); + printf(" Using NAMEs instead of addresses:\n"); + printf(" j1939acd -r 64-95 -c /tmp/1122334455667788.jacd 1122334455667788 vcan0 &\n"); + printf(" j1939-timedate-cli -i vcan0 -n 0x1122334455667788\n"); +} + +static int j1939_timedate_cli_parse_args(struct j1939_timedate_cli_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 opt; + + static struct option long_options[] = { + {"interface", required_argument, 0, 'i'}, + {"local-address", required_argument, 0, 'a'}, + {"local-name", required_argument, 0, 'n'}, + {"remote-address", required_argument, 0, 'r'}, + {"remote-name", required_argument, 0, 'm'}, + {"utc", no_argument, 0, 'u'}, + {0, 0, 0, 0} + }; + + while ((opt = getopt_long(argc, argv, "a:n:r:m:i:u", 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 'u': + priv->utc = true; + break; + default: + j1939_timedate_cli_print_help(); + return -EINVAL; + } + } + + if (!interface_set) { + pr_err("interface not specified"); + j1939_timedate_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\n"); + j1939_timedate_cli_print_help(); + return -EINVAL; + } + + if (!local_address_set && !local_name_set) { + pr_err("Local address and local name not specified. One of them is required\n"); + j1939_timedate_cli_print_help(); + return -EINVAL; + } + + /* If no remote address is set, set it to broadcast */ + if (!remote_address_set && !remote_name_set) { + remote->can_addr.j1939.addr = J1939_NO_ADDR; + priv->broadcast = true; + } + + return 0; +} + +int main(int argc, char *argv[]) +{ + struct j1939_timedate_cli_priv *priv; + struct timespec ts; + int64_t time_diff; + int ret; + + priv = malloc(sizeof(*priv)); + if (!priv) + err(EXIT_FAILURE, "can't allocate priv"); + + bzero(priv, sizeof(*priv)); + + libj1939_init_sockaddr_can(&priv->sockname, J1939_PGN_TD); + libj1939_init_sockaddr_can(&priv->peername, J1939_PGN_REQUEST_PGN); + + ret = j1939_timedate_cli_parse_args(priv, argc, argv); + if (ret) + return ret; + + ret = j1939_timedate_cli_sock_prepare(priv); + if (ret) + return ret; + + clock_gettime(CLOCK_MONOTONIC, &ts); + priv->cmn.next_send_time = ts; + priv->wait_until_time = priv->cmn.next_send_time; + /* Wait one second to collect all responses by default */ + timespec_add_ms(&priv->wait_until_time, 1000); + + ret = j1939_timedate_cli_send_req(priv); + if (ret) + return ret; + + while (1) { + ret = j1939_timedate_cli_process_events_and_tasks(priv); + if (ret) + break; + + if (priv->done) + break; + + clock_gettime(CLOCK_MONOTONIC, &ts); + time_diff = timespec_diff_ms(&priv->wait_until_time, &ts); + if (time_diff < 0) + break; + } + + close(priv->cmn.epoll_fd); + free(priv->cmn.epoll_events); + + close(priv->sock_main); + close(priv->sock_nack); + + return ret; +} + diff --git a/j1939_timedate/j1939_timedate_cmn.h b/j1939_timedate/j1939_timedate_cmn.h new file mode 100644 index 0000000..ba1fca4 --- /dev/null +++ b/j1939_timedate/j1939_timedate_cmn.h @@ -0,0 +1,84 @@ +// SPDX-License-Identifier: LGPL-2.0-only +// SPDX-FileCopyrightText: 2024 Oleksij Rempel + +#ifndef _J1939_TIMEDATE_H_ +#define _J1939_TIMEDATE_H_ + +#include +#include +#include +#include + +#include +#include +#include "../libj1939.h" +#include "../lib.h" + +/* SAE J1939-71:2002 - 5.3 pgn54528 - Time/Date Adjust - TDA - */ +#define J1939_PGN_TDA 0x0d500 /* 54528 */ +/* SAE J1939-71:2002 - 5.3 pgn65254 - Time/Date - TD - */ +#define J1939_PGN_TD 0x0fee6 /* 65254 */ + +#define J1939_PGN_REQUEST_PGN 0x0ea00 /* 59904 */ + +/* ISO 11783-3:2018 - 5.4.5 Acknowledgment */ +#define ISOBUS_PGN_ACK 0x0e800 /* 59392 */ + +#define J1939_TIMEDATE_PRIO_DEFAULT 6 + +#define J1939_TIMEDATE_MAX_TRANSFER_LENGH 8 + +struct j1939_timedate_stats { + int err; + uint32_t tskey_sch; + uint32_t tskey_ack; + uint32_t send; +}; + +struct j1939_timedate_msg { + uint8_t buf[J1939_TIMEDATE_MAX_TRANSFER_LENGH]; + size_t buf_size; + ssize_t len; /* length of received message */ + struct sockaddr_can peername; + socklen_t peer_addr_len; + int sock; +}; + +struct j1939_timedate_err_msg { + struct sock_extended_err *serr; + struct scm_timestamping *tss; + struct j1939_timedate_stats *stats; +}; + +/* + * struct time_date_packet - Represents the PGN 65254 Time/Date packet + * + * @seconds: Seconds since the last minute (0-59) with a scaling factor, + * meaning each increment represents 0.25 seconds. + * @minutes: Minutes since the last hour (0-59) with no scaling. + * @hours: Hours since midnight (0-23) with no scaling. + * @month: Current month (1-12) with no scaling. + * @day: Day of the month with a scaling factor, each increment represents 0.25 + * day. + * @year: Year offset since 1985, each increment represents one year. + * @local_minute_offset: Offset in minutes from UTC, can range from -125 to 125 + * minutes. + * @local_hour_offset: Offset in hours from UTC, can range from -125 to 125 + * hours. + * + * This structure defines each component of the Time/Date as described in + * PGN 65254, using each byte to represent different components of the standard + * UTC time and optionally adjusted local time based on offsets. + */ +struct j1939_time_date_packet { + uint8_t seconds; + uint8_t minutes; + uint8_t hours; + uint8_t month; + uint8_t day; + uint8_t year; + int8_t local_minute_offset; + int8_t local_hour_offset; +}; + +#endif /* !_J1939_TIMEDATE_H_ */ diff --git a/j1939_timedate/j1939_timedate_srv.c b/j1939_timedate/j1939_timedate_srv.c new file mode 100644 index 0000000..d3aa53a --- /dev/null +++ b/j1939_timedate/j1939_timedate_srv.c @@ -0,0 +1,450 @@ +// SPDX-License-Identifier: LGPL-2.0-only +// SPDX-FileCopyrightText: 2024 Oleksij Rempel + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "j1939_timedate_cmn.h" + +#define J1939_TIMEDATE_SRV_MAX_EPOLL_EVENTS 10 + +struct j1939_timedate_srv_priv { + int sock_nack; + int sock_main; + + struct sockaddr_can sockname; + + struct j1939_timedate_stats stats; + + struct libj1939_cmn cmn; +}; + +static void gmtime_to_j1939_pgn_65254_td(struct j1939_time_date_packet *tdp) +{ + struct tm utc_tm_buf, local_tm_buf; + int hour_offset, minute_offset; + struct tm *utc_tm, *local_tm; + int year_since_1985; + time_t now; + + now = time(NULL); + utc_tm = gmtime_r(&now, &utc_tm_buf); + local_tm = localtime_r(&now, &local_tm_buf); + + /* Calculate the offsets */ + hour_offset = local_tm->tm_hour - utc_tm->tm_hour; + minute_offset = local_tm->tm_min - utc_tm->tm_min; + + /* Handle date rollover */ + if (local_tm->tm_mday != utc_tm->tm_mday) { + if (local_tm->tm_hour < 12) + hour_offset += 24; /* past midnight */ + else + hour_offset -= 24; /* before midnight */ + } + + /* + * Seconds (spn959): + * Resolution: 0.25 /bit, 0 offset + * Data Range: 0 to 250 (0 to 62.5 seconds) + * Operational Range: 0 to 239 (0 to 59.75 seconds) + */ + tdp->seconds = (uint8_t)(utc_tm->tm_sec / 0.25); + if (tdp->seconds > 239) + tdp->seconds = 239; + + /* + * Minutes (spn960): + * Resolution: 1 min/bit, 0 offset + * Data Range: 0 to 250 (0 to 250 minutes) + * Operational Range: 0 to 59 (0 to 59 minutes) + */ + tdp->minutes = (uint8_t)utc_tm->tm_min; + if (tdp->minutes > 59) + tdp->minutes = 59; + + /* + * Hours (spn961): + * Resolution: 1 hr/bit, 0 offset + * Data Range: 0 to 250 (0 to 250 hours) + * Operational Range: 0 to 23 (0 to 23 hours) + */ + tdp->hours = (uint8_t)utc_tm->tm_hour; + if (tdp->hours > 23) + tdp->hours = 23; + + /* + * Day (spn962): + * Resolution: 0.25 /bit, 0 offset + * Data Range: 0 to 250 (0 to 62.5 days) + * Operational Range: 1 to 127 (0.25 to 31.75 days) + */ + tdp->day = (uint8_t)(utc_tm->tm_mday / 0.25); + if (tdp->day < 1 || tdp->day > 127) + tdp->day = 1; + + /* + * Month (spn963): + * Resolution: 1 month/bit, 0 offset + * Data Range: 0 to 250 (0 to 250 months) + * Operational Range: 1 to 12 (1 to 12 months) + */ + tdp->month = (uint8_t)(utc_tm->tm_mon + 1); + if (tdp->month < 1 || tdp->month > 12) + tdp->month = 1; + + /* + * Year (spn964): + * Resolution: 1 year/bit, 1985 offset + * Data Range: 0 to 250 (1985 to 2235) + * Operational Range: 0 to 250 (1985 to 2235) + */ + year_since_1985 = utc_tm->tm_year - 85; + if (year_since_1985 < 0) { + /* Fallback to year 1985 if year is before 1985 */ + tdp->year = 0; + } else if (year_since_1985 > 250) { + /* Fallback to year 2235 if year is beyond 2235 */ + tdp->year = 250; + } else { + tdp->year = (uint8_t)year_since_1985; + } + + /* + * Local minute offset (spn1601): + * Resolution: 1 min/bit, -125 offset + * Data Range: -125 to 125 minutes + * Operational Range: -59 to 59 minutes + */ + tdp->local_minute_offset = (int8_t)minute_offset; + + /* + * Local hour offset (spn1602): + * Resolution: 1 hr/bit, -125 offset + * Data Range: -125 to +125 hours + * Operational Range: -24 to +23 hours + * Note: If the local hour offset parameter is equal to 125 (0xFA), + * the local time then the time parameter is the local time instead of + * UTC. + */ + tdp->local_hour_offset = (int8_t)hour_offset; +} + +static int j1939_timedate_srv_send_res(struct j1939_timedate_srv_priv *priv, + struct sockaddr_can *addr) +{ + struct sockaddr_can peername = *addr; + struct j1939_time_date_packet tdp; + int ret; + + gmtime_to_j1939_pgn_65254_td(&tdp); + + peername.can_addr.j1939.pgn = J1939_PGN_TD; + ret = sendto(priv->sock_main, &tdp, sizeof(tdp), 0, + (struct sockaddr *)&peername, sizeof(peername)); + if (ret == -1) { + ret = -errno; + pr_warn("failed to send data: %i (%s)", ret, strerror(ret)); + return ret; + } + + return 0; +} + +// check if the received message is a request for the time and date +static int j1939_timedate_srv_process_request(struct j1939_timedate_srv_priv *priv, + struct j1939_timedate_msg *msg) +{ + + if (msg->buf[0] != (J1939_PGN_TD & 0xff) || + msg->buf[1] != ((J1939_PGN_TD >> 8) & 0xff) || + msg->buf[2] != ((J1939_PGN_TD >> 16) & 0xff)) { + /* Don't care, not a time and date request */ + return 0; + } + + return j1939_timedate_srv_send_res(priv, &msg->peername); +} + +static int j1939_timedate_srv_rx_buf(struct j1939_timedate_srv_priv *priv, struct j1939_timedate_msg *msg) +{ + pgn_t pgn = msg->peername.can_addr.j1939.pgn; + int ret = 0; + + switch (pgn) { + case J1939_PGN_REQUEST_PGN: + ret = j1939_timedate_srv_process_request(priv, msg); + break; + default: + pr_warn("%s: unsupported PGN: %x", __func__, pgn); + /* Not a critical error */ + break; + } + + return ret; +} + +static int j1939_timedate_srv_rx_one(struct j1939_timedate_srv_priv *priv, int sock) +{ + struct j1939_timedate_msg *msg; + int flags = 0; + int ret; + + msg = malloc(sizeof(*msg)); + if (!msg) { + pr_err("can't allocate rx msg struct\n"); + return -ENOMEM; + } + msg->buf_size = J1939_TIMEDATE_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 < 3) { + pr_warn("received too short message: %i", ret); + return -EINVAL; + } + + msg->len = ret; + + ret = j1939_timedate_srv_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 j1939_timedate_srv_handle_events(struct j1939_timedate_srv_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->events & POLLIN) { + ret = j1939_timedate_srv_rx_one(priv, ev->data.fd); + if (ret) { + warn("recv one"); + return ret; + } + } + } + return 0; +} + +static int j1939_timedate_srv_process_events_and_tasks(struct j1939_timedate_srv_priv *priv) +{ + int ret, nfds; + + ret = libj1939_prepare_for_events(&priv->cmn, &nfds, false); + if (ret) + return ret; + + if (nfds > 0) { + ret = j1939_timedate_srv_handle_events(priv, nfds); + if (ret) + return ret; + } + + return 0; +} + +static int j1939_timedate_srv_sock_main_prepare(struct j1939_timedate_srv_priv *priv) +{ + struct sockaddr_can addr = priv->sockname; + int ret; + + ret = libj1939_open_socket(); + if (ret < 0) + return ret; + + priv->sock_main = ret; + + ret = libj1939_bind_socket(priv->sock_main, &addr); + if (ret < 0) + return ret; + + ret = libj1939_socket_prio(priv->sock_main, + J1939_TIMEDATE_PRIO_DEFAULT); + if (ret < 0) + return ret; + + ret = libj1939_set_broadcast(priv->sock_main); + if (ret < 0) + return ret; + + + return libj1939_add_socket_to_epoll(priv->cmn.epoll_fd, + priv->sock_main, EPOLLIN); +} + +static int j1939_timedate_srv_sock_prepare(struct j1939_timedate_srv_priv *priv) +{ + int ret; + + ret = libj1939_create_epoll(); + if (ret < 0) + return ret; + + priv->cmn.epoll_fd = ret; + + priv->cmn.epoll_events = calloc(J1939_TIMEDATE_SRV_MAX_EPOLL_EVENTS, + sizeof(struct epoll_event)); + if (!priv->cmn.epoll_events) + return -ENOMEM; + + priv->cmn.epoll_events_size = J1939_TIMEDATE_SRV_MAX_EPOLL_EVENTS; + + return j1939_timedate_srv_sock_main_prepare(priv); +} + +static void j1939_timedate_srv_print_help(void) +{ + printf("Usage: j1939-timedate-srv [options]\n"); + printf("Options:\n"); + printf(" --interface or -i \n"); + printf(" Specifies the CAN interface to use (mandatory).\n"); + printf(" --local-address or -a \n"); + printf(" Specifies the local address in hexadecimal (mandatory if\n"); + printf(" local name is not provided).\n"); + printf(" --local-name or -n \n"); + printf(" Specifies the local NAME in hexadecimal (mandatory if\n"); + printf(" local address is not provided).\n"); + printf("\n"); + printf("Note: Local address and local name are mutually exclusive and one\n"); + printf(" must be provided.\n"); + printf("\n"); + printf("Usage Examples:\n"); + printf(" Using local address:\n"); + printf(" j1939-timedate-srv -i vcan0 -a 0x90\n"); + printf("\n"); + printf(" Using local NAME:\n"); + printf(" j1939acd -r 64-95 -c /tmp/1122334455667789.jacd 1122334455667789 vcan0 &\n"); + printf(" j1939-timedate-srv -i vcan0 -n 0x1122334455667789\n"); +} + +static int j1939_timedate_srv_parse_args(struct j1939_timedate_srv_priv *priv, + int argc, char *argv[]) +{ + struct sockaddr_can *local = &priv->sockname; + bool local_address_set = false; + bool local_name_set = false; + bool interface_set = false; + int long_index = 0; + int opt; + + static struct option long_options[] = { + {"interface", required_argument, 0, 'i'}, + {"local-address", required_argument, 0, 'a'}, + {"local-name", required_argument, 0, 'n'}, + {0, 0, 0, 0} + }; + + while ((opt = getopt_long(argc, argv, "a:n:i:", 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 '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; + } + interface_set = true; + break; + default: + j1939_timedate_srv_print_help(); + return -EINVAL; + } + } + + if (!interface_set) { + pr_err("interface not specified"); + j1939_timedate_srv_print_help(); + return -EINVAL; + } + + if (local_address_set && local_name_set) { + pr_err("local address and local name or remote address and remote name are mutually exclusive"); + j1939_timedate_srv_print_help(); + return -EINVAL; + } + + return 0; +} + +int main(int argc, char *argv[]) +{ + struct j1939_timedate_srv_priv *priv; + struct timespec ts; + int ret; + + priv = malloc(sizeof(*priv)); + if (!priv) + err(EXIT_FAILURE, "can't allocate priv"); + + bzero(priv, sizeof(*priv)); + + libj1939_init_sockaddr_can(&priv->sockname, J1939_PGN_REQUEST_PGN); + + ret = j1939_timedate_srv_parse_args(priv, argc, argv); + if (ret) + return ret; + + clock_gettime(CLOCK_MONOTONIC, &ts); + priv->cmn.next_send_time = ts; + + ret = j1939_timedate_srv_sock_prepare(priv); + if (ret) + return ret; + + while (1) { + ret = j1939_timedate_srv_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); + + return ret; +} +