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; +} +