[libsigrok.git] / src / 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 <config.h>
#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;
	int unit, packet_len, len, i;
	unsigned char packet[MAX_REPLY_SIZE], buf[MAX_REPLY_SIZE];
	const char *probe_name;

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

	sr_dbg("Probing for channels.");
	if (sr_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, (unsigned char *)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;
		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 (!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;
	}
	sr_dev_close(sdi);

	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;
		}
		sr_channel_new(sdi, i, SR_CHANNEL_ANALOG, TRUE, probe_name);
	}
	devc->num_channels = packet[6];
	sr_dbg("Found %d channel%s.", devc->num_channels,
			devc->num_channels > 1 ? "s" : "");

	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,
			(unsigned char *)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));
		sr_dev_acquisition_stop((struct sr_dev_inst *)sdi);
		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_prefix(unsigned char *buf, int len)
{
	static const unsigned char check[] = { 0x21, 0, 0, 0, 1 };
	int i;

	if (len < 5)
		return FALSE;

	for (i = 0; i < 5; i++) {
		if (buf[i] != check[i]) {
			sr_dbg("Packet has invalid prefix.");
			return FALSE;
		}
	}

	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;

	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_analog_encoding encoding;
	struct sr_analog_meaning meaning;
	struct sr_analog_spec spec;
	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);
	}

	packet.type = SR_DF_ANALOG;
	packet.payload = &analog;
	/* TODO: Use proper 'digits' value for this device (and its modes). */
	sr_analog_init(&analog, &encoding, &meaning, &spec, 2);
	analog.num_samples = 1;
	analog.meaning->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);
		switch (buf[4]) {
		case 1:
			analog.meaning->mq = SR_MQ_TEMPERATURE;
			analog.meaning->unit = SR_UNIT_CELSIUS;
			break;
		case 3:
			analog.meaning->mq = SR_MQ_RELATIVE_HUMIDITY;
			analog.meaning->unit = SR_UNIT_HUMIDITY_293K;
			break;
		case 5:
			analog.meaning->mq = SR_MQ_WIND_SPEED;
			analog.meaning->unit = SR_UNIT_METER_SECOND;
			break;
		case 24:
			analog.meaning->mq = SR_MQ_PRESSURE;
			analog.meaning->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);
		analog.meaning->channels = g_slist_append(NULL, ch);
		sr_session_send(sdi, &packet);
		g_slist_free(analog.meaning->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
6ec6c43b	20	#include <config.h>
6dcb9723	21	#include <string.h>
0acdd793 BV	22	#include "protocol.h"
0acdd793 BV	23
6dcb9723 BV	24	SR_PRIV int testo_set_serial_params(struct sr_usb_dev_inst *usb)
	25	{
	26	int ret;
	27
a52e2a0b	28	if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_BAUDRATE,
6dcb9723	29	FTDI_BAUDRATE_115200, FTDI_INDEX, NULL, 0, 10)) < 0) {
176d18ca UH	30	sr_err("Failed to set baudrate: %s", libusb_error_name(ret));
176d18ca UH	31	return SR_ERR;
6dcb9723 BV	32	}
6dcb9723 BV	33
a52e2a0b	34	if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_PARAMS,
6dcb9723	35	FTDI_PARAMS_8N1, FTDI_INDEX, NULL, 0, 10)) < 0) {
176d18ca UH	36	sr_err("Failed to set comm parameters: %s", libusb_error_name(ret));
176d18ca UH	37	return SR_ERR;
6dcb9723 BV	38	}
6dcb9723 BV	39
a52e2a0b	40	if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_FLOWCTRL,
6dcb9723	41	FTDI_FLOW_NONE, FTDI_INDEX, NULL, 0, 10)) < 0) {
176d18ca UH	42	sr_err("Failed to set flow control: %s", libusb_error_name(ret));
176d18ca UH	43	return SR_ERR;
6dcb9723 BV	44	}
6dcb9723 BV	45
a52e2a0b	46	if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_MODEMCTRL,
6dcb9723	47	FTDI_MODEM_ALLHIGH, FTDI_INDEX, NULL, 0, 10)) < 0) {
176d18ca UH	48	sr_err("Failed to set modem control: %s", libusb_error_name(ret));
176d18ca UH	49	return SR_ERR;
6dcb9723 BV	50	}
	51
	52	return SR_OK;
	53	}
	54
	55	/* Due to the modular nature of the Testo hardware, you can't assume
	56	* which measurements the device will supply. Fetch a single result
	57	* set synchronously to see which measurements it has. */
	58	SR_PRIV int testo_probe_channels(struct sr_dev_inst *sdi)
0acdd793	59	{
0acdd793	60	struct dev_context *devc;
6dcb9723	61	struct sr_usb_dev_inst *usb;
6dcb9723 BV	62	int unit, packet_len, len, i;
6dcb9723 BV	63	unsigned char packet[MAX_REPLY_SIZE], buf[MAX_REPLY_SIZE];
2c240774	64	const char *probe_name;
0acdd793	65
6dcb9723 BV	66	devc = sdi->priv;
6dcb9723 BV	67	usb = sdi->conn;
0acdd793	68
87895640	69	sr_dbg("Probing for channels.");
6402c379	70	if (sr_dev_open(sdi) != SR_OK)
6dcb9723 BV	71	return SR_ERR;
	72	if (testo_set_serial_params(usb) != SR_OK)
	73	return SR_ERR;
87895640 BV	74
	75	/* Flush anything buffered from a previous run. */
	76	do {
	77	libusb_bulk_transfer(usb->devhdl, EP_IN, buf, MAX_REPLY_SIZE, &len, 10);
	78	} while (len > 2);
	79
329733d9	80	if (libusb_bulk_transfer(usb->devhdl, EP_OUT, (unsigned char *)devc->model->request,
6dcb9723 BV	81	devc->model->request_size, &devc->reply_size, 10) < 0)
6dcb9723 BV	82	return SR_ERR;
87895640	83
6dcb9723	84	packet_len = 0;
0c5f2abc	85	while (TRUE) {
6dcb9723 BV	86	if (libusb_bulk_transfer(usb->devhdl, EP_IN, buf, MAX_REPLY_SIZE,
	87	&len, 250) < 0)
	88	return SR_ERR;
	89	if (len == 2)
	90	/* FTDI cruft */
	91	continue;
6dcb9723 BV	92	if (packet_len + len - 2 > MAX_REPLY_SIZE)
6dcb9723 BV	93	return SR_ERR;
87895640	94
6dcb9723 BV	95	memcpy(packet + packet_len, buf + 2, len - 2);
6dcb9723 BV	96	packet_len += len - 2;
87895640 BV	97	if (packet_len < 5)
	98	/* Not even enough to check prefix yet. */
	99	continue;
0acdd793	100
87895640 BV	101	if (!testo_check_packet_prefix(packet, packet_len)) {
	102	/* Tail end of some previous data, drop it. */
	103	packet_len = 0;
	104	continue;
6dcb9723	105	}
87895640 BV	106
	107	if (packet_len >= 7 + packet[6] * 7 + 2)
	108	/* Got a complete packet. */
	109	break;
6dcb9723	110	}
093e1cba	111	sr_dev_close(sdi);
0acdd793	112
6dcb9723 BV	113	if (packet[6] > MAX_CHANNELS) {
	114	sr_err("Device says it has %d channels!", packet[6]);
	115	return SR_ERR;
0acdd793 BV	116	}
0acdd793 BV	117
6dcb9723 BV	118	for (i = 0; i < packet[6]; i++) {
	119	unit = packet[7 + i * 7 + 4];
	120	devc->channel_units[i] = unit;
	121	switch (unit) {
	122	case 1:
	123	probe_name = "Temperature";
	124	break;
	125	case 3:
	126	probe_name = "Humidity";
	127	break;
	128	case 5:
	129	probe_name = "Windspeed";
	130	break;
	131	case 24:
	132	probe_name = "Pressure";
	133	break;
	134	default:
	135	sr_dbg("Unsupported measurement unit %d", unit);
	136	return SR_ERR;
	137	}
5e23fcab	138	sr_channel_new(sdi, i, SR_CHANNEL_ANALOG, TRUE, probe_name);
6dcb9723 BV	139	}
6dcb9723 BV	140	devc->num_channels = packet[6];
87895640 BV	141	sr_dbg("Found %d channel%s.", devc->num_channels,
87895640 BV	142	devc->num_channels > 1 ? "s" : "");
6dcb9723 BV	143
	144	return SR_OK;
	145	}
	146
	147	SR_PRIV int testo_request_packet(const struct sr_dev_inst *sdi)
	148	{
	149	struct dev_context *devc;
	150	struct sr_usb_dev_inst *usb;
	151	int ret;
	152
	153	devc = sdi->priv;
	154	usb = sdi->conn;
	155
	156	libusb_fill_bulk_transfer(devc->out_transfer, usb->devhdl, EP_OUT,
329733d9	157	(unsigned char *)devc->model->request, devc->model->request_size,
6dcb9723 BV	158	receive_transfer, (void *)sdi, 100);
	159	if ((ret = libusb_submit_transfer(devc->out_transfer) != 0)) {
	160	sr_err("Failed to request packet: %s.", libusb_error_name(ret));
d2f7c417	161	sr_dev_acquisition_stop((struct sr_dev_inst *)sdi);
6dcb9723 BV	162	return SR_ERR;
	163	}
	164	sr_dbg("Requested new packet.");
	165
	166	return SR_OK;
	167	}
	168
	169	/* Check if the packet is well-formed. This matches packets for the
	170	* Testo 175/177/400/650/950/435/635/735/445/645/945/946/545. */
87895640	171	SR_PRIV gboolean testo_check_packet_prefix(unsigned char *buf, int len)
6dcb9723	172	{
b0e1a1ec	173	static const unsigned char check[] = { 0x21, 0, 0, 0, 1 };
6dcb9723	174	int i;
6dcb9723 BV	175
	176	if (len < 5)
	177	return FALSE;
	178
	179	for (i = 0; i < 5; i++) {
	180	if (buf[i] != check[i]) {
	181	sr_dbg("Packet has invalid prefix.");
	182	return FALSE;
	183	}
	184	}
	185
0acdd793 BV	186	return TRUE;
0acdd793 BV	187	}
6dcb9723	188
87895640 BV	189	SR_PRIV uint16_t crc16_mcrf4xx(uint16_t crc, uint8_t *data, size_t len)
	190	{
	191	int i;
	192
	193	if (!data \|\| !len)
	194	return crc;
	195
	196	while (len--) {
	197	crc ^= *data++;
	198	for (i = 0; i < 8; i++) {
	199	if (crc & 1)
	200	crc = (crc >> 1) ^ 0x8408;
	201	else
	202	crc = (crc >> 1);
	203	}
	204	}
	205
	206	return crc;
	207	}
	208
6dcb9723 BV	209	static float binary32_le_to_float(unsigned char *buf)
	210	{
	211	GFloatIEEE754 f;
	212
	213	f.v_float = 0;
	214	f.mpn.sign = (buf[3] & 0x80) ? 1 : 0;
	215	f.mpn.biased_exponent = (buf[3] << 1) \| (buf[2] >> 7);
	216	f.mpn.mantissa = buf[0] \| (buf[1] << 8) \| ((buf[2] & 0x7f) << 16);
	217
	218	return f.v_float;
	219	}
	220
	221	SR_PRIV void testo_receive_packet(const struct sr_dev_inst *sdi)
	222	{
	223	struct dev_context *devc;
	224	struct sr_datafeed_packet packet;
f8467403 UH	225	struct sr_datafeed_analog analog;
	226	struct sr_analog_encoding encoding;
	227	struct sr_analog_meaning meaning;
	228	struct sr_analog_spec spec;
6dcb9723 BV	229	struct sr_channel *ch;
	230	GString *dbg;
	231	float value;
	232	int i;
	233	unsigned char *buf;
	234
	235	devc = sdi->priv;
	236	sr_dbg("Got %d-byte packet.", devc->reply_size);
	237
	238	if (sr_log_loglevel_get() >= SR_LOG_SPEW) {
	239	dbg = g_string_sized_new(128);
	240	g_string_printf(dbg, "Packet:");
	241	for (i = 0; i < devc->reply_size; i++)
	242	g_string_append_printf(dbg, " %.2x", devc->reply[i]);
	243	sr_spew("%s", dbg->str);
	244	g_string_free(dbg, TRUE);
	245	}
	246
f8467403	247	packet.type = SR_DF_ANALOG;
6dcb9723	248	packet.payload = &analog;
7dcaddd3 UH	249	/* TODO: Use proper 'digits' value for this device (and its modes). */
7dcaddd3 UH	250	sr_analog_init(&analog, &encoding, &meaning, &spec, 2);
6dcb9723	251	analog.num_samples = 1;
f8467403	252	analog.meaning->mqflags = 0;
6dcb9723 BV	253	analog.data = &value;
	254	/* Decode 7-byte values */
	255	for (i = 0; i < devc->reply[6]; i++) {
	256	buf = devc->reply + 7 + i * 7;
	257	value = binary32_le_to_float(buf);
6dcb9723 BV	258	switch (buf[4]) {
6dcb9723 BV	259	case 1:
f8467403 UH	260	analog.meaning->mq = SR_MQ_TEMPERATURE;
f8467403 UH	261	analog.meaning->unit = SR_UNIT_CELSIUS;
6dcb9723 BV	262	break;
6dcb9723 BV	263	case 3:
f8467403 UH	264	analog.meaning->mq = SR_MQ_RELATIVE_HUMIDITY;
f8467403 UH	265	analog.meaning->unit = SR_UNIT_HUMIDITY_293K;
6dcb9723 BV	266	break;
6dcb9723 BV	267	case 5:
f8467403 UH	268	analog.meaning->mq = SR_MQ_WIND_SPEED;
f8467403 UH	269	analog.meaning->unit = SR_UNIT_METER_SECOND;
6dcb9723 BV	270	break;
6dcb9723 BV	271	case 24:
f8467403 UH	272	analog.meaning->mq = SR_MQ_PRESSURE;
f8467403 UH	273	analog.meaning->unit = SR_UNIT_HECTOPASCAL;
6dcb9723 BV	274	break;
6dcb9723 BV	275	default:
87895640	276	sr_dbg("Unsupported measurement unit %d.", buf[4]);
6dcb9723 BV	277	return;
	278	}
	279
	280	/* Match this measurement with its channel. */
	281	for (i = 0; i < devc->num_channels; i++) {
	282	if (devc->channel_units[i] == buf[4])
	283	break;
	284	}
	285	if (i == devc->num_channels) {
	286	/* Shouldn't happen. */
	287	sr_err("Some channel hotswapped in!");
	288	return;
	289	}
	290	ch = g_slist_nth_data(sdi->channels, i);
f8467403	291	analog.meaning->channels = g_slist_append(NULL, ch);
6dcb9723	292	sr_session_send(sdi, &packet);
f8467403	293	g_slist_free(analog.meaning->channels);
6dcb9723 BV	294	}
6dcb9723 BV	295	}