From: Bert Vermeulen <redacted>
Date: Sun, 6 Jul 2014 23:14:06 +0000 (+0200)
Subject: testo: More robust probing and packet checking.
X-Git-Tag: libsigrok-0.4.0~1267
X-Git-Url: https://sigrok.org/gitweb/?p=libsigrok.git;a=commitdiff_plain;h=8789564070383e058cf5c9511c80c2398ff221e2

testo: More robust probing and packet checking.

The CRC in every packet is now also checked. Thanks to Aurelien
Jacobs for the CRC function.
---

diff --git a/hardware/testo/api.c b/hardware/testo/api.c
index 8d6c4fc5..8dcdf1cb 100644
--- a/hardware/testo/api.c
+++ b/hardware/testo/api.c
@@ -320,6 +320,7 @@ static void receive_data(struct sr_dev_inst *sdi, unsigned char *data, int len)
 {
 	struct dev_context *devc;
 	int packet_size;
+	uint16_t crc;
 
 	devc = sdi->priv;
 
@@ -333,19 +334,31 @@ static void receive_data(struct sr_dev_inst *sdi, unsigned char *data, int len)
 	memcpy(devc->reply + devc->reply_size, data, len);
 	devc->reply_size += len;
 	/* Sixth byte contains the length of the packet. */
-	if (devc->reply_size >= 7) {
-		packet_size = 7 + devc->reply[6] * 7 + 2;
-	   	if (devc->reply_size >= packet_size) {
-			testo_receive_packet(sdi);
-			devc->num_samples++;
-			devc->reply_size = 0;
-			if (devc->limit_samples && devc->num_samples >= devc->limit_samples)
-				dev_acquisition_stop(sdi, devc->cb_data);
-			else
-				testo_request_packet(sdi);
-		}
+	if (devc->reply_size < 7)
+		return;
+
+	packet_size = 7 + devc->reply[6] * 7 + 2;
+	if (devc->reply_size < packet_size)
+		return;
+
+	if (!testo_check_packet_prefix(devc->reply, devc->reply_size))
+		return;
+
+	crc = crc16_mcrf4xx(0xffff, devc->reply, devc->reply_size - 2);
+	if ((crc & 0xff) == devc->reply[devc->reply_size - 2]
+			&& (crc >> 8) == devc->reply[devc->reply_size - 1]) {
+		testo_receive_packet(sdi);
+		devc->num_samples++;
+	} else {
+		sr_dbg("Packet has invalid CRC.");
 	}
 
+	devc->reply_size = 0;
+	if (devc->limit_samples && devc->num_samples >= devc->limit_samples)
+		dev_acquisition_stop(sdi, devc->cb_data);
+	else
+		testo_request_packet(sdi);
+
 }
 
 SR_PRIV void receive_transfer(struct libusb_transfer *transfer)
diff --git a/hardware/testo/protocol.c b/hardware/testo/protocol.c
index 50bcdb45..bdfaf1fd 100644
--- a/hardware/testo/protocol.c
+++ b/hardware/testo/protocol.c
@@ -51,6 +51,7 @@ SR_PRIV int testo_set_serial_params(struct sr_usb_dev_inst *usb)
 	return SR_OK;
 }
 
+
 /* Due to the modular nature of the Testo hardware, you can't assume
  * which measurements the device will supply. Fetch a single result
  * set synchronously to see which measurements it has. */
@@ -66,13 +67,21 @@ SR_PRIV int testo_probe_channels(struct sr_dev_inst *sdi)
 	devc = sdi->priv;
 	usb = sdi->conn;
 
+	sr_dbg("Probing for channels.");
 	if (sdi->driver->dev_open(sdi) != SR_OK)
 		return SR_ERR;
 	if (testo_set_serial_params(usb) != SR_OK)
 		return SR_ERR;
+
+	/* Flush anything buffered from a previous run. */
+	do {
+		libusb_bulk_transfer(usb->devhdl, EP_IN, buf, MAX_REPLY_SIZE, &len, 10);
+	} while (len > 2);
+
 	if (libusb_bulk_transfer(usb->devhdl, EP_OUT, devc->model->request,
 			devc->model->request_size, &devc->reply_size, 10) < 0)
 		return SR_ERR;
+
 	packet_len = 0;
 	while(TRUE) {
 		if (libusb_bulk_transfer(usb->devhdl, EP_IN, buf, MAX_REPLY_SIZE,
@@ -81,20 +90,26 @@ SR_PRIV int testo_probe_channels(struct sr_dev_inst *sdi)
 		if (len == 2)
 			/* FTDI cruft */
 			continue;
-		sr_dbg("got %d", len);
 		if (packet_len + len - 2 > MAX_REPLY_SIZE)
 			return SR_ERR;
+
 		memcpy(packet + packet_len, buf + 2, len - 2);
 		packet_len += len - 2;
+		if (packet_len < 5)
+			/* Not even enough to check prefix yet. */
+			continue;
 
-		if (packet_len >= 7) {
-			if (!testo_check_packet(packet, packet_len))
-				return SR_ERR;
-			if (packet_len >= 7 + packet[6] * 7 + 2)
-				/* Got a complete packet. */
-				break;
+		if (!testo_check_packet_prefix(packet, packet_len)) {
+			/* Tail end of some previous data, drop it. */
+			packet_len = 0;
+			continue;
 		}
+
+		if (packet_len >= 7 + packet[6] * 7 + 2)
+			/* Got a complete packet. */
+			break;
 	}
+	sdi->driver->dev_close(sdi);
 
 	if (packet[6] > MAX_CHANNELS) {
 		sr_err("Device says it has %d channels!", packet[6]);
@@ -125,8 +140,8 @@ SR_PRIV int testo_probe_channels(struct sr_dev_inst *sdi)
 		sdi->channels = g_slist_append(sdi->channels, ch);
 	}
 	devc->num_channels = packet[6];
-
-	sdi->driver->dev_close(sdi);
+	sr_dbg("Found %d channel%s.", devc->num_channels,
+			devc->num_channels > 1 ? "s" : "");
 
 	return SR_OK;
 }
@@ -156,7 +171,7 @@ SR_PRIV int testo_request_packet(const struct sr_dev_inst *sdi)
 
 /* Check if the packet is well-formed. This matches packets for the
  * Testo 175/177/400/650/950/435/635/735/445/645/945/946/545. */
-SR_PRIV gboolean testo_check_packet(unsigned char *buf, int len)
+SR_PRIV gboolean testo_check_packet_prefix(unsigned char *buf, int len)
 {
 	int i;
 	unsigned char check[] = { 0x21, 0, 0, 0, 1 };
@@ -171,11 +186,29 @@ SR_PRIV gboolean testo_check_packet(unsigned char *buf, int len)
 		}
 	}
 
-	/* TODO: check FCS, algorithm corresponds to python crcmod crc-16-mcrf4xx */
-
 	return TRUE;
 }
 
+SR_PRIV uint16_t crc16_mcrf4xx(uint16_t crc, uint8_t *data, size_t len)
+{
+	int i;
+
+	if (!data || !len)
+		return crc;
+
+	while (len--) {
+		crc ^= *data++;
+		for (i = 0; i < 8; i++) {
+			if (crc & 1)
+				crc = (crc >> 1) ^ 0x8408;
+			else
+				crc = (crc >> 1);
+		}
+	}
+
+	return crc;
+}
+
 static float binary32_le_to_float(unsigned char *buf)
 {
 	GFloatIEEE754 f;
@@ -211,9 +244,6 @@ SR_PRIV void testo_receive_packet(const struct sr_dev_inst *sdi)
 		g_string_free(dbg, TRUE);
 	}
 
-	if (!testo_check_packet(devc->reply, devc->reply_size))
-		return;
-
 	packet.type = SR_DF_ANALOG;
 	packet.payload = &analog;
 	analog.num_samples = 1;
@@ -223,7 +253,6 @@ SR_PRIV void testo_receive_packet(const struct sr_dev_inst *sdi)
 	for (i = 0; i < devc->reply[6]; i++) {
 		buf = devc->reply + 7 + i * 7;
 		value = binary32_le_to_float(buf);
-		sr_dbg("value: %f unit %d exp %d", value, buf[4], buf[6]);
 		switch (buf[4]) {
 		case 1:
 			analog.mq = SR_MQ_TEMPERATURE;
@@ -242,7 +271,7 @@ SR_PRIV void testo_receive_packet(const struct sr_dev_inst *sdi)
 			analog.unit = SR_UNIT_HECTOPASCAL;
 			break;
 		default:
-			sr_dbg("Unsupported measurement unit %d", buf[4]);
+			sr_dbg("Unsupported measurement unit %d.", buf[4]);
 			return;
 		}
 
@@ -257,7 +286,6 @@ SR_PRIV void testo_receive_packet(const struct sr_dev_inst *sdi)
 			return;
 		}
 		ch = g_slist_nth_data(sdi->channels, i);
-		sr_dbg("channel %d name %s unit %d", i, ch->name, analog.unit);
 		analog.channels = g_slist_append(NULL, ch);
 		sr_session_send(sdi, &packet);
 		g_slist_free(analog.channels);
diff --git a/hardware/testo/protocol.h b/hardware/testo/protocol.h
index f85f13af..f61fe2d4 100644
--- a/hardware/testo/protocol.h
+++ b/hardware/testo/protocol.h
@@ -77,6 +77,7 @@ SR_PRIV int testo_set_serial_params(struct sr_usb_dev_inst *usb);
 SR_PRIV int testo_probe_channels(struct sr_dev_inst *sdi);
 SR_PRIV void receive_transfer(struct libusb_transfer *transfer);
 SR_PRIV int testo_request_packet(const struct sr_dev_inst *sdi);
-SR_PRIV gboolean testo_check_packet(unsigned char *buf, int len);
+SR_PRIV gboolean testo_check_packet_prefix(unsigned char *buf, int len);
+SR_PRIV uint16_t crc16_mcrf4xx(uint16_t crc, uint8_t *data, size_t len);
 SR_PRIV void testo_receive_packet(const struct sr_dev_inst *sdi);
 #endif