Merge 0cfff2e5da into 95aae6bf83

J1939: VP1/VP2 PG fixes

canlogserver.c

@@ -177,7 +177,8 @@ int main(int argc, char **argv)
 	sigset_t sigset;
 	fd_set rdfs;
 	int s[MAXDEV];
-	int socki, accsocket;
+	int socki;
+	int accsocket = -1;
 	canid_t mask[MAXDEV] = {0};
 	canid_t value[MAXDEV] = {0};
 	int inv_filter[MAXDEV] = {0};
@@ -286,7 +287,7 @@ int main(int argc, char **argv)
 	inaddr.sin_addr.s_addr = htonl(INADDR_ANY);
 	inaddr.sin_port = htons(port);
 
-	while(bind(socki, (struct sockaddr*)&inaddr, sizeof(inaddr)) < 0) {
+	while(running && bind(socki, (struct sockaddr*)&inaddr, sizeof(inaddr)) < 0) {
 		struct timespec f = {
 			.tv_nsec = 100 * 1000 * 1000,
 		};
@@ -295,18 +296,28 @@ int main(int argc, char **argv)
 		nanosleep(&f, NULL);
 	}
 
+	/*
+	 * Check if loop exited due to signal (during nanosleep) rather than
+	 * successful bind.
+	 */
+	if (!running) {
+		close(socki);
+		return 128 + signal_num;
+	}
+
 	if (listen(socki, 3) != 0) {
 		perror("listen");
 		exit(1);
 	}
 
-	while(1) {
+	while(running) {
 		accsocket = accept(socki, (struct sockaddr*)&clientaddr, &sin_size);
 		if (accsocket > 0) {
 			//printf("accepted\n");
 			if (!fork())
 				break;
 			close(accsocket);
+			accsocket = -1;
 		}
 		else if (errno != EINTR) {
 			perror("accept");
@@ -314,6 +325,17 @@ int main(int argc, char **argv)
 		}
 	}
 
+	/*
+	 * Check if loop exited due to signal (accept returned EINTR) rather
+	 * than successful fork
+	 */
+	if (!running) {
+		if (accsocket > 0)
+			close(accsocket);
+		close(socki);
+		return 128 + signal_num;
+	}
+
 	for (i=0; i<currmax; i++) {
 
 		pr_debug("open %d '%s' m%08X v%08X i%d e%d.\n",

isotpsniffer.c

@@ -59,6 +59,7 @@
 #include <linux/can.h>
 #include <linux/can/isotp.h>
 #include <linux/sockios.h>
+#include <errno.h>
 
 #define NO_CAN_ID 0xFFFFFFFFU
 
@@ -66,6 +67,8 @@
 #define FORMAT_ASCII 2
 #define FORMAT_DEFAULT (FORMAT_ASCII | FORMAT_HEX)
 
+#define PDU_BUF_SIZE 4096
+
 void print_usage(char *prg)
 {
 	fprintf(stderr, "\nUsage: %s [options] <CAN interface>\n", prg);
@@ -79,6 +82,7 @@ void print_usage(char *prg)
 	fprintf(stderr, "         -f <format>  (1 = HEX, 2 = ASCII, 3 = HEX & ASCII - default: %d)\n", FORMAT_DEFAULT);
 	fprintf(stderr, "         -L           (set link layer options for CAN FD)\n");
 	fprintf(stderr, "         -h <len>     (head: print only first <len> bytes)\n");
+	fprintf(stderr, "         -i           (ignore syscall errors to receive malformed PDUs)\n");
 	fprintf(stderr, "\nCAN IDs and addresses are given and expected in hexadecimal values.\n");
 	fprintf(stderr, "\n");
 }
@@ -189,15 +193,16 @@ int main(int argc, char **argv)
 	int head = 0;
 	int timestamp = 0;
 	int format = FORMAT_DEFAULT;
+	int ignore_errors = 0;
 	canid_t src = NO_CAN_ID;
 	canid_t dst = NO_CAN_ID;
 	extern int optind, opterr, optopt;
 	static struct timeval tv, last_tv;
 
-	unsigned char buffer[4096];
+	unsigned char buffer[PDU_BUF_SIZE];
 	int nbytes;
 
-	while ((opt = getopt(argc, argv, "s:d:x:X:h:ct:f:L?")) != -1) {
+	while ((opt = getopt(argc, argv, "s:d:x:X:h:ct:f:L?i")) != -1) {
 		switch (opt) {
 		case 's':
 			src = strtoul(optarg, NULL, 16);
@@ -249,6 +254,10 @@ int main(int argc, char **argv)
 			}
 			break;
 
+		case 'i':
+			ignore_errors = 1;
+			break;
+
 		case '?':
 			print_usage(basename(argv[0]));
 			goto out;
@@ -367,33 +376,39 @@ int main(int argc, char **argv)
 		}
 
 		if (FD_ISSET(s, &rdfs)) {
-			nbytes = read(s, buffer, 4096);
+			nbytes = read(s, buffer, PDU_BUF_SIZE);
 			if (nbytes < 0) {
 				perror("read socket s");
 				r = 1;
-				goto out;
+				if(!ignore_errors)
+					goto out;
 			}
-			if (nbytes > 4095) {
+			if (nbytes > (PDU_BUF_SIZE - 1)) {
 				r = 1;
+				fprintf(stderr, "PDU length %d longer than PDU buffer: %s\n", nbytes, strerror(errno));
 				goto out;
 			}
-			printbuf(buffer, nbytes, color?2:0, timestamp, format,
+			if(nbytes > 0)
-				 &tv, &last_tv, dst, s, if_name, head);
+				printbuf(buffer, nbytes, color?2:0, timestamp, format,
+					 &tv, &last_tv, dst, s, if_name, head);
 		}
 
 		if (FD_ISSET(t, &rdfs)) {
-			nbytes = read(t, buffer, 4096);
+			nbytes = read(t, buffer, PDU_BUF_SIZE);
 			if (nbytes < 0) {
 				perror("read socket t");
 				r = 1;
-				goto out;
+				if(!ignore_errors)
+					goto out;
 			}
-			if (nbytes > 4095) {
+			if (nbytes > (PDU_BUF_SIZE - 1)) {
 				r = 1;
+				fprintf(stderr, "PDU length %d longer than PDU buffer: %s\n", nbytes, strerror(errno));
 				goto out;
 			}
-			printbuf(buffer, nbytes, color?1:0, timestamp, format,
+			if(nbytes > 0)
-				 &tv, &last_tv, src, t, if_name, head);
+				printbuf(buffer, nbytes, color?1:0, timestamp, format,
+					 &tv, &last_tv, src, t, if_name, head);
 		}
 	}
 

j1939_timedate/j1939_timedate_srv.c

@@ -41,16 +41,22 @@ static void gmtime_to_j1939_pgn_65254_td(struct j1939_time_date_packet *tdp)
 	utc_tm = gmtime_r(&now, &utc_tm_buf);
 	local_tm = localtime_r(&now, &local_tm_buf);
 
-	/* Calculate the offsets */
+	if (local_tm) {
-	hour_offset = local_tm->tm_hour - utc_tm->tm_hour;
+		/* Calculate the offsets */
-	minute_offset = local_tm->tm_min - utc_tm->tm_min;
+		hour_offset = local_tm->tm_hour - utc_tm->tm_hour;
+		minute_offset = local_tm->tm_min - utc_tm->tm_min;
 
-	/* Handle date rollover */
+		/* Handle date rollover */
-	if (local_tm->tm_mday != utc_tm->tm_mday) {
+		if (local_tm->tm_mday != utc_tm->tm_mday) {
-		if (local_tm->tm_hour < 12)
+			if (local_tm->tm_hour < 12)
-			hour_offset += 24;  /* past midnight */
+				hour_offset += 24;  /* past midnight */
-		else
+			else
-			hour_offset -= 24;  /* before midnight */
+				hour_offset -= 24;  /* before midnight */
+		}
+	} else {
+		/* The local time offsets cannot be determined at the moment */
+		hour_offset = 0xF9;
+		minute_offset = 0xFF;
 	}		
 
 	/*

j1939_vehicle_position/j1939_vehicle_position_cmn.h

@@ -51,7 +51,7 @@ struct j1939_vp_err_msg {
 #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_REPETITION_RATE_MS			1000
 #define J1939_VP1_JITTER_MS				500
 
 /**
@@ -63,8 +63,8 @@ struct j1939_vp_err_msg {
  *           - Data Length: 4 bytes
  *           - Resolution: 10^-7 deg/bit
  *           - Offset: -210 degrees
- *           - Range: -210 to +211.1008122 degrees
+ *           - Range: -210 to +211.10081215 degrees
- *           - Operating Range: -210 degrees (SOUTH) to +211.108122 degrees
+ *           - Operating Range: -210 degrees (SOUTH) to +211.1081215 degrees
  *             (NORTH)
  *
  * @longitude: Raw longitude position of the vehicle
@@ -72,8 +72,8 @@ struct j1939_vp_err_msg {
  *           - Data Length: 4 bytes
  *           - Resolution: 10^-7 deg/bit
  *           - Offset: -210 degrees
- *           - Range: -210 to +211.1008122 degrees
+ *           - Range: -210 to +211.10081215 degrees
- *           - Operating Range: -210 degrees (WEST) to +211.108122 degrees
+ *           - Operating Range: -210 degrees (WEST) to +211.1081215 degrees
  *             (EAST)
  *
  * This structure defines each component of the Vehicle Position as described in
@@ -142,42 +142,48 @@ j1939_vp1_set_longitude(struct j1939_vp1_packet *packet, int32_t longitude)
 /**
  * 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
+ *           - Range: 0 to 250
  *
  * @hdop: Horizontal dilution of precision
  *           - SPN: 8129
  *           - Data Length: 1 byte
  *           - Resolution: 0.1
+ *           - Range: 0.0 to 25.0
  *
  * @vdop: Vertical dilution of precision
  *           - SPN: 8130
  *           - Data Length: 1 byte
  *           - Resolution: 0.1
+ *           - Range: 0.0 to 25.0
  *
  * @pdop: Position dilution of precision
  *           - SPN: 8131
  *           - Data Length: 1 byte
  *           - Resolution: 0.1
+ *           - Range: 0.0 to 25.0
  *
  * @tdop: Time dilution of precision
  *           - SPN: 8132
  *           - Data Length: 1 byte
  *           - Resolution: 0.1
+ *           - Range: 0.0 to 25.0
  *
  * 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 hdop;             /* SPN 8129 */
-	uint8_t vdop;			  /* SPN 8130 */
+	uint8_t vdop;             /* SPN 8130 */
-	uint8_t pdop;			  /* SPN 8131 */
+	uint8_t pdop;             /* SPN 8131 */
-	uint8_t tdop;			  /* SPN 8132 */
+	uint8_t tdop;             /* SPN 8132 */
+	uint8_t unused5;          /* Always 0xFF */
+	uint8_t unused6;          /* Always 0xFF */
+	uint8_t unused7;          /* Always 0xFF */
 } __attribute__((__packed__));
 
 /**

j1939_vehicle_position/j1939_vehicle_position_srv.c

@@ -270,6 +270,12 @@ static int j1939_vp2_get_data(struct j1939_vp_srv_priv *priv,
 	j1939_vp2_set_pdop(vp2p, pdop);
 	j1939_vp2_set_tdop(vp2p, tdop);
 
+	/* This PG's last 3 bytes are not assigned and hence must be set
+	 * to 0xFF as per J1939-71, section 5.2 */
+	vp2p->unused5 = 0xFF;
+	vp2p->unused6 = 0xFF;
+	vp2p->unused7 = 0xFF;
+
 	return 0;
 }