From e8559479fb68c6648d5b7b6ba65fe17e551125e3 Mon Sep 17 00:00:00 2001 From: Oleksij Rempel Date: Fri, 26 Jul 2024 22:23:11 +0200 Subject: [PATCH 1/3] libj1939: Add function to connect a socket Introduce `libj1939_connect_socket` function to handle socket connections. Signed-off-by: Oleksij Rempel --- libj1939.c | 25 +++++++++++++++++++++++++ libj1939.h | 1 + 2 files changed, 26 insertions(+) diff --git a/libj1939.c b/libj1939.c index ed9a554..bb552c7 100644 --- a/libj1939.c +++ b/libj1939.c @@ -253,6 +253,31 @@ int libj1939_bind_socket(int sock, struct sockaddr_can *addr) return 0; } +/** + * libj1939_connect_socket - Connects a socket to a CAN address. + * @sock: The socket file descriptor. + * @addr: The CAN address to connect to. + * + * This function attempts to establish a connection between the given socket + * and the specified CAN address. If the connection fails, it logs an error + * message with the error code and a description of the error. + * + * Return: 0 on success, or a negative error code on failure. + */ +int libj1939_connect_socket(int sock, struct sockaddr_can *addr) +{ + int ret; + + ret = connect(sock, (void *)addr, sizeof(*addr)); + if (ret < 0) { + ret = -errno; + pr_err("failed to connect socket: %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 diff --git a/libj1939.h b/libj1939.h index 7c19f43..62a2d84 100644 --- a/libj1939.h +++ b/libj1939.h @@ -43,6 +43,7 @@ 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_connect_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); From 71b2aec834e74d57c643e7a6776f21a1448ef17a Mon Sep 17 00:00:00 2001 From: Oleksij Rempel Date: Wed, 24 Jul 2024 11:40:35 +0200 Subject: [PATCH 2/3] j1939-vehicle-position-srv: Introduce J1939 and NMEA 2000 Vehicle Position Server This patch adds `j1939-vehicle-position-srv`, a server for sending vehicle position data over CAN using J1939 or NMEA 2000 protocols. It retrieves GPS data from gpsd or simulates data if gpsd is unavailable. By default, it operates in J1939 profile but can switch to NMEA 2000 with the `-p nmea2000` option. Usage Examples: 1. With gpsd: j1939acd -r 64-95 -c /tmp/1122334455667789.jacd 1122334455667789 vcan0 & j1939-vehicle-position-srv -i vcan0 -n 0x1122334455667789 2. In simulation mode without gpsd: j1939-vehicle-position-srv -i vcan0 -s -p nmea2000 Signed-off-by: Oleksij Rempel [Yegor: add CMakeLists.txt integration] Co-developed-by: Yegor Yefremov --- CMakeLists.txt | 28 + Makefile | 15 +- .../j1939_vehicle_position_cmn.h | 1394 +++++++++++++++ .../j1939_vehicle_position_srv.c | 1514 +++++++++++++++++ 4 files changed, 2950 insertions(+), 1 deletion(-) create mode 100644 j1939_vehicle_position/j1939_vehicle_position_cmn.h create mode 100644 j1939_vehicle_position/j1939_vehicle_position_srv.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 528a8d4..f57d36b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,12 @@ endif() # Add an option to enable treating warnings as errors option(ENABLE_WERROR "Treat all compiler warnings as errors" OFF) +option(ENABLE_GPS "Enable GPS support" OFF) + +find_package(PkgConfig REQUIRED) +if(ENABLE_GPS) + pkg_check_modules(GPS REQUIRED libgps) +endif() if(ENABLE_WERROR) add_compile_options(-Werror) @@ -67,6 +73,10 @@ set(PROGRAMS_J1939_TIMEDATE j1939-timedate-cli ) +set(PROGRAMS_J1939_VEHICLE_POSITION + j1939-vehicle-position-srv +) + set(PROGRAMS_ISOBUSFS isobusfs-srv isobusfs-cli @@ -191,6 +201,24 @@ if(NOT ANDROID) j1939-timedate-srv DESTINATION ${CMAKE_INSTALL_BINDIR}) + if(ENABLE_GPS) + set(PUBLIC_HEADER_J1939_VEHICLE_POSITION + j1939_vehicle_position/j1939_vehicle_position_cmn.h + ) + + add_executable(j1939-vehicle-position-srv + j1939_vehicle_position/j1939_vehicle_position_srv.c + ) + + target_link_libraries(j1939-vehicle-position-srv + PRIVATE can j1939 ${GPS_LIBRARIES} + ) + + install(TARGETS + j1939-vehicle-position-srv + DESTINATION ${CMAKE_INSTALL_BINDIR}) + endif() + endif() add_library(can STATIC diff --git a/Makefile b/Makefile index 08366c8..1cbd2de 100644 --- a/Makefile +++ b/Makefile @@ -67,6 +67,9 @@ PROGRAMS_J1939_TIMEDATE := \ j1939-timedate-srv \ j1939-timedate-cli +PROGRAMS_J1939_VEHICLE_POSITION := \ + j1939-vehicle-position-srv + PROGRAMS_ISOBUSFS := \ isobusfs-srv \ isobusfs-cli @@ -98,6 +101,7 @@ PROGRAMS_SLCAN := \ PROGRAMS := \ $(PROGRAMS_CANGW) \ $(PROGRAMS_J1939_TIMEDATE) \ + $(PROGRAMS_J1939_VEHICLE_POSITION) \ $(PROGRAMS_ISOBUSFS) \ $(PROGRAMS_ISOTP) \ $(PROGRAMS_J1939) \ @@ -126,7 +130,8 @@ endif all: $(PROGRAMS) clean: - rm -f $(PROGRAMS) *.o mcp251xfd/*.o isobusfs/*.o j1939_timedate/*.o + rm -f $(PROGRAMS) *.o mcp251xfd/*.o isobusfs/*.o j1939_timedate/*.o \ + j1939_vehicle_position/*.o install: mkdir -p $(DESTDIR)$(PREFIX)/bin @@ -153,6 +158,8 @@ isobusfs_srv.o: lib.h libj1939.h isobusfs_c.o: lib.h libj1939.h j1939_timedate_srv.o: lib.h libj1939.h j1939_timedate_cli.o: lib.h libj1939.h +j1939_vehicle_position_srv.o: lib.h libj1939.h + canframelen.o: canframelen.h asc2log: asc2log.o lib.o @@ -182,6 +189,12 @@ j1939-timedate-cli: lib.o \ j1939_timedate/j1939_timedate_cli.o $(CC) $(LDFLAGS) $^ $(LDLIBS) -o $@ +j1939-vehicle-position-srv: \ + lib.o \ + libj1939.o \ + j1939_vehicle_position/j1939_vehicle_position_srv.o \ + $(CC) $(LDFLAGS) $^ $(LDLIBS) -lgps -o $@ + isobusfs-srv: lib.o \ libj1939.o \ isobusfs/isobusfs_cmn.o \ diff --git a/j1939_vehicle_position/j1939_vehicle_position_cmn.h b/j1939_vehicle_position/j1939_vehicle_position_cmn.h new file mode 100644 index 0000000..5ed6c0b --- /dev/null +++ b/j1939_vehicle_position/j1939_vehicle_position_cmn.h @@ -0,0 +1,1394 @@ +// SPDX-License-Identifier: LGPL-2.0-only +// SPDX-FileCopyrightText: 2024 Oleksij Rempel + +#ifndef _J1939_VEHICLE_POSITION_H_ +#define _J1939_VEHICLE_POSITION_H_ + +#include +#include +#include +#include + +#include +#include +#include "../libj1939.h" +#include "../lib.h" + +#define J1939_PGN_REQUEST_PGN 0x0ea00 /* 59904 */ + +/* ISO 11783-3:2018 - 5.4.5 Acknowledgment */ +#define ISOBUS_PGN_ACK 0x0e800 /* 59392 */ + +#define J1939_MAX_TRANSFER_LENGH 1024 + +struct j1939_vp_stats { + int err; + uint32_t tskey_sch; + uint32_t tskey_ack; + uint32_t send; +}; + +struct j1939_vp_msg { + uint8_t buf[J1939_MAX_TRANSFER_LENGH]; + size_t buf_size; + size_t len; /* length of received message */ + struct sockaddr_can peername; + socklen_t peer_addr_len; + int sock; +}; + +struct j1939_vp_err_msg { + struct sock_extended_err *serr; + struct scm_timestamping *tss; + struct j1939_vp_stats *stats; +}; + +/* SAE J1939 specific definitions */ + +/* SAE J1939-71:2002 - 5.3 pgn65267 - Vehicle Position 1 - VP1 - */ +#define J1939_PGN_VP1 0x0fef3 /* 65267 */ + +#define J1939_VP1_PRIO_DEFAULT 6 +#define J1939_VP1_MAX_TRANSFER_LENGH \ + sizeof(struct j1939_vp1_packet) +#define J1939_VP1_REPETITION_RATE_MS 5000 +#define J1939_VP1_JITTER_MS 500 + +/** + * struct j1939_vp1_packet - Represents the PGN 65267 Vehicle + * Position packet + * + * @latitude: Raw latitude position of the vehicle + * - SPN: 584 + * - Data Length: 4 bytes + * - Resolution: 10^-7 deg/bit + * - Offset: -210 degrees + * - Range: -210 to +211.1008122 degrees + * - Operating Range: -210 degrees (SOUTH) to +211.108122 degrees + * (NORTH) + * + * @longitude: Raw longitude position of the vehicle + * - SPN: 585 + * - Data Length: 4 bytes + * - Resolution: 10^-7 deg/bit + * - Offset: -210 degrees + * - Range: -210 to +211.1008122 degrees + * - Operating Range: -210 degrees (WEST) to +211.108122 degrees + * (EAST) + * + * This structure defines each component of the Vehicle Position as described in + * PGN 65267. + */ +struct j1939_vp1_packet { + __le32 latitude; /* SPN 584 */ + __le32 longitude; /* SPN 585 */ +} __attribute__((__packed__)); + +/** + * j1939_vp1_get_latitude - Get the latitude from the packet + * @packet: Pointer to the J1939 VP1 packet + * + * Return: Latitude in degrees as a signed 32-bit integer + */ +static inline int32_t +j1939_vp1_get_latitude(const struct j1939_vp1_packet *packet) +{ + return le32toh(packet->latitude); +} + +/** + * j1939_vp1_set_latitude - Set the latitude in the packet + * @packet: Pointer to the J1939 VP1 packet + * @latitude: Latitude in degrees as a signed 32-bit integer + */ +static inline void +j1939_vp1_set_latitude(struct j1939_vp1_packet *packet, int32_t latitude) +{ + packet->latitude = htole32(latitude); +} + +/** + * j1939_vp1_get_longitude - Get the longitude from the packet + * @packet: Pointer to the J1939 VP1 packet + * + * Return: Longitude in degrees as a signed 32-bit integer + */ +static inline int32_t +j1939_vp1_get_longitude(const struct j1939_vp1_packet *packet) +{ + return le32toh(packet->longitude); +} + +/** + * j1939_vp1_set_longitude - Set the longitude in the packet + * @packet: Pointer to the J1939 VP1 packet + * @longitude: Longitude in degrees as a signed 32-bit integer + */ +static inline void +j1939_vp1_set_longitude(struct j1939_vp1_packet *packet, int32_t longitude) +{ + packet->longitude = htole32(longitude); +} + +/* SAE J1939xxxxxxxx - xxx pgn64502 - Vehicle Position 2 - VP2 - */ +#define J1939_PGN_VP2 0x0fbf6 /* 64502 */ + +#define J1939_VP2_PRIO_DEFAULT 6 +#define J1939_VP2_MAX_TRANSFER_LENGH \ + sizeof(struct j1939_vp2_packet) +#define J1939_VP2_REPETITION_RATE_MS 5000 +#define J1939_VP2_JITTER_MS 500 + +/** + * struct j1939_vp2_packet - Represents the PGN 64502 Vehicle + * Position 2 packet + * FIXME: current packet layout is guessed based on limited information: + * https://www.isobus.net/isobus/pGNAndSPN/10801?type=PGN + * + * @total_satellites: Total number of satellites in view + * - SPN: 8128 + * - Data Length: 1 byte + * + * @hdop: Horizontal dilution of precision + * - SPN: 8129 + * - Data Length: 1 byte + * - Resolution: 0.1 + * + * @vdop: Vertical dilution of precision + * - SPN: 8130 + * - Data Length: 1 byte + * - Resolution: 0.1 + * + * @pdop: Position dilution of precision + * - SPN: 8131 + * - Data Length: 1 byte + * - Resolution: 0.1 + * + * @tdop: Time dilution of precision + * - SPN: 8132 + * - Data Length: 1 byte + * - Resolution: 0.1 + * + * This structure defines each component of the Vehicle Position 2 as described + * in PGN 64502. + */ +struct j1939_vp2_packet { + uint8_t total_satellites; /* SPN 8128 */ + uint8_t hdop; /* SPN 8129 */ + uint8_t vdop; /* SPN 8130 */ + uint8_t pdop; /* SPN 8131 */ + uint8_t tdop; /* SPN 8132 */ +} __attribute__((__packed__)); + +/** + * j1939_vp2_get_total_satellites - Get the total number of satellites + * @packet: Pointer to the J1939 VP2 packet + * + * Return: Total number of satellites as an 8-bit integer + */ +static inline uint8_t j1939_vp2_get_total_satellites(const struct j1939_vp2_packet *packet) +{ + return packet->total_satellites; +} + +/** + * j1939_vp2_set_total_satellites - Set the total number of satellites + * @packet: Pointer to the J1939 VP2 packet + * @total_satellites: Total number of satellites as an 8-bit integer + */ +static inline void j1939_vp2_set_total_satellites(struct j1939_vp2_packet *packet, + uint8_t total_satellites) +{ + packet->total_satellites = total_satellites; +} + +/** + * j1939_vp2_get_hdop - Get the horizontal dilution of precision (HDOP) + * @packet: Pointer to the J1939 VP2 packet + * + * Return: HDOP as an 8-bit integer + */ +static inline uint8_t j1939_vp2_get_hdop(const struct j1939_vp2_packet *packet) +{ + return packet->hdop; +} + +/** + * j1939_vp2_set_hdop - Set the horizontal dilution of precision (HDOP) + * @packet: Pointer to the J1939 VP2 packet + * @hdop: HDOP as an 8-bit integer + */ +static inline void j1939_vp2_set_hdop(struct j1939_vp2_packet *packet, + uint8_t hdop) +{ + packet->hdop = hdop; +} + +/** + * j1939_vp2_get_vdop - Get the vertical dilution of precision (VDOP) + * @packet: Pointer to the J1939 VP2 packet + * + * Return: VDOP as an 8-bit integer + */ +static inline uint8_t j1939_vp2_get_vdop(const struct j1939_vp2_packet *packet) +{ + return packet->vdop; +} + +/** + * j1939_vp2_set_vdop - Set the vertical dilution of precision (VDOP) + * @packet: Pointer to the J1939 VP2 packet + * @vdop: VDOP as an 8-bit integer + */ +static inline void j1939_vp2_set_vdop(struct j1939_vp2_packet *packet, + uint8_t vdop) +{ + packet->vdop = vdop; +} + +/** + * j1939_vp2_get_pdop - Get the positional dilution of precision (PDOP) + * @packet: Pointer to the J1939 VP2 packet + * + * Return: PDOP as an 8-bit integer + */ +static inline uint8_t j1939_vp2_get_pdop(const struct j1939_vp2_packet *packet) +{ + return packet->pdop; +} + +/** + * j1939_vp2_set_pdop - Set the positional dilution of precision (PDOP) + * @packet: Pointer to the J1939 VP2 packet + * @pdop: PDOP as an 8-bit integer + */ +static inline void j1939_vp2_set_pdop(struct j1939_vp2_packet *packet, + uint8_t pdop) +{ + packet->pdop = pdop; +} + +/** + * j1939_vp2_get_tdop - Get the time dilution of precision (TDOP) + * @packet: Pointer to the J1939 VP2 packet + * + * Return: TDOP as an 8-bit integer + */ +static inline uint8_t j1939_vp2_get_tdop(const struct j1939_vp2_packet *packet) +{ + return packet->tdop; +} + +/** + * j1939_vp2_set_tdop - Set the time dilution of precision (TDOP) + * @packet: Pointer to the J1939 VP2 packet + * @tdop: TDOP as an 8-bit integer + */ +static inline void j1939_vp2_set_tdop(struct j1939_vp2_packet *packet, + uint8_t tdop) +{ + packet->tdop = tdop; +} + +/* NMEA 2000 specific definitions */ + +/* NMEA 2000 - PGN 126992 - System Time */ +#define NMEA2000_PGN_SYS_TIME 0x1F010 /* 126992 */ + +#define NMEA2000_SYS_TIME_PRIO_DEFAULT 6 +#define NMEA2000_SYS_TIME_MAX_TRANSFER_LENGTH \ + sizeof(struct nmea2000_sys_time_packet) +#define NMEA2000_SYS_TIME_REPETITION_RATE_MS 1000 +#define NMEA2000_SYS_TIME_JITTER_MS 100 + +/* Bit masks for the source_and_reserved field */ +#define NMEA2000_SYS_TIME_SOURCE_MASK GENMASK(3, 0) +/** + * enum nmea2000_sys_time_source - Source of time information + * @NMEA2000_SYS_TIME_SOURCE_GPS: GPS + * @NMEA2000_SYS_TIME_SOURCE_GLONASS: GLONASS + * @NMEA2000_SYS_TIME_SOURCE_RADIO_STATION: Radio Station + * @NMEA2000_SYS_TIME_SOURCE_LOCAL_CESIUM: Local Cesium clock + * @NMEA2000_SYS_TIME_SOURCE_LOCAL_RUBIDIUM: Local Rubidium clock + * @NMEA2000_SYS_TIME_SOURCE_LOCAL_CRYSTAL: Local Crystal clock + */ +enum nmea2000_sys_time_source { + NMEA2000_SYS_TIME_SOURCE_GPS = 0, + NMEA2000_SYS_TIME_SOURCE_GLONASS = 1, + NMEA2000_SYS_TIME_SOURCE_RADIO_STATION = 2, + NMEA2000_SYS_TIME_SOURCE_LOCAL_CESIUM = 3, + NMEA2000_SYS_TIME_SOURCE_LOCAL_RUBIDIUM = 4, + NMEA2000_SYS_TIME_SOURCE_LOCAL_CRYSTAL = 5, +}; + +#define NMEA2000_SYS_TIME_RESERVED_MASK GENMASK(7, 4) + +/** + * struct nmea2000_sys_time_packet - Represents the PGN 126992 System Time packet + * + * @sid: Sequence identifier for correlating related PGNs. + * @source: Source of the time information. Possible values are: + * 0 = GPS, 1 = GLONASS, 2 = Radio Station, 3 = Local Cesium clock, + * 4 = Local Rubidium clock, 5 = Local Crystal clock). + * @reserved: Reserved field, set to 0xF. + * @date: UTC Date in days since January 1, 1970. + * @time: UTC Time in 0.0001 seconds since midnight. + */ +struct nmea2000_sys_time_packet { + uint8_t sid; + uint8_t source_reserved; + __le16 date; + __le32 time; +} __attribute__((__packed__)); + +/** + * nmea2000_sys_time_get_sid - Get the sequence identifier (SID) + * @packet: Pointer to the NMEA2000 system time packet + * + * Return: Sequence identifier (8 bits) + */ +static inline uint8_t +nmea2000_sys_time_get_sid(const struct nmea2000_sys_time_packet *packet) +{ + return packet->sid; +} + +/** + * nmea2000_sys_time_set_sid - Set the sequence identifier (SID) + * @packet: Pointer to the NMEA2000 system time packet + * @sid: Sequence identifier to set (8 bits) + */ +static inline void +nmea2000_sys_time_set_sid(struct nmea2000_sys_time_packet *packet, uint8_t sid) +{ + packet->sid = sid; +} + +/** + * nmea2000_sys_time_get_source - Extract the source of time information + * @packet: Pointer to the NMEA2000 system time packet + * + * Return: Source of time information (4 bits) + */ +static inline uint8_t +nmea2000_sys_time_get_source(const struct nmea2000_sys_time_packet *packet) +{ + return FIELD_GET(NMEA2000_SYS_TIME_SOURCE_MASK, packet->source_reserved); +} + +/** + * nmea2000_sys_time_get_reserved - Extract the reserved field + * @packet: Pointer to the NMEA2000 system time packet + * + * Return: Reserved field (4 bits) + */ +static inline uint8_t +nmea2000_sys_time_get_reserved(const struct nmea2000_sys_time_packet *packet) +{ + return FIELD_GET(NMEA2000_SYS_TIME_RESERVED_MASK, packet->source_reserved); +} + +/** + * nmea2000_sys_time_set_source_reserved - Set the source and reserved fields + * @packet: Pointer to the NMEA2000 system time packet + * @source: Source of time information (4 bits) + * @reserved: Reserved field value (4 bits) + */ +static inline void +nmea2000_sys_time_set_source_reserved(struct nmea2000_sys_time_packet *packet, + enum nmea2000_sys_time_source source, + uint8_t reserved) +{ + packet->source_reserved = FIELD_PREP(NMEA2000_SYS_TIME_SOURCE_MASK, + source) | + FIELD_PREP(NMEA2000_SYS_TIME_RESERVED_MASK, + reserved); +} + +/** + * nmea2000_sys_time_get_date - Get the UTC date + * @packet: Pointer to the NMEA2000 system time packet + * + * Return: UTC date in days since January 1, 1970 + */ +static inline uint16_t +nmea2000_sys_time_get_date(const struct nmea2000_sys_time_packet *packet) +{ + return le16toh(packet->date); +} + +/** + * nmea2000_sys_time_set_date - Set the UTC date + * @packet: Pointer to the NMEA2000 system time packet + * @date: UTC date in days since January 1, 1970 + */ +static inline void +nmea2000_sys_time_set_date(struct nmea2000_sys_time_packet *packet, + uint16_t date) +{ + packet->date = htole16(date); +} + +/** + * nmea2000_sys_time_get_time - Get the UTC time + * @packet: Pointer to the NMEA2000 system time packet + * + * Return: UTC time in 0.0001 seconds since midnight + */ +static inline uint32_t +nmea2000_sys_time_get_time(const struct nmea2000_sys_time_packet *packet) +{ + return le32toh(packet->time); +} + +/** + * nmea2000_sys_time_set_time - Set the UTC time + * @packet: Pointer to the NMEA2000 system time packet + * @time: UTC time in 0.0001 seconds since midnight + */ +static inline void +nmea2000_sys_time_set_time(struct nmea2000_sys_time_packet *packet, + uint32_t time) +{ + packet->time = htole32(time); +} + +/* NMEA 2000 - PGN 127258 - Magnetic Variation */ +#define NMEA2000_PGN_MAG_VAR 0x1F11A /* 127258 */ + +#define NMEA2000_MAG_VAR_PRIO_DEFAULT 6 +#define NMEA2000_MAG_VAR_MAX_TRANSFER_LENGTH \ + sizeof(struct nmea2000_mag_var_packet) +#define NMEA2000_MAG_VAR_REPETITION_RATE_MS 1000 +#define NMEA2000_MAG_VAR_JITTER_MS 100 + +#define NMEA2000_MAG_VAR_SOURCE_MASK GENMASK(3, 0) +/** + * enum magnetic_variation_source - Source of magnetic variation + * @MAGNETIC_VARIATION_MANUAL: Manual entry + * @MAGNETIC_VARIATION_AUTOMATIC_CHART: Automatic from chart + * @MAGNETIC_VARIATION_AUTOMATIC_TABLE: Automatic from table + * @MAGNETIC_VARIATION_AUTOMATIC_CALCULATION: Automatic calculation + * @MAGNETIC_VARIATION_WMM_2000: WMM 2000 + * @MAGNETIC_VARIATION_WMM_2005: WMM 2005 + * @MAGNETIC_VARIATION_WMM_2010: WMM 2010 + * @MAGNETIC_VARIATION_WMM_2015: WMM 2015 + * @MAGNETIC_VARIATION_WMM_2020: WMM 2020 + */ +enum magnetic_variation_source { + MAGNETIC_VARIATION_MANUAL = 0, + MAGNETIC_VARIATION_AUTOMATIC_CHART = 1, + MAGNETIC_VARIATION_AUTOMATIC_TABLE = 2, + MAGNETIC_VARIATION_AUTOMATIC_CALCULATION = 3, + MAGNETIC_VARIATION_WMM_2000 = 4, + MAGNETIC_VARIATION_WMM_2005 = 5, + MAGNETIC_VARIATION_WMM_2010 = 6, + MAGNETIC_VARIATION_WMM_2015 = 7, + MAGNETIC_VARIATION_WMM_2020 = 8, +}; + +#define NMEA2000_MAG_VAR_RESERVED_MASK GENMASK(7, 4) + +/** + * struct nmea2000_mag_var_packet - Represents the PGN 127258 Magnetic Variation + * packet + * + * @sid: Sequence identifier for correlating related PGNs. + * @source_reserved: Encodes the source of magnetic variation and reserved bits. + * - Bits 0-3: Source of magnetic variation (4 bits). + * - For example, 5 = WMM2005. + * - Bits 4-7: Reserved field (4 bits). + * - Reserved for future use, typically set to 0xF. + * @age_of_service: UTC Date in days since January 1, 1970 + * @variation: Magnetic variation (positive = Easterly, negative = Westerly) + * + * This structure defines the fields for the Magnetic Variation packet. + */ +struct nmea2000_mag_var_packet { + uint8_t sid; + uint8_t source_reserved; + __le32 age_of_service; + __le16 variation; +} __attribute__((__packed__)); + +/** + * nmea2000_mag_var_get_sid - Get the sequence identifier (SID) + * @packet: Pointer to the NMEA2000 magnetic variation packet + * + * Return: Sequence identifier (8 bits) + */ +static inline uint8_t +nmea2000_mag_var_get_sid(const struct nmea2000_mag_var_packet *packet) +{ + return packet->sid; +} + +/** + * nmea2000_mag_var_set_sid - Set the sequence identifier (SID) + * @packet: Pointer to the NMEA2000 magnetic variation packet + * @sid: Sequence identifier to set (8 bits) + */ +static inline void +nmea2000_mag_var_set_sid(struct nmea2000_mag_var_packet *packet, uint8_t sid) +{ + packet->sid = sid; +} + +/** + * nmea2000_mag_var_get_source - Extract the source of magnetic variation + * @packet: Pointer to the NMEA2000 magnetic variation packet + * + * Return: Source of magnetic variation (4 bits) + */ +static inline uint8_t +nmea2000_mag_var_get_source(const struct nmea2000_mag_var_packet *packet) +{ + return FIELD_GET(NMEA2000_MAG_VAR_SOURCE_MASK, packet->source_reserved); +} + +/** + * nmea2000_mag_var_set_source_reserved - Set the source and reserved fields + * @packet: Pointer to the NMEA2000 magnetic variation packet + * @source: Source of magnetic variation (4 bits) + * @reserved: Reserved field value (4 bits) + */ + +static inline void +nmea2000_mag_var_set_source_reserved(struct nmea2000_mag_var_packet *packet, + enum magnetic_variation_source source, + uint8_t reserved) +{ + packet->source_reserved = FIELD_PREP(NMEA2000_MAG_VAR_SOURCE_MASK, + source) | + FIELD_PREP(NMEA2000_MAG_VAR_RESERVED_MASK, + reserved); +} + +/** + * nmea2000_mag_var_get_age_of_service - Get the age of service + * @packet: Pointer to the NMEA2000 magnetic variation packet + * + * Return: UTC date in days since January 1, 1970 + */ +static inline uint32_t +nmea2000_mag_var_get_age_of_service(const struct nmea2000_mag_var_packet *packet) +{ + return le32toh(packet->age_of_service); +} + +/** + * nmea2000_mag_var_set_age_of_service - Set the age of service + * @packet: Pointer to the NMEA2000 magnetic variation packet + * @age: UTC date in days since January 1, 1970 + */ +static inline void +nmea2000_mag_var_set_age_of_service(struct nmea2000_mag_var_packet *packet, + uint32_t age) +{ + packet->age_of_service = htole32(age); +} + +/** + * nmea2000_mag_var_get_variation - Get the magnetic variation + * @packet: Pointer to the NMEA2000 magnetic variation packet + * + * Return: Magnetic variation in tenths of degrees (positive = Easterly, + * negative = Westerly) + */ +static inline uint16_t +nmea2000_mag_var_get_variation(const struct nmea2000_mag_var_packet *packet) +{ + return le16toh(packet->variation); +} + +/** + * nmea2000_mag_var_set_variation - Set the magnetic variation + * @packet: Pointer to the NMEA2000 magnetic variation packet + * @variation: Magnetic variation in tenths of degrees (positive = Easterly, + * negative = Westerly) + */ +static inline void +nmea2000_mag_var_set_variation(struct nmea2000_mag_var_packet *packet, + uint16_t variation) +{ + packet->variation = htole16(variation); +} + +/* NMEA 2000 - PGN 129025 - Position, Rapid Update */ +#define NMEA2000_PGN_POSITION_RAPID 0x1F801 /* 129025 */ + +#define NMEA2000_POSITION_RAPID_PRIO_DEFAULT 6 +#define NMEA2000_POSITION_RAPID_MAX_TRANSFER_LENGTH \ + sizeof(struct nmea2000_position_rapid_packet) +#define NMEA2000_POSITION_RAPID_REPETITION_RATE_MS 200 +#define NMEA2000_POSITION_RAPID_JITTER_MS 50 + +/** + * struct nmea2000_position_rapid_packet - Represents the PGN 129025 Position, + * Rapid Update packet + * + * @latitude: Latitude in 1e-7 degrees ("-" = south, "+" = north) + * @longitude: Longitude in 1e-7 degrees ("-" = west, "+" = east) + * + * This structure defines the fields for the Position, Rapid Update packet. + */ +struct nmea2000_position_rapid_packet { + __le32 latitude; /* SPN 263 */ + __le32 longitude; /* SPN 264 */ +} __attribute__((__packed__)); + +/** + * nmea2000_position_get_latitude - Get the latitude from the packet + * @packet: Pointer to the NMEA2000 position rapid update packet + * + * Return: Latitude in degrees as a signed 32-bit integer + */ +static inline int32_t +nmea2000_position_get_latitude(const struct nmea2000_position_rapid_packet *packet) +{ + return le32toh(packet->latitude); +} + +/** + * nmea2000_position_set_latitude - Set the latitude in the packet + * @packet: Pointer to the NMEA2000 position rapid update packet + * @latitude: Latitude in degrees as a signed 32-bit integer + */ +static inline void +nmea2000_position_set_latitude(struct nmea2000_position_rapid_packet *packet, + int32_t latitude) +{ + packet->latitude = htole32(latitude); +} + +/** + * nmea2000_position_get_longitude - Get the longitude from the packet + * @packet: Pointer to the NMEA2000 position rapid update packet + * + * Return: Longitude in degrees as a signed 32-bit integer + */ +static inline int32_t +nmea2000_position_get_longitude(const struct nmea2000_position_rapid_packet *packet) +{ + return le32toh(packet->longitude); +} + +/** + * nmea2000_position_set_longitude - Set the longitude in the packet + * @packet: Pointer to the NMEA2000 position rapid update packet + * @longitude: Longitude in degrees as a signed 32-bit integer + */ +static inline void +nmea2000_position_set_longitude(struct nmea2000_position_rapid_packet *packet, + int32_t longitude) +{ + packet->longitude = htole32(longitude); +} + +/* NMEA 2000 - PGN 129026 - COG and SOG, Rapid Update */ +#define NMEA2000_PGN_COG_SOG_RAPID 0x1F802 /* 129026 */ + +#define NMEA2000_COG_SOG_RAPID_PRIO_DEFAULT 6 +#define NMEA2000_COG_SOG_RAPID_MAX_TRANSFER_LENGTH \ + sizeof(struct nmea2000_cog_sog_rapid_packet) +#define NMEA2000_COG_SOG_RAPID_REPETITION_RATE_MS 250 +#define NMEA2000_COG_SOG_RAPID_JITTER_MS 50 + +#define NMEA2000_COG_SOG_REF_MASK GENMASK(1, 0) +/** + * enum nmea2000_cog_reference - Reference for Course Over Ground (COG) + * @NMEA2000_COG_REFERENCE_TRUE: True reference + * @NMEA2000_COG_REFERENCE_MAGNETIC: Magnetic reference + * @NMEA2000_COG_REFERENCE_ERROR: Error + */ +enum nmea2000_cog_reference { + NMEA2000_COG_REFERENCE_TRUE = 0, + NMEA2000_COG_REFERENCE_MAGNETIC = 1, + NMEA2000_COG_REFERENCE_ERROR = 2, +}; + +#define NMEA2000_COG_SOG_RES1_MASK GENMASK(7, 2) + +/** + * struct nmea2000_cog_sog_rapid_packet - Represents the PGN 129026 COG and SOG, + * Rapid Update packet + * + * @sid: Sequence identifier for correlating related PGNs. + * @cog_ref_res1: Encodes the COG reference and reserved1 fields (8 bits). + * - Bits 0-1: COG reference (2 bits). + * - 0: True + * - 1: Magnetic + * - 2: Error + * - Bits 2-7: Reserved1 field (6 bits). + * - Reserved for future use, typically set to 0xFF. + * @cog: Course Over Ground in 1e-4 radians + * @sog: Speed Over Ground in 1e-2 m/s + * @reserved2: Reserved field, set to 0xFFFF. + * + * This structure defines the fields for the COG and SOG, Rapid Update packet. + */ +struct nmea2000_cog_sog_rapid_packet { + uint8_t sid; + uint8_t cog_ref_res1; + __le16 cog; + __le16 sog; + uint16_t reserved2; +} __attribute__((__packed__)); + +/** + * nmea2000_cog_sog_get_sid - Get the sequence identifier (SID) + * @packet: Pointer to the NMEA2000 COG and SOG rapid update packet + * + * Return: Sequence identifier (8 bits) + */ +static inline uint8_t +nmea2000_cog_sog_get_sid(const struct nmea2000_cog_sog_rapid_packet *packet) +{ + return packet->sid; +} + +/** + * nmea2000_cog_sog_set_sid - Set the sequence identifier (SID) + * @packet: Pointer to the NMEA2000 COG and SOG rapid update packet + * @sid: Sequence identifier to set (8 bits) + */ +static inline void +nmea2000_cog_sog_set_sid(struct nmea2000_cog_sog_rapid_packet *packet, + uint8_t sid) +{ + packet->sid = sid; +} + +/** + * nmea2000_cog_sog_get_cog_reference - Extract the COG reference + * @packet: Pointer to the NMEA2000 COG and SOG rapid update packet + * + * Return: COG reference (2 bits) + */ +static inline uint8_t +nmea2000_cog_sog_get_cog_reference(const struct nmea2000_cog_sog_rapid_packet *packet) +{ + return FIELD_GET(NMEA2000_COG_SOG_REF_MASK, packet->cog_ref_res1); +} + +/** + * nmea2000_cog_sog_set_cog_ref_res1 - Set the COG reference and reserved1 fields + * @packet: Pointer to the NMEA2000 COG and SOG rapid update packet + * @cog_reference: COG reference value (2 bits) + * @reserved1: Reserved1 value (6 bits) + */ +static inline void +nmea2000_cog_sog_set_cog_ref_res1(struct nmea2000_cog_sog_rapid_packet *packet, + enum nmea2000_cog_reference cog_reference, + uint8_t reserved1) +{ + packet->cog_ref_res1 = FIELD_PREP(NMEA2000_COG_SOG_REF_MASK, + cog_reference) | + FIELD_PREP(NMEA2000_COG_SOG_RES1_MASK, + reserved1); +} + +/** + * nmea2000_cog_sog_get_cog - Get the Course Over Ground (COG) + * @packet: Pointer to the NMEA2000 COG and SOG rapid update packet + * + * Return: COG in 1e-4 radians + */ +static inline uint16_t +nmea2000_cog_sog_get_cog(const struct nmea2000_cog_sog_rapid_packet *packet) +{ + return le16toh(packet->cog); +} + +/** + * nmea2000_cog_sog_set_cog - Set the Course Over Ground (COG) + * @packet: Pointer to the NMEA2000 COG and SOG rapid update packet + * @cog: COG value in 1e-4 radians + */ +static inline void +nmea2000_cog_sog_set_cog(struct nmea2000_cog_sog_rapid_packet *packet, + uint16_t cog) +{ + packet->cog = htole16(cog); +} + +/** + * nmea2000_cog_sog_get_sog - Get the Speed Over Ground (SOG) + * @packet: Pointer to the NMEA2000 COG and SOG rapid update packet + * + * Return: SOG in 1e-2 m/s + */ +static inline uint16_t +nmea2000_cog_sog_get_sog(const struct nmea2000_cog_sog_rapid_packet *packet) +{ + return le16toh(packet->sog); +} + +/** + * nmea2000_cog_sog_set_sog - Set the Speed Over Ground (SOG) + * @packet: Pointer to the NMEA2000 COG and SOG rapid update packet + * @sog: SOG value in 1e-2 m/s + */ +static inline void +nmea2000_cog_sog_set_sog(struct nmea2000_cog_sog_rapid_packet *packet, + uint16_t sog) +{ + packet->sog = htole16(sog); +} + +/* NMEA 2000 - PGN 129029 - GNSS Position Data */ +#define NMEA2000_PGN_GNSS_POSITION_DATA 0x1F805 /* 129029 */ + +#define NMEA2000_GNSS_POSITION_DATA_PRIO_DEFAULT 6 +#define NMEA2000_GNSS_POSITION_DATA_MAX_TRANSFER_LENGTH \ + sizeof(struct nmea2000_gnss_position_data_packet) +#define NMEA2000_GNSS_POSITION_DATA_REPETITION_RATE_MS 1000 +#define NMEA2000_GNSS_POSITION_DATA_JITTER_MS 100 + +#define NMEA2000_GNSS_TYPE_MASK GENMASK(3, 0) +/** + * enum nmea2000_gnss_type - Types of GNSS systems + * @GNSS_TYPE_GPS: GPS + * @GNSS_TYPE_GLONASS: GLONASS + * @GNSS_TYPE_GPS_GLONASS: Combined GPS and GLONASS + * @GNSS_TYPE_GPS_SBAS_WAAS: GPS with SBAS/WAAS + * @GNSS_TYPE_GPS_SBAS_WAAS_GLONASS: Combined GPS, SBAS/WAAS, and GLONASS + * @GNSS_TYPE_CHAYKA: Chayka navigation system + * @GNSS_TYPE_INTEGRATED: Integrated navigation + * @GNSS_TYPE_SURVEYED: Surveyed position + * @GNSS_TYPE_GALILEO: Galileo navigation system + */ +enum nmea2000_gnss_type { + GNSS_TYPE_GPS = 0, + GNSS_TYPE_GLONASS = 1, + GNSS_TYPE_GPS_GLONASS = 2, + GNSS_TYPE_GPS_SBAS_WAAS = 3, + GNSS_TYPE_GPS_SBAS_WAAS_GLONASS = 4, + GNSS_TYPE_CHAYKA = 5, + GNSS_TYPE_INTEGRATED = 6, + GNSS_TYPE_SURVEYED = 7, + GNSS_TYPE_GALILEO = 8, +}; + +#define NMEA2000_GNSS_METHOD_MASK GENMASK(7, 4) +/** + * enum nmea2000_gnss_method - GNSS methods + * @GNSS_METHOD_NO_GNSS: No GNSS + * @GNSS_METHOD_GNSS_FIX: GNSS fix + * @GNSS_METHOD_DGNSS_FIX: Differential GNSS (DGNSS) fix + * @GNSS_METHOD_PRECISE_GNSS: Precise GNSS fix + * @GNSS_METHOD_RTK_FIXED_INT: RTK fixed integer solution + * @GNSS_METHOD_RTK_FLOAT: RTK float solution + * @GNSS_METHOD_ESTIMATED: Estimated (Dead Reckoning) mode + * @GNSS_METHOD_MANUAL_INPUT: Manual input mode + * @GNSS_METHOD_SIMULATE_MODE: Simulated GNSS mode + */ +enum nmea2000_gnss_method { + GNSS_METHOD_NO_GNSS = 0, + GNSS_METHOD_GNSS_FIX = 1, + GNSS_METHOD_DGNSS_FIX = 2, + GNSS_METHOD_PRECISE_GNSS = 3, + GNSS_METHOD_RTK_FIXED_INT = 4, + GNSS_METHOD_RTK_FLOAT = 5, + GNSS_METHOD_ESTIMATED = 6, + GNSS_METHOD_MANUAL_INPUT = 7, + GNSS_METHOD_SIMULATE_MODE = 8, +}; + +#define NMEA2000_INTEGRITY_MASK GENMASK(1, 0) +/** + * enum nmea2000_integrity_status - Integrity status values + * @NMEA2000_INTEGRITY_NO_CHECKING: No integrity checking + * @NMEA2000_INTEGRITY_SAFE: Safe integrity status + * @NMEA2000_INTEGRITY_CAUTION: Caution integrity status + */ +enum nmea2000_integrity_status { + NMEA2000_INTEGRITY_NO_CHECKING = 0, + NMEA2000_INTEGRITY_SAFE = 1, + NMEA2000_INTEGRITY_CAUTION = 2, +}; + +#define NMEA2000_RESERVED_MASK GENMASK(7, 2) + +/** + * struct nmea2000_gnss_position_data_packet - Represents the PGN 129029 GNSS + * Position Data packet + * + * @sid: Sequence identifier for correlating related PGNs (8 bits). + * @date: UTC Date in days since January 1, 1970 (16 bits). + * @time: UTC Time in 0.0001 seconds since midnight (32 bits). + * @latitude: Latitude in 1e-16 degrees ("-" = south, "+" = north) (64 bits). + * @longitude: Longitude in 1e-16 degrees ("-" = west, "+" = east) (64 bits). + * @altitude: Altitude in 1e-6 meters above WGS-84 (64 bits). + * @gnss_info: Encodes GNSS system type and GNSS method in a single byte. + * - Bits 0-3: GNSS system type. Possible values: + * - 0: GPS + * - 1: GLONASS + * - 2: GPS+GLONASS + * - 3: GPS+SBAS/WAAS + * - 4: GPS+SBAS/WAAS+GLONASS + * - 5: Chayka + * - 6: Integrated + * - 7: Surveyed + * - 8: Galileo + * - Bits 4-7: GNSS method. Possible values: + * - 0: No GNSS + * - 1: GNSS fix + * - 2: DGNSS fix + * - 3: Precise GNSS + * - 4: RTK Fixed Integer + * - 5: RTK float + * - 6: Estimated (DR) mode + * - 7: Manual Input + * - 8: Simulate mode + * + * @status: Encodes integrity status and reserved bits in a single byte. + * - Bits 0-1: Integrity status. Possible values: + * - 0: No integrity checking + * - 1: Safe + * - 2: Caution + * - Bits 2-7: Reserved field. + * @num_svs: Number of satellites used in the solution (8 bits). + * @hdop: Horizontal Dilution of Precision (1e-2) (16 bits). + * @pdop: Positional Dilution of Precision (1e-2) (16 bits). + * @geoidal_separation: Geoidal Separation in 0.01 meters (32 bits). + * @num_ref_stations: Number of reference stations (8 bits). + * + * This structure defines the fields for the GNSS Position Data packet. + */ +struct nmea2000_gnss_position_data_packet { + uint8_t sid; + __le16 date; + __le32 time; + __le64 latitude; + __le64 longitude; + __le64 altitude; + uint8_t gnss_info; + uint8_t status; + uint8_t num_svs; + __le16 hdop; + __le16 pdop; + __le32 geoidal_separation; + uint8_t num_ref_stations; +} __attribute__((__packed__)); + +/** + * nmea2000_gnss_get_sid - Get the sequence identifier + * @packet: Pointer to the NMEA2000 GNSS position data packet + * + * Return: Sequence identifier (8 bits) + */ +static inline uint8_t +nmea2000_gnss_get_sid(const struct nmea2000_gnss_position_data_packet *packet) +{ + return packet->sid; +} + +/** + * nmea2000_gnss_set_sid - Set the sequence identifier + * @packet: Pointer to the NMEA2000 GNSS position data packet + * @sid: Sequence identifier to set + */ +static inline void +nmea2000_gnss_set_sid(struct nmea2000_gnss_position_data_packet *packet, + uint8_t sid) +{ + packet->sid = sid; +} + +/** + * nmea2000_gnss_get_date - Get the UTC date + * @packet: Pointer to the NMEA2000 GNSS position data packet + * + * Return: UTC date in days since January 1, 1970 + */ +static inline uint16_t +nmea2000_gnss_get_date(const struct nmea2000_gnss_position_data_packet *packet) +{ + return le16toh(packet->date); +} + +/** + * nmea2000_gnss_set_date - Set the UTC date + * @packet: Pointer to the NMEA2000 GNSS position data packet + * @date: UTC date in days since January 1, 1970 + */ +static inline void +nmea2000_gnss_set_date(struct nmea2000_gnss_position_data_packet *packet, + uint16_t date) +{ + packet->date = htole16(date); +} + +/** + * nmea2000_gnss_get_time - Get the UTC time + * @packet: Pointer to the NMEA2000 GNSS position data packet + * + * Return: UTC time in 0.0001 seconds since midnight + */ +static inline uint32_t +nmea2000_gnss_get_time(const struct nmea2000_gnss_position_data_packet *packet) +{ + return le32toh(packet->time); +} + +/** + * nmea2000_gnss_set_time - Set the UTC time + * @packet: Pointer to the NMEA2000 GNSS position data packet + * @time: UTC time in 0.0001 seconds since midnight + */ +static inline void +nmea2000_gnss_set_time(struct nmea2000_gnss_position_data_packet *packet, + uint32_t time) +{ + packet->time = htole32(time); +} + +/** + * nmea2000_gnss_get_latitude - Get the latitude + * @packet: Pointer to the NMEA2000 GNSS position data packet + * + * Return: Latitude in 1e-16 degrees (int64_t) + */ +static inline int64_t +nmea2000_gnss_get_latitude(const struct nmea2000_gnss_position_data_packet *packet) +{ + return le64toh(packet->latitude); +} + +/** + * nmea2000_gnss_set_latitude - Set the latitude + * @packet: Pointer to the NMEA2000 GNSS position data packet + * @latitude: Latitude in 1e-16 degrees (int64_t) + */ +static inline void +nmea2000_gnss_set_latitude(struct nmea2000_gnss_position_data_packet *packet, + int64_t latitude) +{ + packet->latitude = htole64(latitude); +} + +/** + * nmea2000_gnss_get_longitude - Get the longitude + * @packet: Pointer to the NMEA2000 GNSS position data packet + * + * Return: Longitude in 1e-16 degrees (int64_t) + */ +static inline int64_t +nmea2000_gnss_get_longitude(const struct nmea2000_gnss_position_data_packet *packet) +{ + return le64toh(packet->longitude); +} + +/** + * nmea2000_gnss_set_longitude - Set the longitude + * @packet: Pointer to the NMEA2000 GNSS position data packet + * @longitude: Longitude in 1e-16 degrees (int64_t) + */ +static inline void +nmea2000_gnss_set_longitude(struct nmea2000_gnss_position_data_packet *packet, + int64_t longitude) +{ + packet->longitude = htole64(longitude); +} + +/** + * nmea2000_gnss_get_altitude - Get the altitude + * @packet: Pointer to the NMEA2000 GNSS position data packet + * + * Return: Altitude in 1e-6 meters above WGS-84 (int64_t) + */ +static inline int64_t +nmea2000_gnss_get_altitude(const struct nmea2000_gnss_position_data_packet *packet) +{ + return le64toh(packet->altitude); +} + +/** + * nmea2000_gnss_set_altitude - Set the altitude + * @packet: Pointer to the NMEA2000 GNSS position data packet + * @altitude: Altitude in 1e-6 meters above WGS-84 (int64_t) + */ +static inline void +nmea2000_gnss_set_altitude(struct nmea2000_gnss_position_data_packet *packet, + int64_t altitude) +{ + packet->altitude = htole64(altitude); +} + +/** + * nmea2000_get_gnss_type - Extract GNSS type from the packet + * @packet: Pointer to the NMEA2000 GNSS position data packet + * + * Return: GNSS type (lower 4 bits of gnss_info field) + */ +static inline uint8_t +nmea2000_get_gnss_type(const struct nmea2000_gnss_position_data_packet *packet) +{ + return FIELD_GET(NMEA2000_GNSS_TYPE_MASK, packet->gnss_info); +} + +/** + * nmea2000_get_gnss_method - Extract GNSS method from the packet + * @packet: Pointer to the NMEA2000 GNSS position data packet + * + * Return: GNSS method (upper 4 bits of gnss_info field) + */ +static inline uint8_t +nmea2000_get_gnss_method(const struct nmea2000_gnss_position_data_packet *packet) +{ + return FIELD_GET(NMEA2000_GNSS_METHOD_MASK, packet->gnss_info); +} + +/** + * nmea2000_set_gnss_info - Set GNSS type and method in the packet + * @packet: Pointer to the NMEA2000 GNSS position data packet + * @gnss_type: GNSS type to set + * @gnss_method: GNSS method to set + */ +static inline void +nmea2000_set_gnss_info(struct nmea2000_gnss_position_data_packet *packet, + enum nmea2000_gnss_type gnss_type, + enum nmea2000_gnss_method gnss_method) +{ + packet->gnss_info = FIELD_PREP(NMEA2000_GNSS_TYPE_MASK, gnss_type) | + FIELD_PREP(NMEA2000_GNSS_METHOD_MASK, gnss_method); +} + +/** + * nmea2000_get_integrity - Extract integrity status from the packet + * @packet: Pointer to the NMEA2000 GNSS position data packet + * + * Return: Integrity status (lower 2 bits of status field) + */ +static inline uint8_t +nmea2000_get_integrity(const struct nmea2000_gnss_position_data_packet *packet) +{ + return FIELD_GET(NMEA2000_INTEGRITY_MASK, packet->status); +} + +/** + * nmea2000_set_status - Set integrity and reserved fields in the packet + * @packet: Pointer to the NMEA2000 GNSS position data packet + * @integrity: Integrity status to set + * @reserved: Reserved value to set + */ +static inline void +nmea2000_set_status(struct nmea2000_gnss_position_data_packet *packet, + enum nmea2000_integrity_status integrity, uint8_t reserved) +{ + packet->status = FIELD_PREP(NMEA2000_INTEGRITY_MASK, integrity) | + FIELD_PREP(NMEA2000_RESERVED_MASK, reserved); +} + +/** + * nmea2000_gnss_get_num_svs - Get the number of satellites used in the solution + * @packet: Pointer to the NMEA2000 GNSS position data packet + * + * Return: Number of satellites used (8 bits) + */ +static inline uint8_t +nmea2000_gnss_get_num_svs(const struct nmea2000_gnss_position_data_packet *packet) +{ + return packet->num_svs; +} + +/** + * nmea2000_gnss_set_num_svs - Set the number of satellites used in the solution + * @packet: Pointer to the NMEA2000 GNSS position data packet + * @num_svs: Number of satellites used (8 bits) + */ +static inline void +nmea2000_gnss_set_num_svs(struct nmea2000_gnss_position_data_packet *packet, + uint8_t num_svs) +{ + packet->num_svs = num_svs; +} + +/** + * nmea2000_gnss_get_hdop - Get the Horizontal Dilution of Precision (HDOP) + * @packet: Pointer to the NMEA2000 GNSS position data packet + * + * Return: HDOP in 1e-2 units (uint16_t) + */ +static inline uint16_t +nmea2000_gnss_get_hdop(const struct nmea2000_gnss_position_data_packet *packet) +{ + return le16toh(packet->hdop); +} + +/** + * nmea2000_gnss_set_hdop - Set the Horizontal Dilution of Precision (HDOP) + * @packet: Pointer to the NMEA2000 GNSS position data packet + * @hdop: HDOP in 1e-2 units (uint16_t) + */ +static inline void +nmea2000_gnss_set_hdop(struct nmea2000_gnss_position_data_packet *packet, + uint16_t hdop) +{ + packet->hdop = htole16(hdop); +} + +/** + * nmea2000_gnss_get_pdop - Get the Positional Dilution of Precision (PDOP) + * @packet: Pointer to the NMEA2000 GNSS position data packet + * + * Return: PDOP in 1e-2 units (uint16_t) + */ +static inline uint16_t +nmea2000_gnss_get_pdop(const struct nmea2000_gnss_position_data_packet *packet) +{ + return le16toh(packet->pdop); +} + +/** + * nmea2000_gnss_set_pdop - Set the Positional Dilution of Precision (PDOP) + * @packet: Pointer to the NMEA2000 GNSS position data packet + * @pdop: PDOP in 1e-2 units (uint16_t) + */ +static inline void +nmea2000_gnss_set_pdop(struct nmea2000_gnss_position_data_packet *packet, + uint16_t pdop) +{ + packet->pdop = htole16(pdop); +} + +/** + * nmea2000_gnss_get_geoidal_separation - Get the Geoidal Separation + * @packet: Pointer to the NMEA2000 GNSS position data packet + * + * Return: Geoidal Separation in 0.01 meters (uint32_t) + */ +static inline uint32_t +nmea2000_gnss_get_geoidal_separation(const struct nmea2000_gnss_position_data_packet *packet) +{ + return le32toh(packet->geoidal_separation); +} + +/** + * nmea2000_gnss_set_geoidal_separation - Set the Geoidal Separation + * @packet: Pointer to the NMEA2000 GNSS position data packet + * @geoidal_separation: Geoidal Separation in 0.01 meters (uint32_t) + */ +static inline void +nmea2000_gnss_set_geoidal_separation(struct nmea2000_gnss_position_data_packet *packet, + uint32_t geoidal_separation) +{ + packet->geoidal_separation = htole32(geoidal_separation); +} + +/** + * nmea2000_gnss_get_num_ref_stations - Get the number of reference stations + * @packet: Pointer to the NMEA2000 GNSS position data packet + * + * Return: Number of reference stations (8 bits) + */ +static inline uint8_t +nmea2000_gnss_get_num_ref_stations(const struct nmea2000_gnss_position_data_packet *packet) +{ + return packet->num_ref_stations; +} + +/** + * nmea2000_gnss_set_num_ref_stations - Set the number of reference stations + * @packet: Pointer to the NMEA2000 GNSS position data packet + * @num_ref_stations: Number of reference stations (8 bits) + */ +static inline void +nmea2000_gnss_set_num_ref_stations(struct nmea2000_gnss_position_data_packet *packet, + uint8_t num_ref_stations) +{ + packet->num_ref_stations = num_ref_stations; +} + +#define NMEA2000_REF_STATION_TYPE_MASK GENMASK(3, 0) +#define NMEA2000_REF_STATION_ID_MASK GENMASK(15, 4) + +/** + * struct nmea2000_reference_station - Represents the reference station fields + * in PGN 129029 + * + * @type_id: Encodes the type and ID of the reference station (16 bits). + * - Bits 0-3: Type of reference station (4 bits). + * - Values range from 0 to 13, indicating different types of reference + * stations. + * - Bits 4-15: Reference Station ID (12 bits). + * - Unique identifier for the reference station. + * @dgnss_age: Age of DGNSS corrections in 0.01 seconds (16 bits). + * - Indicates the age of the differential GNSS corrections. + * + * This structure defines the optional repeating fields for reference stations + * in the GNSS Position Data packet. Bitfield operations on the type and ID + * are handled using FIELD_GET and FIELD_PREP macros for clarity and safety. + */ +struct nmea2000_reference_station { + __le16 type_id; + __le16 dgnss_age; +} __attribute__((__packed__)); + +/** + * nmea2000_ref_station_get_type - Extract the type of reference station + * @station: Pointer to the NMEA2000 reference station structure + * + * Return: Type of the reference station (4 bits) + */ +static inline uint8_t +nmea2000_ref_station_get_type(const struct nmea2000_reference_station *station) +{ + return FIELD_GET(NMEA2000_REF_STATION_TYPE_MASK, + le16toh(station->type_id)); +} + +/** + * nmea2000_ref_station_get_id - Extract the reference station ID + * @station: Pointer to the NMEA2000 reference station structure + * + * Return: Reference station ID (12 bits) + */ +static inline uint16_t +nmea2000_ref_station_get_id(const struct nmea2000_reference_station *station) +{ + return FIELD_GET(NMEA2000_REF_STATION_ID_MASK, + le16toh(station->type_id)); +} + +/** + * nmea2000_ref_station_set_type_id - Set type and ID of reference station + * @station: Pointer to the NMEA2000 reference station structure + * @type: Type of the reference station (4 bits) + * @id: Reference station ID (12 bits) + */ +static inline void +nmea2000_ref_station_set_type_id(struct nmea2000_reference_station *station, + uint8_t type, uint16_t id) +{ + station->type_id = + htole16(FIELD_PREP(NMEA2000_REF_STATION_TYPE_MASK, type) | + FIELD_PREP(NMEA2000_REF_STATION_ID_MASK, id)); +} + +/** + * nmea2000_ref_station_get_dgnss_age - Get the age of DGNSS corrections + * @station: Pointer to the NMEA2000 reference station structure + * + * Return: Age of DGNSS corrections in 0.01 seconds (16 bits) + */ +static inline uint16_t +nmea2000_ref_station_get_dgnss_age(const struct nmea2000_reference_station *station) +{ + return le16toh(station->dgnss_age); +} + +/** + * nmea2000_ref_station_set_dgnss_age - Set the age of DGNSS corrections + * @station: Pointer to the NMEA2000 reference station structure + * @dgnss_age: Age of DGNSS corrections in 0.01 seconds (16 bits) + */ +static inline void +nmea2000_ref_station_set_dgnss_age(struct nmea2000_reference_station *station, + uint16_t dgnss_age) +{ + station->dgnss_age = htole16(dgnss_age); +} + +#endif /* !_J1939_VEHICLE_POSITION_H_ */ diff --git a/j1939_vehicle_position/j1939_vehicle_position_srv.c b/j1939_vehicle_position/j1939_vehicle_position_srv.c new file mode 100644 index 0000000..1ed9c74 --- /dev/null +++ b/j1939_vehicle_position/j1939_vehicle_position_srv.c @@ -0,0 +1,1514 @@ +// SPDX-License-Identifier: LGPL-2.0-only +// SPDX-FileCopyrightText: 2024 Oleksij Rempel + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "j1939_vehicle_position_cmn.h" + +#define J1939_VP_SRV_MAX_EPOLL_EVENTS 10 + +#define PROFILE_J1939 BIT(0) +#define PROFILE_NMEA2000 BIT(1) + +struct j1939_vp_srv_priv; + +struct j1939_pgn_handler { + uint32_t pgn; + int (*prepare_data)(struct j1939_vp_srv_priv *priv, void *data); + int sock; + int sock_priority; + struct timespec last_time; + struct timespec next_time; + int repetition_rate_ms; + int jitter_ms; + size_t data_size; + uint8_t profile; +}; + +struct j1939_vp_srv_priv { + struct sockaddr_can sockname; + struct j1939_vp_stats stats; + struct libj1939_cmn cmn; + bool sim_mode; + struct gps_data_t gps_data; + uint8_t sid; + struct j1939_pgn_handler *handlers; + size_t num_handlers; + uint8_t profile; +}; + + +/** + * timespec_to_nmea2000_datetime - Convert timespec_t to NMEA 2000 date and time + * + * @ts: Pointer to the timespec_t structure representing the time + * @date: Pointer to store the converted NMEA 2000 date (days since 1970-01-01) + * or NULL + * @time: Pointer to store the converted NMEA 2000 time (0.0001 seconds since + * midnight) or NULL + * + * This function converts the provided timespec_t structure to NMEA 2000 date + * and time formats. The date is calculated as the number of days since + * 1970-01-01, and the time is calculated as 0.0001 seconds since midnight. + */ +static void timespec_to_nmea2000_datetime(const struct timespec *ts, + uint16_t *date, uint32_t *time) +{ + time_t time_secs = ts->tv_sec; + long gps_nsec = ts->tv_nsec; + + if (date) { + /* Calculate the number of days since January 1, 1970 */ + *date = time_secs / 86400; + } + + if (time) { + /* Calculate the time of day in 0.0001 seconds since midnight */ + *time = (time_secs % 86400) * 10000 + gps_nsec / 100000; + } +} + +/** + * update_real_gps_data - Update the GPS data from the GPS device. + * @priv: Pointer to the private data structure of the J1939 VP server. + * + * This function checks if there is new data available from the GPS device. + * It then attempts to read the data and verifies if the GPS mode is set + * properly. The function returns 0 on success and propagates original error + * codes from gpsd library functions on failure. + * + * Return: 0 on success, negative error code on failure. + */ +static int update_real_gps_data(struct j1939_vp_srv_priv *priv) +{ + static time_t last_warn_time; + static bool gps_waiting_error; + int ret; + + if (!gps_waiting(&priv->gps_data, 0)) { + if (errno) { + ret = -errno; + pr_warn("gps_waiting() error: %s\n", strerror(errno)); + return ret; + } else { + time_t now = time(NULL); + + /* Warn only once every 10 seconds */ + if (!gps_waiting_error) { + last_warn_time = now; + gps_waiting_error = true; + return -EAGAIN; + } else if (now - last_warn_time > 10) { + last_warn_time = now; + pr_warn("No GPS data available\n"); + return -ENODATA; + } else { + return -EAGAIN; + } + } + } + + gps_waiting_error = false; + + ret = gps_read(&priv->gps_data, NULL, 0); + if (ret == -1) { + if (errno) { + ret = -errno; + pr_warn("gps_read() Unix-level error: %s\n", strerror(errno)); + return ret; + } else { + pr_warn("gps_read() returned -1 without setting errno, possibly connection closed or shared memory unavailable.\n"); + return -EIO; + } + } else if (ret == 0) { + pr_warn("gps_read() returned 0, no data available.\n"); + return -ENODATA; + } + + if (MODE_SET != (MODE_SET & priv->gps_data.set)) { + pr_warn("GPS mode not set\n"); + return -EINVAL; + } + + priv->sid++; + + return 0; +} + +/** + * update_simulation_gps_data - Simulate GPS data for testing purposes. + * @priv: Pointer to the private data structure of the J1939 VP server. + * + * This function generates simulated GPS data to mimic real-world GPS signals. + * It increments the latitude and longitude slightly with each call, loops the + * number of visible satellites, and adjusts DOP values. This simulation mode + * is useful for testing and debugging when real GPS hardware is not available. + * The data is updated in the priv->gps_data structure, which can be used by + * other parts of the application. + */ +static void simulate_gps_data(struct j1939_vp_srv_priv *priv) +{ + int ret; + + /* The initial coordinates (15.1205, 18.0513) are a fun easter egg, + * based on the author's name, Oleksij Rempel. "ole" from Oleksij sets + * the latitude, and "rem" from Rempel sets the longitude. It's a little + * personal touch that makes the simulation mode unique. + */ + static double sim_latitude = 15.1205; + static double sim_longitude = 18.0513; + static uint8_t sim_satellites = 5; + static double sim_hdop = 0.8; + static double sim_vdop = 1.0; + static double sim_pdop = 1.2; + static double sim_tdop = 1.5; + + /* Increment the simulated data for variability */ + sim_latitude += 0.0001; + sim_longitude += 0.0001; + sim_satellites = (sim_satellites + 1) % 16; // Loop from 0 to 15 + sim_hdop += 0.01; + sim_vdop += 0.01; + sim_pdop += 0.01; + sim_tdop += 0.01; + + /* Ensure the values stay within reasonable bounds */ + if (sim_latitude > 90.0) + sim_latitude = -90.0; + if (sim_longitude > 180.0) + sim_longitude = -180.0; + if (sim_hdop > 2.0) + sim_hdop = 0.8; + if (sim_vdop > 2.5) + sim_vdop = 1.0; + if (sim_pdop > 3.0) + sim_pdop = 1.2; + if (sim_tdop > 3.5) + sim_tdop = 1.5; + + priv->gps_data.fix.latitude = sim_latitude; + priv->gps_data.fix.longitude = sim_longitude; + priv->gps_data.satellites_visible = sim_satellites; + priv->gps_data.dop.hdop = sim_hdop; + priv->gps_data.dop.vdop = sim_vdop; + priv->gps_data.dop.pdop = sim_pdop; + priv->gps_data.dop.tdop = sim_tdop; + priv->gps_data.set = MODE_SET | LATLON_SET | DOP_SET | SATELLITE_SET; + priv->gps_data.fix.mode = MODE_2D; + + /* Set the time to the current system time */ + ret = clock_gettime(CLOCK_REALTIME, &priv->gps_data.fix.time); + if (ret < 0) { + pr_warn("Failed to get current time: %s\n", strerror(errno)); + } else { + priv->gps_data.set |= TIME_SET; + } + + /* Set the speed and track to 0 */ + priv->gps_data.fix.speed = 0.0; + priv->gps_data.fix.track = 0.0; + priv->gps_data.set |= TRACK_SET | SPEED_SET; + + priv->sid++; +} + +static int update_gps_data(struct j1939_vp_srv_priv *priv) +{ + if (priv->sim_mode) { + simulate_gps_data(priv); + return 0; + } + + return update_real_gps_data(priv); +} + +/* ----------------- PGN handlers start ----------------- */ +/* ----------------- SAE J1939 specific ----------------- */ +/** + * j1939_vp2_get_data - Fills the VP2 packet with current GPS data. + * @priv: Pointer to the server's private data structure. + * @vp2p: Pointer to the VP2 packet structure to populate. + * + * This function retrieves GPS data from the server's private data and fills + * the provided VP2 packet structure with information such as the number of + * visible satellites and various DOP values. The values are converted using + * a scale factor based on assumptions, as the exact specification is not + * defined. + * + * Return: 0 on success. + */ +static int j1939_vp2_get_data(struct j1939_vp_srv_priv *priv, + struct j1939_vp2_packet *vp2p) +{ + uint8_t hdop, vdop, pdop, tdop; + + if (priv->gps_data.set & DOP_SET) { + hdop = priv->gps_data.dop.hdop * 10; + vdop = priv->gps_data.dop.vdop * 10; + pdop = priv->gps_data.dop.pdop * 10; + tdop = priv->gps_data.dop.tdop * 10; + } else { + hdop = UINT8_MAX; + vdop = UINT8_MAX; + pdop = UINT8_MAX; + tdop = UINT8_MAX; + } + + j1939_vp2_set_total_satellites(vp2p, priv->gps_data.satellites_visible); + j1939_vp2_set_hdop(vp2p, hdop); + j1939_vp2_set_vdop(vp2p, vdop); + j1939_vp2_set_pdop(vp2p, pdop); + j1939_vp2_set_tdop(vp2p, tdop); + + return 0; +} + +static int j1939_vp2_prepare_data(struct j1939_vp_srv_priv *priv, void *data) +{ + struct j1939_vp2_packet *vp2p = data; + + return j1939_vp2_get_data(priv, vp2p); +} + +/** + * j1939_vp1_get_data - Populates the VP1 packet with GPS coordinates. + * @priv: Pointer to the server's private data structure. + * @vp1p: Pointer to the VP1 packet structure to populate. + * + * This function retrieves the current GPS coordinates (latitude and longitude) + * from the server's data and populates the provided VP1 packet. It checks for + * a valid GPS fix and converts the coordinates to a format suitable for + * transmission. + * + * Return: 0 on success, or -ENODATA if the GPS data is invalid or unavailable. + */ +static int j1939_vp1_get_data(struct j1939_vp_srv_priv *priv, + struct j1939_vp1_packet *vp1p) +{ + uint32_t latitude, longitude; + + if (priv->gps_data.set & LATLON_SET) { + latitude = (priv->gps_data.fix.latitude + 210.0) * 1e7; + longitude = (priv->gps_data.fix.longitude + 210.0) * 1e7; + } else { + latitude = UINT32_MAX; + longitude = UINT32_MAX; + } + + j1939_vp1_set_latitude(vp1p, latitude); + j1939_vp1_set_longitude(vp1p, longitude); + + return 0; +} + +static int j1939_vp1_prepare_data(struct j1939_vp_srv_priv *priv, void *data) +{ + struct j1939_vp1_packet *vp1p = data; + + return j1939_vp1_get_data(priv, vp1p); +} + +/* ----------------- NMEA 2000 specific ----------------- */ +/** + * nmea2000_sys_time_get_data - Fills the System Time packet with current GPS + * time data. + * @priv: Pointer to the server's private data structure. + * @stp: Pointer to the System Time packet structure to populate. + * + * This function retrieves the current UTC date and time from the GPS data + * provided by gpsd and fills the provided System Time packet structure. The + * date is given as the number of days since January 1, 1970, and the time is + * in 0.0001 seconds since midnight. The sequence identifier (sid) is managed + * by the server's private data structure. + * + * Return: 0 on success, or a negative error code on failure. + */ + +static int nmea2000_sys_time_get_data(struct j1939_vp_srv_priv *priv, + struct nmea2000_sys_time_packet *stp) +{ + enum nmea2000_sys_time_source source; + uint16_t nmea2000_date; + uint32_t nmea2000_time; + + nmea2000_sys_time_set_sid(stp, priv->sid); + + if (priv->sim_mode) + source = NMEA2000_SYS_TIME_SOURCE_LOCAL_CRYSTAL; + else + source = NMEA2000_SYS_TIME_SOURCE_GPS; + + nmea2000_sys_time_set_source_reserved(stp, source, 0xf); + + if (priv->gps_data.set & TIME_SET) { + timespec_to_nmea2000_datetime(&priv->gps_data.fix.time, + &nmea2000_date, &nmea2000_time); + + } else { + nmea2000_date = UINT16_MAX; + nmea2000_time = UINT32_MAX; + } + + nmea2000_sys_time_set_date(stp, nmea2000_date); + nmea2000_sys_time_set_time(stp, nmea2000_time); + + return 0; +} + +/** + * nmea2000_sys_time_prepare_data - Prepares the data for the System Time packet. + * @priv: Pointer to the server's private data structure. + * @data: Pointer to the buffer where the packet data will be stored. + * + * This function calls the nmea2000_sys_time_get_data function to populate the + * System Time packet structure with the current time data and stores it in the + * provided buffer. + * + * Return: 0 on success, or a negative error code on failure. + */ +static int +nmea2000_sys_time_prepare_data(struct j1939_vp_srv_priv *priv, void *data) +{ + struct nmea2000_sys_time_packet *stp = data; + + return nmea2000_sys_time_get_data(priv, stp); +} + +/** + * nmea2000_mag_var_get_data - Fills the Magnetic Variation packet with current + * data. + * @priv: Pointer to the server's private data structure. + * @mvp: Pointer to the Magnetic Variation packet structure to populate. + * + * This function retrieves the current magnetic variation data, including the + * source, age of service, and the magnetic variation itself. The values are + * filled into the provided Magnetic Variation packet structure. + * + * Return: 0 on success, or a negative error code on failure. + */ +static int nmea2000_mag_var_get_data(struct j1939_vp_srv_priv *priv, + struct nmea2000_mag_var_packet *mvp) +{ + nmea2000_mag_var_set_sid(mvp, priv->sid); + + /* FIXME: provide valid values */ + nmea2000_mag_var_set_source_reserved(mvp, MAGNETIC_VARIATION_MANUAL, + 0xf); + nmea2000_mag_var_set_age_of_service(mvp, UINT32_MAX); + nmea2000_mag_var_set_variation(mvp, UINT16_MAX); + + return 0; +} + +/** + * nmea2000_mag_var_prepare_data - Prepares the data for the Magnetic Variation + * packet. + * @priv: Pointer to the server's private data structure. + * @data: Pointer to the buffer where the packet data will be stored. + * + * This function calls the nmea2000_mag_var_get_data function to populate the + * Magnetic Variation packet structure with the current data and stores it in + * the provided buffer. + * + * Return: 0 on success, or a negative error code on failure. + */ +static int +nmea2000_mag_var_prepare_data(struct j1939_vp_srv_priv *priv, void *data) +{ + struct nmea2000_mag_var_packet *mvp = data; + + return nmea2000_mag_var_get_data(priv, mvp); +} + +/** + * nmea2000_position_rapid_get_data - Fills the Position, Rapid Update packet + * with current GPS data. + * @priv: Pointer to the server's private data structure. + * @prp: Pointer to the Position, Rapid Update packet structure to populate. + * + * This function retrieves the current latitude and longitude from the GPS data + * provided by gpsd and fills the provided Position, Rapid Update packet + * structure. The latitude and longitude are provided in units of 1e-7 degrees, + * with negative values indicating south and west, and positive values + * indicating north and east. + * + * Return: 0 on success, or a negative error code on failure. + */ +static int nmea2000_position_rapid_get_data(struct j1939_vp_srv_priv *priv, + struct nmea2000_position_rapid_packet *prp) +{ + int32_t latitude, longitude; + + if (priv->gps_data.set & LATLON_SET) { + latitude = priv->gps_data.fix.latitude * 1e7; + longitude = priv->gps_data.fix.longitude * 1e7; + } else { + latitude = INT32_MAX; + longitude = INT32_MAX; + } + + nmea2000_position_set_latitude(prp, latitude); + nmea2000_position_set_longitude(prp, longitude); + + return 0; +} + +/** + * nmea2000_position_rapid_prepare_data - Prepares the data for the Position, + * Rapid Update packet. + * @priv: Pointer to the server's private data structure. + * @data: Pointer to the buffer where the packet data will be stored. + * + * This function calls the nmea2000_position_rapid_get_data function to populate + * the Position, Rapid Update packet structure with the current GPS data and + * stores it in the provided buffer. + * + * Return: 0 on success, or a negative error code on failure. + */ +static int nmea2000_position_rapid_prepare_data(struct j1939_vp_srv_priv *priv, + void *data) +{ + struct nmea2000_position_rapid_packet *prp = data; + + return nmea2000_position_rapid_get_data(priv, prp); +} + +/** + * nmea2000_cog_sog_rapid_get_data - Fills the COG and SOG, Rapid Update packet + * with current GPS data. + * @priv: Pointer to the server's private data structure. + * @csr: Pointer to the COG and SOG, Rapid Update packet structure to populate. + * + * This function retrieves the current Course Over Ground (COG) and Speed Over + * Ground (SOG) from the GPS data provided by gpsd and fills the provided COG + * and SOG, Rapid Update packet structure. The COG is given in units of 1e-4 + * radians and the SOG in 1e-2 m/s. + * + * Return: 0 on success, or a negative error code on failure. + */ +static int nmea2000_cog_sog_rapid_get_data(struct j1939_vp_srv_priv *priv, + struct nmea2000_cog_sog_rapid_packet *csr) +{ + uint16_t cog, sog; + + nmea2000_cog_sog_set_sid(csr, priv->sid); + + /* FIXME: set proper COG reference */ + nmea2000_cog_sog_set_cog_ref_res1(csr, NMEA2000_COG_REFERENCE_ERROR, + 0x3f); + + csr->reserved2 = UINT16_MAX; + + if (!(priv->gps_data.set & TRACK_SET)) + cog = UINT16_MAX; + else + /* COG in 1e-4 radians */ + cog = priv->gps_data.fix.track * 10000; + + if (!(priv->gps_data.set & SPEED_SET)) + sog = UINT16_MAX; + else + /* SOG in 1e-2 m/s */ + sog = priv->gps_data.fix.speed * 100; + + + nmea2000_cog_sog_set_cog(csr, cog); + nmea2000_cog_sog_set_sog(csr, sog); + + return 0; +} + +/** + * nmea2000_cog_sog_rapid_prepare_data - Prepares the data for the COG and SOG, + * Rapid Update packet. + * @priv: Pointer to the server's private data structure. + * @data: Pointer to the buffer where the packet data will be stored. + * + * This function calls the nmea2000_cog_sog_rapid_get_data function to populate + * the COG and SOG, Rapid Update packet structure with the current GPS data and + * stores it in the provided buffer. + * + * Return: 0 on success, or a negative error code on failure. + */ +static int nmea2000_cog_sog_rapid_prepare_data(struct j1939_vp_srv_priv *priv, + void *data) +{ + struct nmea2000_cog_sog_rapid_packet *csr = data; + + return nmea2000_cog_sog_rapid_get_data(priv, csr); +} + +/** + * gpsd_system_to_nmea2000_gnss_system_type - Converts GPSD system type to + * NMEA 2000 GNSS system type. + * @system: The GPSD system type identifier. + * + * This function maps GPSD system type identifiers to corresponding NMEA 2000 + * GNSS system types. + * + * Return: The NMEA 2000 GNSS system type corresponding to the input GPSD + * system type. + */ +static enum nmea2000_gnss_type +gpsd_system_to_nmea2000_gnss_system_type(int system) +{ + switch (system) { + case NAVSYSTEM_GPS: + return GNSS_TYPE_GPS; + case NAVSYSTEM_GLONASS: + return GNSS_TYPE_GLONASS; + case NAVSYSTEM_GALILEO: + return GNSS_TYPE_GALILEO; + default: + return GNSS_TYPE_GPS; + } +} + +/** + * gpsd_mode_to_nmea2000_gnss_method - Converts GPSD mode to NMEA 2000 GNSS + * method. + * @mode: The GPSD mode identifier. + * + * This function translates the GPSD mode (such as no fix, 2D, 3D) to the + * corresponding NMEA 2000 GNSS method. + * + * Return: The NMEA 2000 GNSS method corresponding to the input GPSD mode. + */ +static enum nmea2000_gnss_method gpsd_mode_to_nmea2000_gnss_method(int mode) +{ + switch (mode) { + case MODE_NO_FIX: + return GNSS_METHOD_NO_GNSS; + case MODE_2D: + return GNSS_METHOD_GNSS_FIX; + case MODE_3D: + return GNSS_METHOD_PRECISE_GNSS; + default: + return GNSS_METHOD_NO_GNSS; + } +} + +/** + * nmea2000_gnss_position_data_get_data - Fills the GNSS Position Data packet + * with current GPS data. + * @priv: Pointer to the server's private data structure. + * @gpdp: Pointer to the GNSS Position Data packet structure to populate. + * + * This function retrieves GNSS position data, including date, time, latitude, + * longitude, altitude, and various other GNSS-related parameters. The data is + * obtained from the gpsd interface and converted to the appropriate units and + * formats for the NMEA 2000 protocol. + * + * Return: 0 on success, or a negative error code on failure. + */ +static int +nmea2000_gnss_position_data_get_data(struct j1939_vp_srv_priv *priv, + struct nmea2000_gnss_position_data_packet *gpdp) +{ + uint64_t latitude, longitude, altitude; + enum nmea2000_gnss_method gnss_method; + uint16_t nmea2000_date, hdop, pdop; + enum nmea2000_gnss_type gnss_type; + uint32_t nmea2000_time; + + nmea2000_gnss_set_sid(gpdp, priv->sid); + + /* FIXME: find out, what should be used for not supported fields - + * UINT*_MAX or INT*_MAX + */ + if (priv->gps_data.set & TIME_SET) { + timespec_to_nmea2000_datetime(&priv->gps_data.fix.time, + &nmea2000_date, &nmea2000_time); + + } else { + nmea2000_date = UINT16_MAX; + nmea2000_time = UINT32_MAX; + } + + nmea2000_gnss_set_date(gpdp, nmea2000_date); + nmea2000_gnss_set_time(gpdp, nmea2000_time); + + if (priv->gps_data.set & LATLON_SET) { + latitude = priv->gps_data.fix.latitude * 1e16; + longitude = priv->gps_data.fix.longitude * 1e16; + } else { + latitude = INT64_MAX; + longitude = INT64_MAX; + } + + nmea2000_gnss_set_latitude(gpdp, latitude); + nmea2000_gnss_set_longitude(gpdp, longitude); + + if (priv->gps_data.set & ALTITUDE_SET) { + altitude = priv->gps_data.fix.altitude * 1e6; + } else { + altitude = INT64_MAX; + } + + nmea2000_gnss_set_altitude(gpdp, altitude); + + /* FIXME: This is hardcoded to GPS for now. Need to add support for + * other systems. + */ + gnss_type = gpsd_system_to_nmea2000_gnss_system_type(NAVSYSTEM_GPS); + + if (priv->sim_mode) + gnss_method = GNSS_METHOD_SIMULATE_MODE; + else + gnss_method = + gpsd_mode_to_nmea2000_gnss_method(priv->gps_data.fix.mode); + + nmea2000_set_gnss_info(gpdp, gnss_type, gnss_method); + + /* FIXME: no integrity checking is implemented */ + nmea2000_set_status(gpdp, NMEA2000_INTEGRITY_NO_CHECKING, 0xff); + + nmea2000_gnss_set_num_svs(gpdp, priv->gps_data.satellites_visible); + + if (priv->gps_data.set & DOP_SET) { + hdop = priv->gps_data.dop.hdop * 100; + pdop = priv->gps_data.dop.pdop * 100; + } else { + hdop = INT16_MAX; + pdop = INT16_MAX; + } + + nmea2000_gnss_set_hdop(gpdp, hdop); + nmea2000_gnss_set_pdop(gpdp, pdop); + + /* FIXME: use proper values for following fields: */ + nmea2000_gnss_set_geoidal_separation(gpdp, INT32_MAX); + nmea2000_gnss_set_num_ref_stations(gpdp, 0); + + return 0; +} + +/** + * nmea2000_gnss_position_data_prepare_data - Prepares the data for the GNSS + * Position Data packet. + * @priv: Pointer to the server's private data structure. + * @data: Pointer to the buffer where the packet data will be stored. + * + * This function calls nmea2000_gnss_position_data_get_data to populate the + * GNSS Position Data packet structure with the current GPS data and stores it + * in the provided buffer. + * + * Return: 0 on success, or a negative error code on failure. + */ +static int +nmea2000_gnss_position_data_prepare_data(struct j1939_vp_srv_priv *priv, + void *data) +{ + struct nmea2000_gnss_position_data_packet *gpdp = data; + + return nmea2000_gnss_position_data_get_data(priv, gpdp); +} + +/* ----------------- PGN handlers end ----------------- */ + +/** + * prepare_and_send_message - Handles data preparation and transmission for a + * PGN handler. + * @priv: Pointer to the server's private data structure. + * @handler: Pointer to the PGN handler with the necessary data functions. + * + * This function is responsible for preparing the data associated with a specific + * PGN handler and sending it via the corresponding socket. + * + * Return: 0 on success or a negative error code if an issue occurs. + */ +static int prepare_and_send_message(struct j1939_vp_srv_priv *priv, + struct j1939_pgn_handler *handler) +{ + /* Data size is limited to 256 bytes. Probably, it is too much for + * most of the cases. + */ + uint8_t data[256]; + int ret; + + if (sizeof(data) < handler->data_size) { + pr_warn("Data buffer too small for PGN %u: %zu < %zu\n", + handler->pgn, sizeof(data), handler->data_size); + return -EINVAL; + } + + memset(data, 0, handler->data_size); + ret = handler->prepare_data(priv, data); + if (ret < 0) { + pr_warn("Failed to prepare data for PGN %u: %i\n", + handler->pgn, ret); + return ret; + } + + ret = send(handler->sock, data, handler->data_size, MSG_DONTWAIT); + if (ret == -1) { + ret = -errno; + pr_warn("Failed to send data for PGN %u: %i (%s)\n", + handler->pgn, ret, strerror(-ret)); + return ret; + } + + return 0; +} + +/** + * j1939_vp_srv_process_pgn_request - Processes a PGN request message. + * @priv: Pointer to the server's private data structure. + * @msg: Pointer to the received J1939 VP message. + * + * This function processes a PGN request message and sends the corresponding + * PGN message back to the requester. It iterates over the available PGN + * handlers to find the appropriate one based on the requested PGN. + * + * Return: 0 on success or a negative error code if an issue occurs. + */ +static int j1939_vp_srv_process_pgn_request(struct j1939_vp_srv_priv *priv, + struct j1939_vp_msg *msg) +{ + uint32_t requested_pgn = (msg->buf[2] << 16) | (msg->buf[1] << 8) | + msg->buf[0]; + bool gps_data_updated = false; + int ret = -EINVAL; + + /* Iterate over all handlers to find the appropriate PGN */ + for (size_t i = 0; i < priv->num_handlers; ++i) { + struct j1939_pgn_handler *handler = &priv->handlers[i]; + + if (handler->pgn != requested_pgn) + continue; + + if (!(priv->profile & handler->profile)) + continue; + + if (!gps_data_updated) { + ret = update_gps_data(priv); + if (ret < 0) { + pr_warn("failed to update GPS data: %i\n", ret); + return ret; + } + gps_data_updated = true; + } + + ret = prepare_and_send_message(priv, handler); + if (ret < 0) { + pr_warn("Handler for PGN %u returned error %d\n", + handler->pgn, ret); + } + return ret; + } + + pr_warn("No handler found for PGN %u\n", requested_pgn); + return ret; +} + +/** + * j1939_vp_srv_rx_buf - Processes a received J1939 message. + * @priv: Pointer to the server's private data structure. + * @msg: Pointer to the received J1939 VP message. + * + * This function processes a received J1939 message by checking the PGN + * and calling the appropriate handler function. + * Currently, only the PGN_REQUEST_PGN message is supported, and other + * messages are ignored with a warning message. + * + * Return: 0 on success or a negative error code if an issue occurs. + */ +static int j1939_vp_srv_rx_buf(struct j1939_vp_srv_priv *priv, + struct j1939_vp_msg *msg) +{ + pgn_t pgn = msg->peername.can_addr.j1939.pgn; + int ret = 0; + + switch (pgn) { + case J1939_PGN_REQUEST_PGN: + ret = j1939_vp_srv_process_pgn_request(priv, msg); + break; + default: + pr_warn("%s: unsupported PGN: %x\n", __func__, pgn); + /* Not a critical error */ + break; + } + + return ret; +} + +/** + * j1939_vp_srv_rx_one - Receives a single J1939 message from a socket. + * @priv: Pointer to the server's private data structure. + * @sock: The file descriptor of the socket to receive from. + * + * This function receives a single J1939 message from the specified socket + * and processes it by calling j1939_vp_srv_rx_buf. + * + * Return: 0 on success or a negative error code if an issue occurs. + */ +static int j1939_vp_srv_rx_one(struct j1939_vp_srv_priv *priv, int sock) +{ + struct j1939_vp_msg msg = {0}; + int flags = 0; + int ret; + + msg.buf_size = J1939_VP1_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\n", ret, strerror(-ret)); + return ret; + } + + if (ret < 3) { + pr_warn("received too short message: %i\n", ret); + return -EINVAL; + } + + msg.len = ret; + + ret = j1939_vp_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; +} + +/** + * j1939_vp_srv_handle_events - Handles events for the J1939 VP server. + * @priv: Pointer to the server's private data structure. + * @nfds: The number of file descriptors to handle. + * + * This function processes events for the J1939 VP server by checking for + * incoming messages on the sockets and calling the appropriate handler + * functions. + * + * Return: 0 on success or a negative error code if an issue occurs. + */ +static int j1939_vp_srv_handle_events(struct j1939_vp_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_vp_srv_rx_one(priv, ev->data.fd); + if (ret) { + warn("recv one"); + return ret; + } + } + } + return 0; +} + +/** + * determine_earliest_next_send_time - Determines the earliest next send time. + * @priv: Pointer to the server's private data structure. + * + * This function determines the earliest next send time for all PGN handlers + * based on their repetition rates and last send times. + * + * Return: the earliest time as a struct timespec. + */ +static struct timespec +determine_earliest_next_send_time(struct j1939_vp_srv_priv *priv) +{ + struct timespec earliest = {0, 0}; + + for (size_t i = 1; i < priv->num_handlers; ++i) { + if (!(priv->profile & priv->handlers[i].profile)) + continue; + + if (earliest.tv_sec == 0 && earliest.tv_nsec == 0) + earliest = priv->handlers[i].next_time; + + if ((priv->handlers[i].next_time.tv_sec < earliest.tv_sec) || + (priv->handlers[i].next_time.tv_sec == earliest.tv_sec && + priv->handlers[i].next_time.tv_nsec < earliest.tv_nsec)) + earliest = priv->handlers[i].next_time; + } + + return earliest; +} + +/** + * send_message_for_handler - Sends a periodic message for a PGN handler. + * @priv: Pointer to the server's private data structure. + * @handler: Pointer to the PGN handler structure. + * + * This function sends a periodic message for a specific PGN handler based on + * the repetition rate and jitter. It calculates the time difference between + * the last and next send times and sends the message if the time is within + * the jitter range. It updates the last and next send times for the handler. + * + * Returns 0 on success or a negative error code if an issue occurs. + */ +static int send_message_for_handler(struct j1939_vp_srv_priv *priv, + struct j1939_pgn_handler *handler) +{ + int64_t time_diff; + int ret; + + if (!(priv->profile & handler->profile)) + return 0; + + time_diff = timespec_diff_ms(&handler->next_time, &priv->cmn.last_time); + if (time_diff > handler->jitter_ms) + return 0; + + ret = prepare_and_send_message(priv, handler); + if (ret < 0) + return ret; + + handler->last_time = priv->cmn.last_time; + handler->next_time = priv->cmn.last_time; + timespec_add_ms(&handler->next_time, handler->repetition_rate_ms); + + return 0; +} + +/** + * send_periodic_messages - Sends periodic messages for all PGN handlers. + * @priv: Pointer to the server's private data structure. + * + * This function sends periodic messages for all PGN handlers based on their + * repetition rates and jitter. It iterates over all handlers and calls the + * send_message_for_handler function to send the messages. + * + * Return: 0 on success or a negative error code if an issue occurs. + */ +static int send_periodic_messages(struct j1939_vp_srv_priv *priv) +{ + int ret = 0; + + for (size_t i = 0; i < priv->num_handlers; ++i) { + ret = send_message_for_handler(priv, &priv->handlers[i]); + if (ret < 0) { + pr_warn("Failed to send periodic message for handler %zu. Error: %d (%s)\n", + i, ret, strerror(-ret)); + } + } + + return ret; +} + +/** + * j1939_vp_srv_process_events_and_tasks - Processes events and tasks for the + * J1939 VP server. + * @priv: Pointer to the server's private data structure. + * + * This function processes events and tasks for the J1939 VP server by preparing + * for events, handling events, updating GPS data, and sending periodic + * messages. + * + * Return: 0 on success or a negative error code if an issue occurs. + */ +static int j1939_vp_srv_process_events_and_tasks(struct j1939_vp_srv_priv *priv) +{ + int64_t time_diff; + int ret, nfds; + + priv->cmn.next_send_time = determine_earliest_next_send_time(priv); + ret = libj1939_prepare_for_events(&priv->cmn, &nfds, false); + if (ret) + pr_err("failed to prepare for events: %i (%s)\n", ret, + strerror(-ret)); + + if (!ret && nfds > 0) { + ret = j1939_vp_srv_handle_events(priv, nfds); + if (ret) + pr_err("failed to handle events: %i (%s)\n", ret, + strerror(-ret)); + } + + /* Test if it is proper time to send next status message. */ + time_diff = timespec_diff_ms(&priv->cmn.next_send_time, + &priv->cmn.last_time); + if (time_diff > 0) { + /* Too early to send next message */ + return 0; + } + + ret = update_gps_data(priv); + if (ret < 0 && ret != -EAGAIN) + pr_warn("failed to update GPS data: %i\n", ret); + + return send_periodic_messages(priv); +} + +static struct j1939_pgn_handler pgn_handlers[] = { + /* SAE J1939 specific PGNs */ + { + .pgn = J1939_PGN_VP1, + .prepare_data = j1939_vp1_prepare_data, + .sock_priority = J1939_VP1_PRIO_DEFAULT, + .repetition_rate_ms = J1939_VP1_REPETITION_RATE_MS, + .jitter_ms = J1939_VP1_JITTER_MS, + .data_size = sizeof(struct j1939_vp1_packet), + .profile = PROFILE_J1939, + }, + { + .pgn = J1939_PGN_VP2, + .prepare_data = j1939_vp2_prepare_data, + .sock_priority = J1939_VP2_PRIO_DEFAULT, + .repetition_rate_ms = J1939_VP2_REPETITION_RATE_MS, + .jitter_ms = J1939_VP2_JITTER_MS, + .data_size = sizeof(struct j1939_vp2_packet), + .profile = PROFILE_J1939, + }, + /* NMEA 2000 specific PGNs */ + { + .pgn = NMEA2000_PGN_SYS_TIME, + .prepare_data = nmea2000_sys_time_prepare_data, + .sock_priority = NMEA2000_SYS_TIME_PRIO_DEFAULT, + .repetition_rate_ms = NMEA2000_SYS_TIME_REPETITION_RATE_MS, + .jitter_ms = NMEA2000_SYS_TIME_JITTER_MS, + .data_size = NMEA2000_SYS_TIME_MAX_TRANSFER_LENGTH, + .profile = PROFILE_NMEA2000, + }, + { + .pgn = NMEA2000_PGN_MAG_VAR, + .prepare_data = nmea2000_mag_var_prepare_data, + .sock_priority = NMEA2000_MAG_VAR_PRIO_DEFAULT, + .repetition_rate_ms = NMEA2000_MAG_VAR_REPETITION_RATE_MS, + .jitter_ms = NMEA2000_MAG_VAR_JITTER_MS, + .data_size = NMEA2000_MAG_VAR_MAX_TRANSFER_LENGTH, + .profile = 0, /* currently we can't provide this data */ + }, + { + .pgn = NMEA2000_PGN_POSITION_RAPID, + .prepare_data = nmea2000_position_rapid_prepare_data, + .sock_priority = NMEA2000_POSITION_RAPID_PRIO_DEFAULT, + .repetition_rate_ms = NMEA2000_POSITION_RAPID_REPETITION_RATE_MS, + .jitter_ms = NMEA2000_POSITION_RAPID_JITTER_MS, + .data_size = NMEA2000_POSITION_RAPID_MAX_TRANSFER_LENGTH, + .profile = PROFILE_NMEA2000, + }, + { + .pgn = NMEA2000_PGN_COG_SOG_RAPID, + .prepare_data = nmea2000_cog_sog_rapid_prepare_data, + .sock_priority = NMEA2000_COG_SOG_RAPID_PRIO_DEFAULT, + .repetition_rate_ms = NMEA2000_COG_SOG_RAPID_REPETITION_RATE_MS, + .jitter_ms = NMEA2000_COG_SOG_RAPID_JITTER_MS, + .data_size = NMEA2000_COG_SOG_RAPID_MAX_TRANSFER_LENGTH, + .profile = PROFILE_NMEA2000, + }, + { + .pgn = NMEA2000_PGN_GNSS_POSITION_DATA, + .prepare_data = nmea2000_gnss_position_data_prepare_data, + .sock_priority = NMEA2000_GNSS_POSITION_DATA_PRIO_DEFAULT, + .repetition_rate_ms = NMEA2000_GNSS_POSITION_DATA_REPETITION_RATE_MS, + .jitter_ms = NMEA2000_GNSS_POSITION_DATA_JITTER_MS, + .data_size = NMEA2000_GNSS_POSITION_DATA_MAX_TRANSFER_LENGTH, + .profile = PROFILE_NMEA2000, + }, +}; + +/** + * initialize_socket_for_handler - Initializes a socket for a PGN handler. + * @priv: Pointer to the server's private data structure. + * @handler: Pointer to the PGN handler structure. + * + * This function initializes a socket for a specific PGN handler by opening, + * binding, and connecting the socket. It sets the socket priority, enables + * broadcast, and adds the socket to the epoll instance for event handling. + * + * Return: 0 on success or a negative error code if an issue occurs. + */ +static int initialize_socket_for_handler(struct j1939_vp_srv_priv *priv, + struct j1939_pgn_handler *handler) +{ + struct sockaddr_can addr = priv->sockname; + int ret; + + ret = libj1939_open_socket(); + if (ret < 0) { + pr_err("Failed to open socket for PGN %u: %d\n", + handler->pgn, ret); + return ret; + } + + handler->sock = ret; + + ret = libj1939_bind_socket(handler->sock, &addr); + if (ret < 0) { + pr_err("Failed to bind socket for PGN %u: %d\n", + handler->pgn, ret); + return ret; + } + + ret = libj1939_socket_prio(handler->sock, handler->sock_priority); + if (ret < 0) { + pr_err("Failed to set socket priority for PGN %u: %d\n", + handler->pgn, ret); + return ret; + } + + ret = libj1939_set_broadcast(handler->sock); + if (ret < 0) { + pr_err("Failed to set broadcast for PGN %u: %d\n", + handler->pgn, ret); + return ret; + } + + addr.can_addr.j1939.name = J1939_NO_NAME; + addr.can_addr.j1939.addr = J1939_NO_ADDR; + addr.can_addr.j1939.pgn = handler->pgn; + ret = libj1939_connect_socket(handler->sock, &addr); + if (ret < 0) { + pr_err("Failed to connect socket for PGN %u: %d\n", + handler->pgn, ret); + return ret; + } + + ret = libj1939_add_socket_to_epoll(priv->cmn.epoll_fd, handler->sock, + EPOLLIN); + if (ret < 0) { + pr_err("Failed to add socket to epoll for PGN %u: %d\n", + handler->pgn, ret); + return ret; + } + + return 0; +} + +/** + * j1939_vp_srv_init - Initializes the J1939 VP server. + * @priv: Pointer to the server's private data structure. + * + * This function sets up the necessary resources for the J1939 VP server, + * including creating an epoll instance, allocating memory for epoll events, + * and initializing sockets for each PGN handler. It registers the handlers, + * assigns their sockets, and sets up initial timing information. + * + * Return: 0 on success or a negative error code if an issue occurs. + */ +static int j1939_vp_srv_init(struct j1939_vp_srv_priv *priv) +{ + struct timespec ts; + int ret; + + ret = libj1939_create_epoll(); + if (ret < 0) { + pr_err("Failed to create epoll: %d\n", ret); + return ret; + } + + priv->cmn.epoll_fd = ret; + priv->cmn.epoll_events = calloc(J1939_VP_SRV_MAX_EPOLL_EVENTS, + sizeof(struct epoll_event)); + if (!priv->cmn.epoll_events) { + pr_err("Failed to allocate memory for epoll events\n"); + return -ENOMEM; + } + priv->cmn.epoll_events_size = J1939_VP_SRV_MAX_EPOLL_EVENTS; + + ret = clock_gettime(CLOCK_MONOTONIC, &ts); + if (ret < 0) { + ret = -errno; + pr_err("Failed to get current time: %d (%s)\n", ret, + strerror(-ret)); + return ret; + } + + priv->handlers = pgn_handlers; + priv->num_handlers = sizeof(pgn_handlers) / sizeof(pgn_handlers[0]); + + for (size_t i = 0; i < priv->num_handlers; ++i) { + struct j1939_pgn_handler *handler = &priv->handlers[i]; + + handler->sock = -1; + if (!(priv->profile & handler->profile)) + continue; + + ret = initialize_socket_for_handler(priv, handler); + if (ret < 0) { + pr_err("Failed to initialize socket for handler %zu: %d\n", + i, ret); + return ret; + } + + handler->next_time = ts; + } + + return 0; +} + +static void j1939_vp_srv_print_help(void) +{ + printf("j1939-vehicle-position-srv - J1939 Vehicle Position Server\n"); + printf("\n"); + printf("This program acts as a J1939 Vehicle Position Server, sending J1939 or NMEA 2000\n"); + printf("messages with vehicle position data. It reads GPS data from gpsd and sends it\n"); + printf("periodically to the specified CAN interface.\n"); + printf("\n"); + printf("Supported PGNs:\n"); + printf(" J1939:\n"); + printf(" - Vehicle Position 1 (PGN 65265)\n"); + printf(" - Vehicle Position 2 (PGN 65266)\n"); + printf(" NMEA 2000:\n"); + printf(" - System Time (PGN 126992)\n"); + printf(" - Position, Rapid Update (PGN 129025)\n"); + printf(" - COG and SOG, Rapid Update (PGN 129026)\n"); + printf(" - GNSS Position Data (PGN 129029)\n"); + printf("\n"); + printf("Usage: j1939-vehicle-position-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(" --sim-mode or -s\n"); + printf(" Enables simulation mode to generate position data instead of using real GPSd data.\n"); + printf("\n"); + printf(" --profile or -p \n"); + printf(" Selects the profile for protocol-specific behavior. Available profiles:\n"); + printf(" - 'j1939': Configures for J1939 protocol, used in heavy-duty vehicles.\n"); + printf(" - 'nmea2000': Configures for NMEA 2000 protocol, used in marine electronics.\n"); + printf("\n"); + printf("Usage Examples:\n"); + printf(" Using local address:\n"); + printf(" j1939-vehicle-position-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-vehicle-position-srv -i vcan0 -n 0x1122334455667789\n"); +} + +/** + * j1939_vp_srv_parse_args - Parses command line arguments for the J1939 VP server. + * @priv: Pointer to the server's private data structure. + * @argc: The number of command line arguments. + * @argv: The array of command line arguments. + * + * This function parses the command line arguments for the J1939 VP server, + * including the interface, local address, local name, and simulation mode. + * It sets the corresponding values in the private data structure. + * + * Return: 0 on success or a negative error code if an issue occurs. + */ +static int j1939_vp_srv_parse_args(struct j1939_vp_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'}, + {"sim-mode", no_argument, 0, 's'}, + {"profile", required_argument, 0, 'p'}, + {0, 0, 0, 0} + }; + + while ((opt = getopt_long(argc, argv, "a:n:i:sp:", 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; + case 's': + priv->sim_mode = true; + break; + case 'p': + if (strcmp(optarg, "j1939") == 0) { + priv->profile |= PROFILE_J1939; + } else if (strcmp(optarg, "nmea2000") == 0) { + priv->profile |= PROFILE_NMEA2000; + } else { + pr_err("Unknown profile: %s\n", optarg); + j1939_vp_srv_print_help(); + return -EINVAL; + } + break; + default: + j1939_vp_srv_print_help(); + return -EINVAL; + } + } + + if (priv->profile == 0) { + pr_info("Profile not specified. Using default profile: j1939\n"); + priv->profile = PROFILE_J1939; + } + + if (!interface_set) { + pr_err("interface not specified\n"); + j1939_vp_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\n"); + j1939_vp_srv_print_help(); + return -EINVAL; + } + + return 0; +} + +/** + * j1939_vp_close_handler_sockets - Closes the sockets for all PGN handlers. + * @priv: Pointer to the server's private data structure. + * + * This function closes the sockets for all PGN handlers by iterating over + * the handlers and closing the socket if it is open. It resets the socket + * descriptor to an invalid value after closing. + */ +static void j1939_vp_close_handler_sockets(struct j1939_vp_srv_priv *priv) +{ + for (size_t i = 0; i < priv->num_handlers; ++i) { + struct j1939_pgn_handler *handler = &priv->handlers[i]; + + if (handler->sock >= 0) { + close(handler->sock); + handler->sock = -1; + } + } +} + +/** + * j1939_vp_srv_gps_open - Opens a connection to the GPS device. + * @priv: Pointer to the server's private data structure. + * + * This function opens a connection to the GPS device using the GPSD library. + * It connects to the GPSD daemon running on localhost and port 2947 and + * enables the GPS stream in JSON format. + * + * Return: 0 on success or a negative error code if an issue occurs. + */ +static int j1939_vp_srv_gps_open(struct j1939_vp_srv_priv *priv) +{ + if (priv->sim_mode) + return 0; + + if (gps_open("localhost", "2947", &priv->gps_data) != 0) { + pr_err("No GPSD running or connection failed.\n"); + return 1; + } + + gps_stream(&priv->gps_data, WATCH_ENABLE | WATCH_JSON, NULL); + + return 0; +} + +/** + * j1939_vp_srv_gps_close - Closes the connection to the GPS device. + * @priv: Pointer to the server's private data structure. + * + * This function closes the connection to the GPS device by disabling the GPS + * stream and closing the GPSD connection. + */ +static void j1939_vp_srv_gps_close(struct j1939_vp_srv_priv *priv) +{ + if (priv->sim_mode) + return; + + gps_stream(&priv->gps_data, WATCH_DISABLE, NULL); + gps_close(&priv->gps_data); +} + +static void j1939_vp_srv_close(struct j1939_vp_srv_priv *priv) +{ + j1939_vp_close_handler_sockets(priv); + + close(priv->cmn.epoll_fd); + free(priv->cmn.epoll_events); +} + +int main(int argc, char *argv[]) +{ + struct j1939_vp_srv_priv *priv; + 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_vp_srv_parse_args(priv, argc, argv); + if (ret) + return ret; + + ret = j1939_vp_srv_init(priv); + if (ret) { + pr_err("failed to initialize: %i (%s)\n", ret, strerror(-ret)); + return ret; + } + + ret = j1939_vp_srv_gps_open(priv); + if (ret) + return ret; + + while (1) { + ret = j1939_vp_srv_process_events_and_tasks(priv); + /* Even if error we continue to do our best. But we need to + * slow down to avoid busy loop. So, we sleep for a while. + */ + if (ret) { + pr_warn("failed to process events and tasks: %i (%s). Sleeping for a while\n", + ret, strerror(-ret)); + sleep(1); + } + } + + j1939_vp_srv_gps_close(priv); + + j1939_vp_srv_close(priv); + free(priv); + + return ret; +} + From e448d542e8dcb441018e9b6bce1946d3d1cc4719 Mon Sep 17 00:00:00 2001 From: Marc Kleine-Budde Date: Mon, 5 Aug 2024 11:09:10 +0200 Subject: [PATCH 3/3] github-actions: install libgps-dev where available --- .github/workflows/compile.yml | 69 +++++++++++++++++++++++++++++++---- 1 file changed, 61 insertions(+), 8 deletions(-) diff --git a/.github/workflows/compile.yml b/.github/workflows/compile.yml index 5c045a8..1c6a067 100644 --- a/.github/workflows/compile.yml +++ b/.github/workflows/compile.yml @@ -30,7 +30,45 @@ jobs: podman run --name stable -di --userns=keep-id:uid=1000,gid=1000 -v "$PWD":/home -w /home ${{ matrix.release }} bash podman exec -i stable uname -a podman exec -i stable id + + - name: Update APT Sources List (Ubuntu Only) + if: + startsWith(matrix.release, 'ubuntu:') && matrix.release != 'ubuntu:20.04' + run: | podman exec -i -u root stable apt update + podman exec -e DEBIAN_FRONTEND='noninteractive' -i -u root stable apt install -o APT::Install-Suggests=false -qy \ + lsb-release + + podman exec -i -u root stable \ + test -e /etc/apt/sources.list && + podman exec -i -u root stable \ + sed -i -e 's|\(http.*:\)|[arch=amd64] \1|g' /etc/apt/sources.list + + podman exec -i -u root stable \ + test -e /etc/apt/sources.list.d/ubuntu.sources && + podman exec -i -u root stable \ + sed -i -e '/^Components:/a Architectures: amd64' /etc/apt/sources.list.d/ubuntu.sources + + echo "deb [arch=armhf,arm64] http://ports.ubuntu.com/ubuntu-ports/ $(podman exec -i stable lsb_release -cs) main restricted universe multiverse" | \ + podman exec -i -u root stable tee -a /etc/apt/sources.list.d/cross.list + echo "deb [arch=armhf,arm64] http://ports.ubuntu.com/ubuntu-ports/ $(podman exec -i stable lsb_release -cs)-updates main restricted universe multiverse" | \ + podman exec -i -u root stable tee -a /etc/apt/sources.list.d/cross.list + echo "deb [arch=armhf,arm64] http://ports.ubuntu.com/ubuntu-ports/ $(podman exec -i stable lsb_release -cs)-backports main restricted universe multiverse" | \ + podman exec -i -u root stable tee -a /etc/apt/sources.list.d/cross.list + + - name: Add Architecture + if: + matrix.release != 'ubuntu:20.04' + run: | + podman exec -i -u root stable dpkg --add-architecture arm64 + podman exec -i -u root stable dpkg --add-architecture armhf + + - name: Install Development Packages + env: + release: ${{ matrix.release == 'debian:experimental' && '-t experimental' || '' }} + run: | + podman exec -i -u root stable apt update + podman exec -e DEBIAN_FRONTEND='noninteractive' -i -u root stable apt upgrade -o APT::Install-Suggests=false -qy podman exec -e DEBIAN_FRONTEND='noninteractive' -i -u root stable apt install -o APT::Install-Suggests=false -qy ${release} \ clang \ cmake \ @@ -38,52 +76,67 @@ jobs: gcc-aarch64-linux-gnu \ gcc-arm-linux-gnueabihf \ gcc-mips-linux-gnu \ + libgps-dev \ make + - name: Install Cross Libs + env: + release: ${{ matrix.release == 'debian:experimental' && '-t experimental' || '' }} + if: + matrix.release != 'ubuntu:20.04' + run: | + podman exec -e DEBIAN_FRONTEND='noninteractive' -i -u root stable apt install -o APT::Install-Suggests=false -qy ${release} \ + libgps-dev:arm64 \ + libgps-dev:armhf + - name: Configure & Build with gcc env: cc: gcc run: | - podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_C_COMPILER=${cc} -DENABLE_WERROR=ON -B build-${cc} + podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_C_COMPILER=${cc} -DENABLE_WERROR=ON -DENABLE_GPS=ON -B build-${cc} podman exec -i stable cmake --build build-${cc} - name: Configure & Build with clang env: cc: clang run: | - podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_C_COMPILER=${cc} -DENABLE_WERROR=ON -B build-${cc} + podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_C_COMPILER=${cc} -DENABLE_WERROR=ON -DENABLE_GPS=ON -B build-${cc} podman exec -i stable cmake --build build-${cc} - name: Configure & Build with arm-linux-gnueabihf-gcc env: toolchain: arm-linux-gnueabihf-gcc + gps: ${{ matrix.release == 'ubuntu:20.04' && 'OFF' || 'ON' }} run: | - podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_TOOLCHAIN_FILE=cmake/${toolchain}.cmake -DENABLE_WERROR=ON -B build-${toolchain} + podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_TOOLCHAIN_FILE=cmake/${toolchain}.cmake -DENABLE_WERROR=ON -DENABLE_GPS=${gps} -B build-${toolchain} podman exec -i stable cmake --build build-${toolchain} - name: Configure & Build with arm-linux-gnueabihf-clang if: - ${{ matrix.release != 'ubuntu:20.04' && matrix.release != 'debian:oldstable-slim' }} + matrix.release != 'ubuntu:20.04' && matrix.release != 'debian:oldstable-slim' env: toolchain: arm-linux-gnueabihf-clang + gps: ${{ matrix.release == 'ubuntu:20.04' && 'OFF' || 'ON' }} run: | - podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_TOOLCHAIN_FILE=cmake/${toolchain}.cmake -DENABLE_WERROR=ON -B build-${toolchain} + podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_TOOLCHAIN_FILE=cmake/${toolchain}.cmake -DENABLE_WERROR=ON -DENABLE_GPS=${gps} -B build-${toolchain} podman exec -i stable cmake --build build-${toolchain} - name: Configure & Build with aarch64-linux-gnu-gcc env: toolchain: aarch64-linux-gnu-gcc + gps: ${{ matrix.release == 'ubuntu:20.04' && 'OFF' || 'ON' }} run: | - podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_TOOLCHAIN_FILE=cmake/${toolchain}.cmake -DENABLE_WERROR=ON -B build-${toolchain} + podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_TOOLCHAIN_FILE=cmake/${toolchain}.cmake -DENABLE_WERROR=ON -DENABLE_GPS=${gps} -B build-${toolchain} podman exec -i stable cmake --build build-${toolchain} - name: Configure & Build with aarch64-linux-gnu-clang if: - ${{ matrix.release != 'ubuntu:20.04' && matrix.release != 'debian:oldstable-slim' }} + matrix.release != 'ubuntu:20.04' && matrix.release != 'debian:oldstable-slim' env: toolchain: aarch64-linux-gnu-clang + gps: ${{ matrix.release == 'ubuntu:20.04' && 'OFF' || 'ON' }} run: | - podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_TOOLCHAIN_FILE=cmake/${toolchain}.cmake -DENABLE_WERROR=ON -B build-${toolchain} + podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_TOOLCHAIN_FILE=cmake/${toolchain}.cmake -DENABLE_WERROR=ON -DENABLE_GPS=${gps} -B build-${toolchain} podman exec -i stable cmake --build build-${toolchain} - name: Configure & Build with mips-linux-gnu-gcc