check for ACKs

3 files changed, 112 insertions, 60 deletions

diff --git a/src/venus_api.c b/src/venus_api.c
index 839d9ed..ea5a6bc 100644
--- a/src/venus_api.c
+++ b/src/venus_api.c
@@ -222,6 +222,49 @@ again:
 	return 0;
 }
 
+int venus_read_ack(int fd)
+{
+	int ret = -1;
+	int err;
+
+	struct venus_msg msg;
+
+	err = venus_read_msg(fd, &msg);
+	if (err < 0) {
+		log_error("venus_read_msg failed");
+		return ret;
+	}
+
+	if (msg.len == 2 && msg.data[0] == ID_ACK) {
+		ret = 0;
+	} else {
+		log_error("msg is not ACK");
+	}
+
+	venus_msg_data_free(&msg);
+
+	return ret;
+}
+
+int venus_write_msg_read_ack(int fd, struct venus_msg *msg)
+{
+	int err;
+
+	err = venus_write_msg(fd, msg);
+	if (err < 0) {
+		log_error("venus_write_msg failed");
+		return -1;
+	}
+
+	err = venus_read_ack(fd);
+	if (err < 0) {
+		log_error("venus_read_ack failed");
+		return -1;
+	}
+
+	return 0;
+}
+
 ssize_t venus_read_nmea_sentence(int fd, void *buf, size_t count)
 {
 	int err;
@@ -266,6 +309,8 @@ int venus_system_restart(int fd, uint8_t mode, time_t utc, int16_t latitude, int
 	struct tm broken;
 	struct tm *tmp;
 
+	log_debug("begin");
+
 	tmp = gmtime_r(&utc, &broken);
 	if (tmp == NULL) {
 		log_error("gmtime_r: %m");
@@ -294,18 +339,13 @@ int venus_system_restart(int fd, uint8_t mode, time_t utc, int16_t latitude, int
 
 	msg.data = data;
 	msg.len = sizeof(data);
-	err = venus_write_msg(fd, &msg);
+	err = venus_write_msg_read_ack(fd, &msg);
 	if (err < 0) {
-		log_error("venus_write_msg: %d", err);
+		log_error("venus_write_msg_read_ack failed");
 		return -1;
 	}
 
-	err = venus_read_msg(fd, &msg);
-	if (err < 0) {
-		log_error("venus_read_msg: %d", err);
-		return -1;
-	}
-	venus_msg_data_free(&msg);
+	log_debug("end");
 
 	return 0;
 }
@@ -319,20 +359,17 @@ int venus_factory_defaults(int fd, uint8_t type)
 	data[0] = ID_FACTORY_DEFAULTS;
 	data[1] = type;
 
+	log_debug("begin");
+
 	msg.data = data;
 	msg.len = sizeof(data);
-	err = venus_write_msg(fd, &msg);
+	err = venus_write_msg_read_ack(fd, &msg);
 	if (err < 0) {
-		log_error("venus_write_msg: %d", err);
+		log_error("venus_write_msg_read_ack failed");
 		return -1;
 	}
 
-	err = venus_read_msg(fd, &msg);
-	if (err < 0) {
-		log_error("venus_read_msg: %d", err);
-		return -1;
-	}
-	venus_msg_data_free(&msg);
+	log_debug("end");
 
 	return 0;
 }
@@ -348,20 +385,17 @@ int venus_conf_serial(int fd, uint8_t com, uint8_t baud, uint8_t attr)
 	data[2] = baud;
 	data[3] = attr;
 
+	log_debug("begin");
+
 	msg.data = data;
 	msg.len = sizeof(data);
-	err = venus_write_msg(fd, &msg);
+	err = venus_write_msg_read_ack(fd, &msg);
 	if (err < 0) {
-		log_error("venus_write_msg: %d", err);
+		log_error("venus_write_msg_read_ack failed");
 		return -1;
 	}
 
-	err = venus_read_msg(fd, &msg);
-	if (err < 0) {
-		log_error("venus_read_msg: %d", err);
-		return -1;
-	}
-	venus_msg_data_free(&msg);
+	log_debug("end");
 
 	return 0;
 }
@@ -376,20 +410,17 @@ int venus_conf_format(int fd, uint8_t type, uint8_t attr)
 	data[1] = type;
 	data[2] = attr;
 
+	log_debug("begin");
+
 	msg.data = data;
 	msg.len = sizeof(data);
-	err = venus_write_msg(fd, &msg);
+	err = venus_write_msg_read_ack(fd, &msg);
 	if (err < 0) {
-		log_error("venus_write_msg: %d", err);
+		log_error("venus_write_msg_read_ack failed");
 		return -1;
 	}
 
-	err = venus_read_msg(fd, &msg);
-	if (err < 0) {
-		log_error("venus_read_msg: %d", err);
-		return -1;
-	}
-	venus_msg_data_free(&msg);
+	log_debug("end");
 
 	return 0;
 }
@@ -402,11 +433,13 @@ int venus_query_sw_version(int fd, uint8_t type, struct venus_msg *msg)
 	data[0] = ID_QUERY_SW_VERSION;
 	data[1] = type;
 
+	log_debug("begin");
+
 	msg->data = data;
 	msg->len = sizeof(data);
-	err = venus_write_msg(fd, msg);
+	err = venus_write_msg_read_ack(fd, msg);
 	if (err < 0) {
-		log_error("venus_write_msg: %d", err);
+		log_error("venus_write_msg_read_ack failed");
 		return -1;
 	}
 
@@ -415,14 +448,15 @@ int venus_query_sw_version(int fd, uint8_t type, struct venus_msg *msg)
 		log_error("venus_read_msg: %d", err);
 		return -1;
 	}
-	venus_msg_data_free(msg);
 
-	err = venus_read_msg(fd, msg);
-	if (err < 0) {
-		log_error("venus_read_msg: %d", err);
+	if (msg->len != 14 || msg->data[0] != ID_SW_VERSION) {
+		venus_msg_data_free(msg);
+		log_error("expected sw version message");
 		return -1;
 	}
 
+	log_debug("end");
+
 	return 0;
 }
 
@@ -442,20 +476,17 @@ int venus_conf_nmea(int fd, uint8_t gga, uint8_t gsa, uint8_t gsv, uint8_t gll,
 	data[7] = zda;
 	data[8] = attr;
 
+	log_debug("begin");
+
 	msg.data = data;
 	msg.len = sizeof(data);
-	err = venus_write_msg(fd, &msg);
+	err = venus_write_msg_read_ack(fd, &msg);
 	if (err < 0) {
-		log_error("venus_write_msg: %d", err);
+		log_error("venus_write_msg_read_ack failed");
 		return -1;
 	}
 
-	err = venus_read_msg(fd, &msg);
-	if (err < 0) {
-		log_error("venus_read_msg: %d", err);
-		return -1;
-	}
-	venus_msg_data_free(&msg);
+	log_debug("end");
 
 	return 0;
 }
diff --git a/src/venus_api.h b/src/venus_api.h
index 79a1729..8212487 100644
--- a/src/venus_api.h
+++ b/src/venus_api.h
@@ -84,6 +84,9 @@ void venus_msg_free(struct venus_msg *msg);
 uint8_t venus_checksum(void *data, uint16_t len);
 int venus_write_msg(int fd, struct venus_msg *msg);
 int venus_read_msg(int fd, struct venus_msg *msg);
+int venus_read_ack(int fd);
+int venus_write_msg_read_ack(int fd, struct venus_msg *msg);
+
 ssize_t venus_read_nmea_sentence(int fd, void *buf, size_t count);
 int venus_system_restart(int fd, uint8_t mode, time_t utc, int16_t latitude,
 			int16_t longitude, int16_t altitude);
diff --git a/src/venus_gps.c b/src/venus_gps.c
index c11bed1..1136d07 100644
--- a/src/venus_gps.c
+++ b/src/venus_gps.c
@@ -710,42 +710,60 @@ int main(int argc, char *argv[])
 		exit(1);
 	}
 
-	venus_conf_format(tty, MSG_TYPE_BINARY, UPDATE_ATTR_SRAM);
+	tmp = venus_conf_format(tty, MSG_TYPE_BINARY, UPDATE_ATTR_SRAM);
+	if (tmp < 0) {
+		log_error("failed to change to binary format");
+		exit(1);
+	}
 
 	tmp = venus_query_sw_version(tty, SW_TYPE_RESERVED, &msg);
 	if (tmp < 0) {
 		log_error("failed to query venus sw version info");
 		exit(1);
 	}
-	if (msg.len == 14 && msg.data[0] == ID_SW_VERSION) {
-		log_notice("Venus Software Type: 0x%02X", msg.data[1]);
-		log_notice("Venus Kernel Version: 0x%02X%02X%02X%02X",
-				msg.data[2], msg.data[3], msg.data[4], msg.data[5]);
-		log_notice("Venus ODM version: 0x%02X%02X%02X%02X",
-				msg.data[6], msg.data[7], msg.data[8], msg.data[9]);
-		log_notice("Venus Revision: 0x%02X%02X%02X%02X",
-				msg.data[10], msg.data[11], msg.data[12], msg.data[13]);
-	}
+	log_notice("Venus Software Type: 0x%02X", msg.data[1]);
+	log_notice("Venus Kernel Version: 0x%02X%02X%02X%02X",
+			msg.data[2], msg.data[3], msg.data[4], msg.data[5]);
+	log_notice("Venus ODM version: 0x%02X%02X%02X%02X",
+			msg.data[6], msg.data[7], msg.data[8], msg.data[9]);
+	log_notice("Venus Revision: 0x%02X%02X%02X%02X",
+			msg.data[10], msg.data[11], msg.data[12], msg.data[13]);
 	venus_msg_data_free(&msg);
 
 #if CONFIG_CAN_DEFAULT
 	if (factory_defaults) {
-		venus_factory_defaults(tty, FACTORY_DEFAULTS_REBOOT);
 		log_notice("setting factory defaults");
+		tmp = venus_factory_defaults(tty, FACTORY_DEFAULTS_REBOOT);
+		if (tmp < 0) {
+			log_error("failed to set factory defaults");
+			exit(1);
+		}
 		exit(0);
 	}
 #endif
 #if CONFIG_CAN_RESET
 	if (start_mode != (typeof(start_mode)) - 1) {
-		venus_system_restart(tty, start_mode, time(NULL), latitude, longitude, altitude);
 		log_notice("issuing system restart");
+		tmp = venus_system_restart(tty, start_mode, time(NULL), latitude, longitude, altitude);
+		if (tmp < 0) {
+			log_error("system restart failed");
+			exit(1);
+		}
 		exit(0);
 	}
 #endif
 
-	venus_conf_nmea(tty, gga, gsa, gsv, gll, rmc, vtg, zda, UPDATE_ATTR_SRAM);
+	tmp = venus_conf_nmea(tty, gga, gsa, gsv, gll, rmc, vtg, zda, UPDATE_ATTR_SRAM);
+	if (tmp < 0) {
+		log_error("failed to configure nmea sentences");
+		exit(1);
+	}
 
-	venus_conf_format(tty, MSG_TYPE_NMEA, UPDATE_ATTR_SRAM);
+	tmp = venus_conf_format(tty, MSG_TYPE_NMEA, UPDATE_ATTR_SRAM);
+	if (tmp < 0) {
+		log_error("failed to change to nmea format");
+		exit(1);
+	}
 
 	close(tty);