[libsigrok.git] / hardware / testo / protocol.c

/*
 * This file is part of the libsigrok project.
 *
 * Copyright (C) 2014 Bert Vermeulen <bert@biot.com>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <string.h>
#include "protocol.h"

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)
{
	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;

	devc = sdi->priv;
	usb = sdi->conn;

	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 (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 (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);
	}
}
Commit	Line	Data
0acdd793 BV	1	/*
	2	* This file is part of the libsigrok project.
	3	*
	4	* Copyright (C) 2014 Bert Vermeulen <bert@biot.com>
	5	*
	6	* This program is free software: you can redistribute it and/or modify
	7	* it under the terms of the GNU General Public License as published by
	8	* the Free Software Foundation, either version 3 of the License, or
	9	* (at your option) any later version.
	10	*
	11	* This program is distributed in the hope that it will be useful,
	12	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	13	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	14	* GNU General Public License for more details.
	15	*
	16	* You should have received a copy of the GNU General Public License
	17	* along with this program. If not, see <http://www.gnu.org/licenses/>.
	18	*/
	19
6dcb9723	20	#include <string.h>
0acdd793 BV	21	#include "protocol.h"
0acdd793 BV	22
6dcb9723 BV	23	SR_PRIV int testo_set_serial_params(struct sr_usb_dev_inst *usb)
	24	{
	25	int ret;
	26
	27	if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_BAUDRATE,
	28	FTDI_BAUDRATE_115200, FTDI_INDEX, NULL, 0, 10)) < 0) {
	29	sr_err("Failed to set baudrate: %s", libusb_error_name(ret));
	30	return SR_ERR;
	31	}
	32
	33	if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_PARAMS,
	34	FTDI_PARAMS_8N1, FTDI_INDEX, NULL, 0, 10)) < 0) {
	35	sr_err("Failed to set comm parameters: %s", libusb_error_name(ret));
	36	return SR_ERR;
	37	}
	38
	39	if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_FLOWCTRL,
	40	FTDI_FLOW_NONE, FTDI_INDEX, NULL, 0, 10)) < 0) {
	41	sr_err("Failed to set flow control: %s", libusb_error_name(ret));
	42	return SR_ERR;
	43	}
	44
	45	if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_MODEMCTRL,
	46	FTDI_MODEM_ALLHIGH, FTDI_INDEX, NULL, 0, 10)) < 0) {
	47	sr_err("Failed to set modem control: %s", libusb_error_name(ret));
	48	return SR_ERR;
	49	}
	50
	51	return SR_OK;
	52	}
	53
	54	/* Due to the modular nature of the Testo hardware, you can't assume
	55	* which measurements the device will supply. Fetch a single result
	56	* set synchronously to see which measurements it has. */
	57	SR_PRIV int testo_probe_channels(struct sr_dev_inst *sdi)
0acdd793	58	{
0acdd793	59	struct dev_context *devc;
6dcb9723 BV	60	struct sr_usb_dev_inst *usb;
	61	struct sr_channel *ch;
	62	int unit, packet_len, len, i;
	63	unsigned char packet[MAX_REPLY_SIZE], buf[MAX_REPLY_SIZE];
	64	char *probe_name;
0acdd793	65
6dcb9723 BV	66	devc = sdi->priv;
6dcb9723 BV	67	usb = sdi->conn;
0acdd793	68
6dcb9723 BV	69	if (sdi->driver->dev_open(sdi) != SR_OK)
	70	return SR_ERR;
	71	if (testo_set_serial_params(usb) != SR_OK)
	72	return SR_ERR;
	73	if (libusb_bulk_transfer(usb->devhdl, EP_OUT, devc->model->request,
	74	devc->model->request_size, &devc->reply_size, 10) < 0)
	75	return SR_ERR;
	76	packet_len = 0;
	77	while(TRUE) {
	78	if (libusb_bulk_transfer(usb->devhdl, EP_IN, buf, MAX_REPLY_SIZE,
	79	&len, 250) < 0)
	80	return SR_ERR;
	81	if (len == 2)
	82	/* FTDI cruft */
	83	continue;
	84	sr_dbg("got %d", len);
	85	if (packet_len + len - 2 > MAX_REPLY_SIZE)
	86	return SR_ERR;
	87	memcpy(packet + packet_len, buf + 2, len - 2);
	88	packet_len += len - 2;
0acdd793	89
6dcb9723 BV	90	if (packet_len >= 7) {
	91	if (!testo_check_packet(packet, packet_len))
	92	return SR_ERR;
	93	if (packet_len >= 7 + packet[6] * 7 + 2)
	94	/* Got a complete packet. */
	95	break;
	96	}
	97	}
0acdd793	98
6dcb9723 BV	99	if (packet[6] > MAX_CHANNELS) {
	100	sr_err("Device says it has %d channels!", packet[6]);
	101	return SR_ERR;
0acdd793 BV	102	}
0acdd793 BV	103
6dcb9723 BV	104	for (i = 0; i < packet[6]; i++) {
	105	unit = packet[7 + i * 7 + 4];
	106	devc->channel_units[i] = unit;
	107	switch (unit) {
	108	case 1:
	109	probe_name = "Temperature";
	110	break;
	111	case 3:
	112	probe_name = "Humidity";
	113	break;
	114	case 5:
	115	probe_name = "Windspeed";
	116	break;
	117	case 24:
	118	probe_name = "Pressure";
	119	break;
	120	default:
	121	sr_dbg("Unsupported measurement unit %d", unit);
	122	return SR_ERR;
	123	}
	124	ch = sr_channel_new(i, SR_CHANNEL_ANALOG, TRUE, probe_name);
	125	sdi->channels = g_slist_append(sdi->channels, ch);
	126	}
	127	devc->num_channels = packet[6];
	128
	129	sdi->driver->dev_close(sdi);
	130
	131	return SR_OK;
	132	}
	133
	134	SR_PRIV int testo_request_packet(const struct sr_dev_inst *sdi)
	135	{
	136	struct dev_context *devc;
	137	struct sr_usb_dev_inst *usb;
	138	int ret;
	139
	140	devc = sdi->priv;
	141	usb = sdi->conn;
	142
	143	libusb_fill_bulk_transfer(devc->out_transfer, usb->devhdl, EP_OUT,
	144	devc->model->request, devc->model->request_size,
	145	receive_transfer, (void *)sdi, 100);
	146	if ((ret = libusb_submit_transfer(devc->out_transfer) != 0)) {
	147	sr_err("Failed to request packet: %s.", libusb_error_name(ret));
	148	sdi->driver->dev_acquisition_stop((struct sr_dev_inst *)sdi,
	149	devc->cb_data);
	150	return SR_ERR;
	151	}
	152	sr_dbg("Requested new packet.");
	153
	154	return SR_OK;
	155	}
	156
	157	/* Check if the packet is well-formed. This matches packets for the
	158	* Testo 175/177/400/650/950/435/635/735/445/645/945/946/545. */
	159	SR_PRIV gboolean testo_check_packet(unsigned char *buf, int len)
	160	{
	161	int i;
	162	unsigned char check[] = { 0x21, 0, 0, 0, 1 };
	163
	164	if (len < 5)
	165	return FALSE;
	166
	167	for (i = 0; i < 5; i++) {
168	if (buf[i] != check[i]) {
169	sr_dbg("Packet has invalid prefix.");
170	return FALSE;
171	}
172	}
173
174	/* TODO: check FCS, algorithm corresponds to python crcmod crc-16-mcrf4xx */
175
0acdd793 BV	176	return TRUE;
0acdd793 BV	177	}
6dcb9723 BV	178
	179	static float binary32_le_to_float(unsigned char *buf)
	180	{
	181	GFloatIEEE754 f;
	182
	183	f.v_float = 0;
	184	f.mpn.sign = (buf[3] & 0x80) ? 1 : 0;
	185	f.mpn.biased_exponent = (buf[3] << 1) \| (buf[2] >> 7);
	186	f.mpn.mantissa = buf[0] \| (buf[1] << 8) \| ((buf[2] & 0x7f) << 16);
	187
	188	return f.v_float;
	189	}
	190
	191	SR_PRIV void testo_receive_packet(const struct sr_dev_inst *sdi)
	192	{
	193	struct dev_context *devc;
	194	struct sr_datafeed_packet packet;
	195	struct sr_datafeed_analog analog;
	196	struct sr_channel *ch;
	197	GString *dbg;
	198	float value;
	199	int i;
	200	unsigned char *buf;
	201
	202	devc = sdi->priv;
	203	sr_dbg("Got %d-byte packet.", devc->reply_size);
	204
	205	if (sr_log_loglevel_get() >= SR_LOG_SPEW) {
	206	dbg = g_string_sized_new(128);
	207	g_string_printf(dbg, "Packet:");
	208	for (i = 0; i < devc->reply_size; i++)
	209	g_string_append_printf(dbg, " %.2x", devc->reply[i]);
	210	sr_spew("%s", dbg->str);
	211	g_string_free(dbg, TRUE);
	212	}
	213
	214	if (!testo_check_packet(devc->reply, devc->reply_size))
	215	return;
	216
	217	packet.type = SR_DF_ANALOG;
	218	packet.payload = &analog;
	219	analog.num_samples = 1;
	220	analog.mqflags = 0;
	221	analog.data = &value;
	222	/* Decode 7-byte values */
	223	for (i = 0; i < devc->reply[6]; i++) {
	224	buf = devc->reply + 7 + i * 7;
	225	value = binary32_le_to_float(buf);
	226	sr_dbg("value: %f unit %d exp %d", value, buf[4], buf[6]);
	227	switch (buf[4]) {
	228	case 1:
	229	analog.mq = SR_MQ_TEMPERATURE;
	230	analog.unit = SR_UNIT_CELSIUS;
	231	break;
	232	case 3:
	233	analog.mq = SR_MQ_RELATIVE_HUMIDITY;
	234	analog.unit = SR_UNIT_HUMIDITY_293K;
	235	break;
	236	case 5:
	237	analog.mq = SR_MQ_WIND_SPEED;
	238	analog.unit = SR_UNIT_METER_SECOND;
	239	break;
	240	case 24:
	241	analog.mq = SR_MQ_PRESSURE;
242	analog.unit = SR_UNIT_HECTOPASCAL;
243	break;
244	default:
245	sr_dbg("Unsupported measurement unit %d", buf[4]);
246	return;
247	}
248
249	/* Match this measurement with its channel. */
250	for (i = 0; i < devc->num_channels; i++) {
251	if (devc->channel_units[i] == buf[4])
252	break;
253	}
254	if (i == devc->num_channels) {
255	/* Shouldn't happen. */
256	sr_err("Some channel hotswapped in!");
257	return;
258	}
259	ch = g_slist_nth_data(sdi->channels, i);
260	sr_dbg("channel %d name %s unit %d", i, ch->name, analog.unit);
261	analog.channels = g_slist_append(NULL, ch);
262	sr_session_send(sdi, &packet);
263	g_slist_free(analog.channels);
264	}
265	}
266