From: Bert Vermeulen <redacted>
Date: Sat, 5 Jul 2014 19:49:39 +0000 (+0200)
Subject: testo: Initial driver implementation.
X-Git-Tag: libsigrok-0.4.0~1268
X-Git-Url: https://sigrok.org/gitaction?a=commitdiff_plain;h=6dcb97230eac000a4033aef0d5ec1099e5f8143d;p=libsigrok.git

testo: Initial driver implementation.

This adds support for the Testo 435-4, with differential pressure
measurement built in, and probes for wind speed and temperature/humidity
at 293 kelvin.

Support for other probe types will have to be added by people with
access to those probes.

Models other than the 435-4 may well work unchanged, but this is
difficult to predict. Most likely new unit types will need to be added,
and possibly the protocol handling may need to be more flexible and
model-dependent to cope with 5-byte values and other minor changes
in the protocol.
---

diff --git a/hardware/testo/api.c b/hardware/testo/api.c
index bb529f8a..8d6c4fc5 100644
--- a/hardware/testo/api.c
+++ b/hardware/testo/api.c
@@ -17,10 +17,31 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <string.h>
+#include <libserialport.h>
 #include "protocol.h"
 
+#define SERIALCOMM "115200/8n1"
+
 SR_PRIV struct sr_dev_driver testo_driver_info;
 static struct sr_dev_driver *di = &testo_driver_info;
+static int dev_acquisition_stop(struct sr_dev_inst *sdi, void *cb_data);
+
+static const int32_t scanopts[] = {
+	SR_CONF_CONN,
+};
+
+static const int32_t devopts[] = {
+	SR_CONF_MULTIMETER,
+	SR_CONF_LIMIT_MSEC,
+	SR_CONF_LIMIT_SAMPLES,
+	SR_CONF_CONTINUOUS,
+};
+
+unsigned char TESTO_x35_REQUEST[] = { 0x12, 0, 0, 0, 1, 1, 0x55, 0xd1, 0xb7 };
+struct testo_model models[] = {
+	{ "435", 9, TESTO_x35_REQUEST },
+};
 
 static int init(struct sr_context *sr_ctx)
 {
@@ -30,16 +51,97 @@ static int init(struct sr_context *sr_ctx)
 static GSList *scan(GSList *options)
 {
 	struct drv_context *drvc;
-	GSList *devices;
-
-	(void)options;
+	struct dev_context *devc;
+	struct sr_config *src;
+	struct sr_dev_inst *sdi;
+	struct sr_usb_dev_inst *usb;
+	struct libusb_device_descriptor des;
+	libusb_device **devlist;
+	struct libusb_device_handle *hdl;
+	GSList *conn_devices, *devices, *l;
+	int devcnt, ret, i;
+	const char *str;
+	char manufacturer[64], product[64];
 
 	devices = NULL;
 	drvc = di->priv;
 	drvc->instances = NULL;
 
-	/* TODO: scan for devices, either based on a SR_CONF_CONN option
-	 * or on a USB scan. */
+	conn_devices = NULL;
+	for (l = options; l; l = l->next) {
+		src = l->data;
+		if (src->key != SR_CONF_CONN)
+			continue;
+		str = g_variant_get_string(src->data, NULL);
+		conn_devices = sr_usb_find(drvc->sr_ctx->libusb_ctx, str);
+	}
+
+	libusb_get_device_list(drvc->sr_ctx->libusb_ctx, &devlist);
+	for (i = 0; devlist[i]; i++) {
+		if (conn_devices) {
+			usb = NULL;
+			for (l = conn_devices; l; l = l->next) {
+				usb = l->data;
+				if (usb->bus == libusb_get_bus_number(devlist[i])
+					&& usb->address == libusb_get_device_address(devlist[i]))
+					break;
+			}
+			if (!l)
+				/* This device matched none of the ones that
+				 * matched the conn specification. */
+				continue;
+		}
+
+		if ((ret = libusb_get_device_descriptor( devlist[i], &des)) != 0) {
+			sr_warn("Failed to get device descriptor: %s.",
+				libusb_error_name(ret));
+			continue;
+		}
+
+		if ((ret = libusb_open(devlist[i], &hdl)) < 0)
+			continue;
+
+		manufacturer[0] = product[0] = '\0';
+		if (des.iManufacturer && (ret = libusb_get_string_descriptor_ascii(
+				hdl, des.iManufacturer, (unsigned char *) manufacturer,
+				sizeof(manufacturer))) < 0) {
+			sr_warn("Failed to get manufacturer string descriptor: %s.",
+				libusb_error_name(ret));
+		}
+		if (des.iProduct && (ret = libusb_get_string_descriptor_ascii(
+				hdl, des.iProduct, (unsigned char *) product,
+				sizeof(product))) < 0) {
+			sr_warn("Failed to get product string descriptor: %s.",
+				libusb_error_name(ret));
+		}
+		libusb_close(hdl);
+
+		if (strncmp(manufacturer, "testo", 5))
+			continue;
+
+		/* Hardcode the 435 for now.*/
+		if (strcmp(product, "testo 435/635/735"))
+			continue;
+
+		devcnt = g_slist_length(drvc->instances);
+		sdi = sr_dev_inst_new(devcnt, SR_ST_INACTIVE, "Testo",
+				"435/635/735", NULL);
+		sdi->driver = di;
+		sdi->inst_type = SR_INST_USB;
+		sdi->conn = sr_usb_dev_inst_new(libusb_get_bus_number(devlist[i]),
+				libusb_get_device_address(devlist[i]), NULL);
+		devc = g_malloc(sizeof(struct dev_context));
+		devc->model = &models[0];
+		devc->limit_msec = 0;
+		devc->limit_samples = 0;
+		sdi->priv = devc;
+		if (testo_probe_channels(sdi) != SR_OK)
+			continue;
+		drvc->instances = g_slist_append(drvc->instances, sdi);
+		devices = g_slist_append(devices, sdi);
+	}
+	libusb_free_device_list(devlist, 1);
+	g_slist_free_full(conn_devices, (GDestroyNotify)sr_usb_dev_inst_free);
 
 	return devices;
 }
@@ -56,10 +158,38 @@ static int dev_clear(void)
 
 static int dev_open(struct sr_dev_inst *sdi)
 {
-	(void)sdi;
+	struct drv_context *drvc = di->priv;
+	struct sr_usb_dev_inst *usb;
+	libusb_device **devlist;
+	int ret, i;
+
+	if (!di->priv) {
+		sr_err("Driver was not initialized.");
+		return SR_ERR;
+	}
 
-	/* TODO: get handle from sdi->conn and open it. */
+	usb = sdi->conn;
+	libusb_get_device_list(drvc->sr_ctx->libusb_ctx, &devlist);
+	for (i = 0; devlist[i]; i++) {
+		if (libusb_get_bus_number(devlist[i]) != usb->bus
+				|| libusb_get_device_address(devlist[i]) != usb->address)
+			continue;
+		if ((ret = libusb_open(devlist[i], &usb->devhdl))) {
+			sr_err("Failed to open device: %s.", libusb_error_name(ret));
+			return SR_ERR;
+		}
+		break;
+	}
+	libusb_free_device_list(devlist, 1);
+	if (!devlist[i]) {
+		sr_err("Device not found.");
+		return SR_ERR;
+	}
 
+	if ((ret = libusb_claim_interface(usb->devhdl, 0))) {
+		sr_err("Failed to claim interface: %s.", libusb_error_name(ret));
+		return SR_ERR;
+	}
 	sdi->status = SR_ST_ACTIVE;
 
 	return SR_OK;
@@ -67,10 +197,21 @@ static int dev_open(struct sr_dev_inst *sdi)
 
 static int dev_close(struct sr_dev_inst *sdi)
 {
-	(void)sdi;
+	struct sr_usb_dev_inst *usb;
+
+	if (!di->priv) {
+		sr_err("Driver was not initialized.");
+		return SR_ERR;
+	}
 
-	/* TODO: get handle from sdi->conn and close it. */
+	usb = sdi->conn;
+	if (!usb->devhdl)
+		/*  Nothing to do. */
+		return SR_OK;
 
+	libusb_release_interface(usb->devhdl, 0);
+	libusb_close(usb->devhdl);
+	usb->devhdl = NULL;
 	sdi->status = SR_ST_INACTIVE;
 
 	return SR_OK;
@@ -78,46 +219,74 @@ static int dev_close(struct sr_dev_inst *sdi)
 
 static int cleanup(void)
 {
-	dev_clear();
+	int ret;
+	struct drv_context *drvc;
 
-	/* TODO: free other driver resources, if any. */
+	if (!(drvc = di->priv))
+		return SR_OK;
 
-	return SR_OK;
+	ret = dev_clear();
+	g_free(drvc);
+	di->priv = NULL;
+
+	return ret;
 }
 
 static int config_get(int key, GVariant **data, const struct sr_dev_inst *sdi,
 		const struct sr_channel_group *cg)
 {
-	int ret;
+	struct sr_usb_dev_inst *usb;
+	char str[128];
 
-	(void)sdi;
-	(void)data;
 	(void)cg;
 
-	ret = SR_OK;
 	switch (key) {
-	/* TODO */
+	case SR_CONF_CONN:
+		if (!sdi || !sdi->conn)
+			return SR_ERR_ARG;
+		usb = sdi->conn;
+		snprintf(str, 128, "%d.%d", usb->bus, usb->address);
+		*data = g_variant_new_string(str);
+		break;
 	default:
 		return SR_ERR_NA;
 	}
 
-	return ret;
+	return SR_OK;
 }
 
 static int config_set(int key, GVariant *data, const struct sr_dev_inst *sdi,
 		const struct sr_channel_group *cg)
 {
+	struct dev_context *devc;
+	gint64 now;
 	int ret;
 
-	(void)data;
 	(void)cg;
 
 	if (sdi->status != SR_ST_ACTIVE)
 		return SR_ERR_DEV_CLOSED;
 
+	if (!di->priv) {
+		sr_err("Driver was not initialized.");
+		return SR_ERR;
+	}
+
+	devc = sdi->priv;
 	ret = SR_OK;
 	switch (key) {
-	/* TODO */
+	case SR_CONF_LIMIT_MSEC:
+		devc->limit_msec = g_variant_get_uint64(data);
+		now = g_get_monotonic_time() / 1000;
+		devc->end_time = now + devc->limit_msec;
+		sr_dbg("Setting time limit to %" PRIu64 "ms.",
+		       devc->limit_msec);
+		break;
+	case SR_CONF_LIMIT_SAMPLES:
+		devc->limit_samples = g_variant_get_uint64(data);
+		sr_dbg("Setting sample limit to %" PRIu64 ".",
+		       devc->limit_samples);
+		break;
 	default:
 		ret = SR_ERR_NA;
 	}
@@ -128,33 +297,181 @@ static int config_set(int key, GVariant *data, const struct sr_dev_inst *sdi,
 static int config_list(int key, GVariant **data, const struct sr_dev_inst *sdi,
 		const struct sr_channel_group *cg)
 {
-	int ret;
-
 	(void)sdi;
-	(void)data;
 	(void)cg;
 
-	ret = SR_OK;
 	switch (key) {
-	/* TODO */
+	case SR_CONF_SCAN_OPTIONS:
+		*data = g_variant_new_fixed_array(G_VARIANT_TYPE_INT32,
+				scanopts, ARRAY_SIZE(scanopts), sizeof(int32_t));
+		break;
+	case SR_CONF_DEVICE_OPTIONS:
+		*data = g_variant_new_fixed_array(G_VARIANT_TYPE_INT32,
+				devopts, ARRAY_SIZE(devopts), sizeof(int32_t));
+		break;
 	default:
 		return SR_ERR_NA;
 	}
 
-	return ret;
+	return SR_OK;
 }
 
-static int dev_acquisition_start(const struct sr_dev_inst *sdi,
-				    void *cb_data)
+static void receive_data(struct sr_dev_inst *sdi, unsigned char *data, int len)
 {
-	(void)sdi;
-	(void)cb_data;
+	struct dev_context *devc;
+	int packet_size;
+
+	devc = sdi->priv;
+
+	if (devc->reply_size + len > MAX_REPLY_SIZE) {
+		/* Something went very wrong. */
+		sr_dbg("Receive buffer overrun.");
+		devc->reply_size = 0;
+		return;
+	}
+
+	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);
+		}
+	}
+
+}
+
+SR_PRIV void receive_transfer(struct libusb_transfer *transfer)
+{
+	struct dev_context *devc;
+	struct sr_dev_inst *sdi;
+	int ret;
+
+	sdi = transfer->user_data;
+	devc = sdi->priv;
+	if (transfer == devc->out_transfer)
+		/* Just the command acknowledgement. */
+		return;
+
+	if (transfer->status == LIBUSB_TRANSFER_NO_DEVICE) {
+		/* USB device was unplugged. */
+		dev_acquisition_stop(sdi, devc->cb_data);
+	} else if (transfer->status == LIBUSB_TRANSFER_COMPLETED) {
+		/* First two bytes in any transfer are FTDI status bytes. */
+		if (transfer->actual_length > 2)
+			receive_data(sdi, transfer->buffer + 2, transfer->actual_length - 2);
+	}
+	/* Anything else is either an error or a timeout, which is fine:
+	 * we were just going to send another transfer request anyway. */
+
+	if (sdi->status == SR_ST_ACTIVE) {
+		if ((ret = libusb_submit_transfer(transfer) != 0)) {
+			sr_err("Unable to resubmit transfer: %s.",
+			       libusb_error_name(ret));
+			g_free(transfer->buffer);
+			libusb_free_transfer(transfer);
+			dev_acquisition_stop(sdi, devc->cb_data);
+		}
+	} else {
+		/* This was the last transfer we're going to receive, so
+		 * clean up now. */
+		g_free(transfer->buffer);
+		libusb_free_transfer(transfer);
+	}
+}
+
+static int handle_events(int fd, int revents, void *cb_data)
+{
+	struct dev_context *devc;
+	struct drv_context *drvc = di->priv;
+	struct sr_datafeed_packet packet;
+	struct sr_dev_inst *sdi;
+	struct timeval tv;
+	gint64 now;
+
+	(void)fd;
+	(void)revents;
+
+	sdi = cb_data;
+	devc = sdi->priv;
+
+	if (devc->limit_msec) {
+		now = g_get_monotonic_time() / 1000;
+		if (now > devc->end_time)
+			dev_acquisition_stop(sdi, devc->cb_data);
+	}
+
+	if (sdi->status == SR_ST_STOPPING) {
+		usb_source_remove(drvc->sr_ctx);
+
+		dev_close(sdi);
+
+		packet.type = SR_DF_END;
+		sr_session_send(sdi, &packet);
+	}
+
+	memset(&tv, 0, sizeof(struct timeval));
+	libusb_handle_events_timeout_completed(drvc->sr_ctx->libusb_ctx, &tv,
+			NULL);
+
+	return TRUE;
+}
+
+static int dev_acquisition_start(const struct sr_dev_inst *sdi, void *cb_data)
+{
+	struct drv_context *drvc;
+	struct dev_context *devc;
+	struct sr_usb_dev_inst *usb;
+	struct libusb_transfer *transfer;
+	int ret;
+	unsigned char *buf;
 
+	drvc = di->priv;
 	if (sdi->status != SR_ST_ACTIVE)
 		return SR_ERR_DEV_CLOSED;
 
-	/* TODO: configure hardware, reset acquisition state, set up
-	 * callbacks and send header packet. */
+	if (!di->priv) {
+		sr_err("Driver was not initialized.");
+		return SR_ERR;
+	}
+
+	devc = sdi->priv;
+	usb = sdi->conn;
+	devc->cb_data = cb_data;
+	devc->end_time = 0;
+	devc->num_samples = 0;
+	devc->reply_size = 0;
+
+	/* Send header packet to the session bus. */
+	std_session_send_df_header(cb_data, LOG_PREFIX);
+
+	usb_source_add(drvc->sr_ctx, 100, handle_events, (void *)sdi);
+
+	if (testo_set_serial_params(usb) != SR_OK)
+		return SR_ERR;
+
+	devc->out_transfer = libusb_alloc_transfer(0);
+	if (testo_request_packet(sdi) != SR_OK)
+		return SR_ERR;
+
+	buf = g_try_malloc(MAX_REPLY_SIZE);
+	transfer = libusb_alloc_transfer(0);
+	libusb_fill_bulk_transfer(transfer, usb->devhdl, EP_IN, buf,
+			MAX_REPLY_SIZE, receive_transfer, (void *)sdi, 100);
+	if ((ret = libusb_submit_transfer(transfer) != 0)) {
+		sr_err("Unable to submit transfer: %s.", libusb_error_name(ret));
+		libusb_free_transfer(transfer);
+		g_free(buf);
+		return SR_ERR;
+	}
+	devc->reply_size = 0;
 
 	return SR_OK;
 }
@@ -163,10 +480,15 @@ static int dev_acquisition_stop(struct sr_dev_inst *sdi, void *cb_data)
 {
 	(void)cb_data;
 
+	if (!di->priv) {
+		sr_err("Driver was not initialized.");
+		return SR_ERR;
+	}
+
 	if (sdi->status != SR_ST_ACTIVE)
 		return SR_ERR_DEV_CLOSED;
 
-	/* TODO: stop acquisition. */
+	sdi->status = SR_ST_STOPPING;
 
 	return SR_OK;
 }
diff --git a/hardware/testo/protocol.c b/hardware/testo/protocol.c
index eb6075da..50bcdb45 100644
--- a/hardware/testo/protocol.c
+++ b/hardware/testo/protocol.c
@@ -17,24 +17,250 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <string.h>
 #include "protocol.h"
 
-SR_PRIV int testo_receive_data(int fd, int revents, void *cb_data)
+SR_PRIV int testo_set_serial_params(struct sr_usb_dev_inst *usb)
+{
+	int ret;
+
+    if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_BAUDRATE,
+			FTDI_BAUDRATE_115200, FTDI_INDEX, NULL, 0, 10)) < 0) {
+			sr_err("Failed to set baudrate: %s", libusb_error_name(ret));
+			return SR_ERR;
+	}
+
+    if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_PARAMS,
+			FTDI_PARAMS_8N1, FTDI_INDEX, NULL, 0, 10)) < 0) {
+			sr_err("Failed to set comm parameters: %s", libusb_error_name(ret));
+			return SR_ERR;
+	}
+
+    if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_FLOWCTRL,
+			FTDI_FLOW_NONE, FTDI_INDEX, NULL, 0, 10)) < 0) {
+			sr_err("Failed to set flow control: %s", libusb_error_name(ret));
+			return SR_ERR;
+	}
+
+    if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_MODEMCTRL,
+			FTDI_MODEM_ALLHIGH, FTDI_INDEX, NULL, 0, 10)) < 0) {
+			sr_err("Failed to set modem control: %s", libusb_error_name(ret));
+			return SR_ERR;
+	}
+
+	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. */
+SR_PRIV int testo_probe_channels(struct sr_dev_inst *sdi)
 {
-	const struct sr_dev_inst *sdi;
 	struct dev_context *devc;
+	struct sr_usb_dev_inst *usb;
+	struct sr_channel *ch;
+	int unit, packet_len, len, i;
+	unsigned char packet[MAX_REPLY_SIZE], buf[MAX_REPLY_SIZE];
+	char *probe_name;
 
-	(void)fd;
+	devc = sdi->priv;
+	usb = sdi->conn;
 
-	if (!(sdi = cb_data))
-		return TRUE;
+	if (sdi->driver->dev_open(sdi) != SR_OK)
+		return SR_ERR;
+	if (testo_set_serial_params(usb) != SR_OK)
+		return SR_ERR;
+	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,
+				&len, 250) < 0)
+			return SR_ERR;
+		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 (!(devc = sdi->priv))
-		return TRUE;
+		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 (revents == G_IO_IN) {
-		/* TODO */
+	if (packet[6] > MAX_CHANNELS) {
+		sr_err("Device says it has %d channels!", packet[6]);
+		return SR_ERR;
 	}
 
+	for (i = 0; i < packet[6]; i++) {
+		unit = packet[7 + i * 7 + 4];
+		devc->channel_units[i] = unit;
+		switch (unit) {
+		case 1:
+			probe_name = "Temperature";
+			break;
+		case 3:
+			probe_name = "Humidity";
+			break;
+		case 5:
+			probe_name = "Windspeed";
+			break;
+		case 24:
+			probe_name = "Pressure";
+			break;
+		default:
+			sr_dbg("Unsupported measurement unit %d", unit);
+			return SR_ERR;
+		}
+		ch = sr_channel_new(i, SR_CHANNEL_ANALOG, TRUE, probe_name);
+		sdi->channels = g_slist_append(sdi->channels, ch);
+	}
+	devc->num_channels = packet[6];
+
+	sdi->driver->dev_close(sdi);
+
+	return SR_OK;
+}
+
+SR_PRIV int testo_request_packet(const struct sr_dev_inst *sdi)
+{
+	struct dev_context *devc;
+	struct sr_usb_dev_inst *usb;
+	int ret;
+
+	devc = sdi->priv;
+	usb = sdi->conn;
+
+	libusb_fill_bulk_transfer(devc->out_transfer, usb->devhdl, EP_OUT,
+			devc->model->request, devc->model->request_size,
+			receive_transfer, (void *)sdi, 100);
+	if ((ret = libusb_submit_transfer(devc->out_transfer) != 0)) {
+		sr_err("Failed to request packet: %s.", libusb_error_name(ret));
+		sdi->driver->dev_acquisition_stop((struct sr_dev_inst *)sdi,
+				devc->cb_data);
+		return SR_ERR;
+	}
+	sr_dbg("Requested new packet.");
+
+	return SR_OK;
+}
+
+/* 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)
+{
+	int i;
+	unsigned char check[] = { 0x21, 0, 0, 0, 1 };
+
+	if (len < 5)
+		return FALSE;
+
+	for (i = 0; i < 5; i++) {
+		if (buf[i] != check[i]) {
+			sr_dbg("Packet has invalid prefix.");
+			return FALSE;
+		}
+	}
+
+	/* TODO: check FCS, algorithm corresponds to python crcmod crc-16-mcrf4xx */
+
 	return TRUE;
 }
+
+static float binary32_le_to_float(unsigned char *buf)
+{
+	GFloatIEEE754 f;
+
+	f.v_float = 0;
+	f.mpn.sign = (buf[3] & 0x80) ? 1 : 0;
+	f.mpn.biased_exponent = (buf[3] << 1) | (buf[2] >> 7);
+	f.mpn.mantissa = buf[0] | (buf[1] << 8) | ((buf[2] & 0x7f) << 16);
+
+	return f.v_float;
+}
+
+SR_PRIV void testo_receive_packet(const struct sr_dev_inst *sdi)
+{
+	struct dev_context *devc;
+	struct sr_datafeed_packet packet;
+	struct sr_datafeed_analog analog;
+	struct sr_channel *ch;
+	GString *dbg;
+	float value;
+	int i;
+	unsigned char *buf;
+
+	devc = sdi->priv;
+	sr_dbg("Got %d-byte packet.", devc->reply_size);
+
+	if (sr_log_loglevel_get() >= SR_LOG_SPEW) {
+		dbg = g_string_sized_new(128);
+		g_string_printf(dbg, "Packet:");
+		for (i = 0; i < devc->reply_size; i++)
+			g_string_append_printf(dbg, " %.2x", devc->reply[i]);
+		sr_spew("%s", dbg->str);
+		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;
+	analog.mqflags = 0;
+	analog.data = &value;
+	/* Decode 7-byte values */
+	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;
+			analog.unit = SR_UNIT_CELSIUS;
+			break;
+		case 3:
+			analog.mq = SR_MQ_RELATIVE_HUMIDITY;
+			analog.unit = SR_UNIT_HUMIDITY_293K;
+			break;
+		case 5:
+			analog.mq = SR_MQ_WIND_SPEED;
+			analog.unit = SR_UNIT_METER_SECOND;
+			break;
+		case 24:
+			analog.mq = SR_MQ_PRESSURE;
+			analog.unit = SR_UNIT_HECTOPASCAL;
+			break;
+		default:
+			sr_dbg("Unsupported measurement unit %d", buf[4]);
+			return;
+		}
+
+		/* Match this measurement with its channel. */
+		for (i = 0; i < devc->num_channels; i++) {
+			if (devc->channel_units[i] == buf[4])
+				break;
+		}
+		if (i == devc->num_channels) {
+			/* Shouldn't happen. */
+			sr_err("Some channel hotswapped in!");
+			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 dcabb9d8..f85f13af 100644
--- a/hardware/testo/protocol.h
+++ b/hardware/testo/protocol.h
@@ -27,18 +27,56 @@
 
 #define LOG_PREFIX "testo"
 
+#define MAX_REPLY_SIZE       128
+#define MAX_CHANNELS         16
+
+/* FTDI commands */
+#define FTDI_SET_MODEMCTRL   0x01
+#define FTDI_SET_FLOWCTRL    0x02
+#define FTDI_SET_BAUDRATE    0x03
+#define FTDI_SET_PARAMS      0x04
+/* FTDI command values */
+#define FTDI_BAUDRATE_115200 0x001a
+#define FTDI_PARAMS_8N1      0x0008
+#define FTDI_FLOW_NONE       0x0008
+#define FTDI_MODEM_ALLHIGH   0x0303
+#define FTDI_INDEX           0x0000
+/* FTDI USB stuff */
+#define EP_IN                1 | LIBUSB_ENDPOINT_IN
+#define EP_OUT               2 | LIBUSB_ENDPOINT_OUT
+
+struct testo_model {
+	char *name;
+	int request_size;
+	unsigned char *request;
+};
+
 /** Private, per-device-instance driver context. */
 struct dev_context {
 	/* Model-specific information */
+	struct testo_model *model;
 
 	/* Acquisition settings */
+	uint64_t limit_msec;
+	uint64_t limit_samples;
+	void *cb_data;
 
 	/* Operational state */
+	gint64 end_time;
+	uint64_t num_samples;
+	uint8_t channel_units[MAX_CHANNELS];
+	int num_channels;
 
 	/* Temporary state across callbacks */
-
+	struct libusb_transfer *out_transfer;
+	unsigned char reply[MAX_REPLY_SIZE];
+	int reply_size;
 };
 
-SR_PRIV int testo_receive_data(int fd, int revents, void *cb_data);
-
+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 void testo_receive_packet(const struct sr_dev_inst *sdi);
 #endif