diff --git a/CMakeLists.txt b/CMakeLists.txt index 01f90f94..97ab6fb6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,6 +67,10 @@ set(PROGRAMS_J1939_TIMEDATE j1939-timedate-cli ) +set(PROGRAMS_J1939_VEHICLE_POSITION + j1939-vehicle-position-srv +) + set(PROGRAMS_ISOBUSFS isobusfs-srv isobusfs-cli @@ -190,6 +194,22 @@ if(NOT ANDROID) j1939-timedate-srv DESTINATION ${CMAKE_INSTALL_BINDIR}) + 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 + ) + + install(TARGETS + j1939-vehicle-position-srv + DESTINATION ${CMAKE_INSTALL_BINDIR}) + endif() add_library(can STATIC diff --git a/Makefile b/Makefile index 08366c84..1cbd2de9 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 00000000..5ed6c0b8 --- /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 00000000..1ed9c745 --- /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; +} +