diff --git a/CMakeLists.txt b/CMakeLists.txt index c7991ab..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 @@ -105,6 +110,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 +143,7 @@ if(NOT ANDROID) ) target_link_libraries(isobusfs-cli - PRIVATE isobusfs can + PRIVATE isobusfs can j1939 ) add_executable(isobusfs-srv @@ -148,13 +157,39 @@ if(NOT ANDROID) ) target_link_libraries(isobusfs-srv - PRIVATE isobusfs can + PRIVATE isobusfs can j1939 ) install(TARGETS 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 @@ -167,7 +202,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/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(...) 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/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; +} + 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