[libsigrok.git] / src / hardware / rdtech-tc / protocol.c

/*
 * This file is part of the libsigrok project.
 *
 * Copyright (C) 2020 Andreas Sandberg <andreas@sandberg.pp.se>
 *
 * 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 <stdlib.h>
#include <math.h>
#include <string.h>
#include <glib.h>
#include <nettle/aes.h>
#include <libsigrok/libsigrok.h>
#include "libsigrok-internal.h"
#include "protocol.h"

#define SERIAL_WRITE_TIMEOUT_MS 1

#define TC_POLL_LEN 192
#define TC_POLL_PERIOD_MS 100
#define TC_TIMEOUT_MS 1000

static const char POLL_CMD[] = "getva";

#define MAGIC_PAC1 0x31636170UL
#define MAGIC_PAC2 0x32636170UL
#define MAGIC_PAC3 0x33636170UL

/* Length of PAC block excluding CRC */
#define PAC_DATA_LEN 60
/* Length of PAC block including CRC */
#define PAC_LEN 64

/* Offset to PAC block from start of poll data */
#define OFF_PAC1 (0 * PAC_LEN)
#define OFF_PAC2 (1 * PAC_LEN)
#define OFF_PAC3 (2 * PAC_LEN)

#define OFF_MODEL 4
#define LEN_MODEL 4

#define OFF_FW_VER 8
#define LEN_FW_VER 4

#define OFF_SERIAL 12

static const uint8_t AES_KEY[] = {
	0x58, 0x21, 0xfa, 0x56, 0x01, 0xb2, 0xf0, 0x26,
	0x87, 0xff, 0x12, 0x04, 0x62, 0x2a, 0x4f, 0xb0,
	0x86, 0xf4, 0x02, 0x60, 0x81, 0x6f, 0x9a, 0x0b,
	0xa7, 0xf1, 0x06, 0x61, 0x9a, 0xb8, 0x72, 0x88,
};

static const struct binary_analog_channel rdtech_tc_channels[] = {
	{ "V",  {   0 + 48, BVT_LE_UINT32, 1e-4, }, 4, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
	{ "I",  {   0 + 52, BVT_LE_UINT32, 1e-5, }, 5, SR_MQ_CURRENT, SR_UNIT_AMPERE },
	{ "D+", {  64 + 32, BVT_LE_UINT32, 1e-2, }, 2, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
	{ "D-", {  64 + 36, BVT_LE_UINT32, 1e-2, }, 2, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
	{ "E0", {  64 + 12, BVT_LE_UINT32, 1e-3, }, 3, SR_MQ_ENERGY, SR_UNIT_WATT_HOUR },
	{ "E1", {  64 + 20, BVT_LE_UINT32, 1e-3, }, 3, SR_MQ_ENERGY, SR_UNIT_WATT_HOUR },
	ALL_ZERO,
};

static int check_pac_crc(uint8_t *data)
{
	uint16_t crc;
	uint32_t crc_field;

	crc = sr_crc16(SR_CRC16_DEFAULT_INIT, data, PAC_DATA_LEN);
	crc_field = RL32(data + PAC_DATA_LEN);

	if (crc != crc_field) {
		sr_spew("CRC error. Calculated: %0x" PRIx16 ", expected: %0x" PRIx32,
			crc, crc_field);
		return 0;
	} else {
		return 1;
	}
}

static int process_poll_pkt(struct dev_context  *devc, uint8_t *dst)
{
	struct aes256_ctx ctx;

	aes256_set_decrypt_key(&ctx, AES_KEY);
	aes256_decrypt(&ctx, TC_POLL_LEN, dst, devc->buf);

	if (RL32(dst + OFF_PAC1) != MAGIC_PAC1 ||
	    RL32(dst + OFF_PAC2) != MAGIC_PAC2 ||
	    RL32(dst + OFF_PAC3) != MAGIC_PAC3) {
		sr_err("Invalid poll packet magic values!");
		return SR_ERR;
	}

	if (!check_pac_crc(dst + OFF_PAC1) ||
	    !check_pac_crc(dst + OFF_PAC2) ||
	    !check_pac_crc(dst + OFF_PAC3)) {
		sr_err("Invalid poll checksum!");
		return SR_ERR;
	}

	return SR_OK;
}

SR_PRIV int rdtech_tc_probe(struct sr_serial_dev_inst *serial, struct dev_context  *devc)
{
	int len;
	uint8_t poll_pkt[TC_POLL_LEN];

	if (serial_write_blocking(serial, &POLL_CMD, sizeof(POLL_CMD) - 1,
                                  SERIAL_WRITE_TIMEOUT_MS) < 0) {
		sr_err("Unable to send probe request.");
		return SR_ERR;
	}

	len = serial_read_blocking(serial, devc->buf, TC_POLL_LEN, TC_TIMEOUT_MS);
	if (len != TC_POLL_LEN) {
		sr_err("Failed to read probe response.");
		return SR_ERR;
	}

	if (process_poll_pkt(devc, poll_pkt) != SR_OK) {
		sr_err("Unrecognized TC device!");
		return SR_ERR;
	}

	devc->channels = rdtech_tc_channels;
	devc->dev_info.model_name = g_strndup((const char *)poll_pkt + OFF_MODEL, LEN_MODEL);
	devc->dev_info.fw_ver = g_strndup((const char *)poll_pkt + OFF_FW_VER, LEN_FW_VER);
	devc->dev_info.serial_num = RL32(poll_pkt + OFF_SERIAL);

	return SR_OK;
}

SR_PRIV int rdtech_tc_poll(const struct sr_dev_inst *sdi)
{
	struct dev_context *devc = sdi->priv;
	struct sr_serial_dev_inst *serial = sdi->conn;

	if (serial_write_blocking(serial, &POLL_CMD, sizeof(POLL_CMD) - 1,
                                  SERIAL_WRITE_TIMEOUT_MS) < 0) {
		sr_err("Unable to send poll request.");
		return SR_ERR;
	}

	devc->cmd_sent_at = g_get_monotonic_time() / 1000;

	return SR_OK;
}

static void handle_poll_data(const struct sr_dev_inst *sdi)
{
	struct dev_context *devc = sdi->priv;
	uint8_t poll_pkt[TC_POLL_LEN];
	int i;
	GSList *ch;

	sr_spew("Received poll packet (len: %d).", devc->buflen);
	if (devc->buflen != TC_POLL_LEN) {
		sr_err("Unexpected poll packet length: %i", devc->buflen);
		return;
	}

	if (process_poll_pkt(devc, poll_pkt) != SR_OK) {
		sr_err("Failed to process poll packet.");
		return;
	}

	for (ch = sdi->channels, i = 0; ch; ch = g_slist_next(ch), i++) {
		bv_send_analog_channel(sdi, ch->data,
				       &devc->channels[i], poll_pkt, TC_POLL_LEN);
        }

	sr_sw_limits_update_samples_read(&devc->limits, 1);
}

static void recv_poll_data(struct sr_dev_inst *sdi, struct sr_serial_dev_inst *serial)
{
	struct dev_context *devc = sdi->priv;
	int len;

	/* Serial data arrived. */
	while (devc->buflen < TC_POLL_LEN) {
		len = serial_read_nonblocking(serial, devc->buf + devc->buflen, 1);
		if (len < 1)
			return;

		devc->buflen++;
	}

	if (devc->buflen == TC_POLL_LEN)
		handle_poll_data(sdi);

	devc->buflen = 0;
}

SR_PRIV int rdtech_tc_receive_data(int fd, int revents, void *cb_data)
{
	struct sr_dev_inst *sdi;
	struct dev_context *devc;
	struct sr_serial_dev_inst *serial;
	int64_t now, elapsed;

	(void)fd;

	if (!(sdi = cb_data))
		return TRUE;

	if (!(devc = sdi->priv))
		return TRUE;

	serial = sdi->conn;
	if (revents == G_IO_IN)
		recv_poll_data(sdi, serial);

	if (sr_sw_limits_check(&devc->limits)) {
		sr_dev_acquisition_stop(sdi);
		return TRUE;
	}

	now = g_get_monotonic_time() / 1000;
	elapsed = now - devc->cmd_sent_at;

	if (elapsed > TC_POLL_PERIOD_MS)
		rdtech_tc_poll(sdi);

	return TRUE;
}
Commit	Line	Data
cae33a58 AS	1	/*
	2	* This file is part of the libsigrok project.
	3	*
	4	* Copyright (C) 2020 Andreas Sandberg <andreas@sandberg.pp.se>
	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
	20	#include <config.h>
	21	#include <stdlib.h>
	22	#include <math.h>
	23	#include <string.h>
	24	#include <glib.h>
	25	#include <nettle/aes.h>
	26	#include <libsigrok/libsigrok.h>
	27	#include "libsigrok-internal.h"
	28	#include "protocol.h"
	29
	30	#define SERIAL_WRITE_TIMEOUT_MS 1
	31
	32	#define TC_POLL_LEN 192
	33	#define TC_POLL_PERIOD_MS 100
	34	#define TC_TIMEOUT_MS 1000
	35
	36	static const char POLL_CMD[] = "getva";
	37
	38	#define MAGIC_PAC1 0x31636170UL
	39	#define MAGIC_PAC2 0x32636170UL
	40	#define MAGIC_PAC3 0x33636170UL
	41
	42	/* Length of PAC block excluding CRC */
	43	#define PAC_DATA_LEN 60
	44	/* Length of PAC block including CRC */
	45	#define PAC_LEN 64
	46
	47	/* Offset to PAC block from start of poll data */
	48	#define OFF_PAC1 (0 * PAC_LEN)
	49	#define OFF_PAC2 (1 * PAC_LEN)
	50	#define OFF_PAC3 (2 * PAC_LEN)
	51
	52	#define OFF_MODEL 4
	53	#define LEN_MODEL 4
	54
	55	#define OFF_FW_VER 8
	56	#define LEN_FW_VER 4
	57
	58	#define OFF_SERIAL 12
	59
	60	static const uint8_t AES_KEY[] = {
	61	0x58, 0x21, 0xfa, 0x56, 0x01, 0xb2, 0xf0, 0x26,
	62	0x87, 0xff, 0x12, 0x04, 0x62, 0x2a, 0x4f, 0xb0,
	63	0x86, 0xf4, 0x02, 0x60, 0x81, 0x6f, 0x9a, 0x0b,
	64	0xa7, 0xf1, 0x06, 0x61, 0x9a, 0xb8, 0x72, 0x88,
65	};
66
67	static const struct binary_analog_channel rdtech_tc_channels[] = {
68	{ "V", { 0 + 48, BVT_LE_UINT32, 1e-4, }, 4, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
69	{ "I", { 0 + 52, BVT_LE_UINT32, 1e-5, }, 5, SR_MQ_CURRENT, SR_UNIT_AMPERE },
70	{ "D+", { 64 + 32, BVT_LE_UINT32, 1e-2, }, 2, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
71	{ "D-", { 64 + 36, BVT_LE_UINT32, 1e-2, }, 2, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
72	{ "E0", { 64 + 12, BVT_LE_UINT32, 1e-3, }, 3, SR_MQ_ENERGY, SR_UNIT_WATT_HOUR },
73	{ "E1", { 64 + 20, BVT_LE_UINT32, 1e-3, }, 3, SR_MQ_ENERGY, SR_UNIT_WATT_HOUR },
0988d4ae	74	ALL_ZERO,
cae33a58 AS	75	};
	76
	77	static int check_pac_crc(uint8_t *data)
	78	{
	79	uint16_t crc;
	80	uint32_t crc_field;
	81
	82	crc = sr_crc16(SR_CRC16_DEFAULT_INIT, data, PAC_DATA_LEN);
	83	crc_field = RL32(data + PAC_DATA_LEN);
	84
	85	if (crc != crc_field) {
	86	sr_spew("CRC error. Calculated: %0x" PRIx16 ", expected: %0x" PRIx32,
	87	crc, crc_field);
	88	return 0;
	89	} else {
	90	return 1;
	91	}
	92	}
	93
	94	static int process_poll_pkt(struct dev_context devc, uint8_t dst)
	95	{
	96	struct aes256_ctx ctx;
	97
	98	aes256_set_decrypt_key(&ctx, AES_KEY);
	99	aes256_decrypt(&ctx, TC_POLL_LEN, dst, devc->buf);
	100
	101	if (RL32(dst + OFF_PAC1) != MAGIC_PAC1 \|\|
	102	RL32(dst + OFF_PAC2) != MAGIC_PAC2 \|\|
	103	RL32(dst + OFF_PAC3) != MAGIC_PAC3) {
	104	sr_err("Invalid poll packet magic values!");
	105	return SR_ERR;
	106	}
	107
	108	if (!check_pac_crc(dst + OFF_PAC1) \|\|
	109	!check_pac_crc(dst + OFF_PAC2) \|\|
	110	!check_pac_crc(dst + OFF_PAC3)) {
	111	sr_err("Invalid poll checksum!");
	112	return SR_ERR;
	113	}
	114
	115	return SR_OK;
	116	}
	117
	118	SR_PRIV int rdtech_tc_probe(struct sr_serial_dev_inst serial, struct dev_context devc)
	119	{
	120	int len;
	121	uint8_t poll_pkt[TC_POLL_LEN];
	122
	123	if (serial_write_blocking(serial, &POLL_CMD, sizeof(POLL_CMD) - 1,
	124	SERIAL_WRITE_TIMEOUT_MS) < 0) {
	125	sr_err("Unable to send probe request.");
	126	return SR_ERR;
	127	}
	128
	129	len = serial_read_blocking(serial, devc->buf, TC_POLL_LEN, TC_TIMEOUT_MS);
	130	if (len != TC_POLL_LEN) {
	131	sr_err("Failed to read probe response.");
	132	return SR_ERR;
	133	}
	134
	135	if (process_poll_pkt(devc, poll_pkt) != SR_OK) {
	136	sr_err("Unrecognized TC device!");
	137	return SR_ERR;
	138	}
139
140	devc->channels = rdtech_tc_channels;
141	devc->dev_info.model_name = g_strndup((const char *)poll_pkt + OFF_MODEL, LEN_MODEL);
142	devc->dev_info.fw_ver = g_strndup((const char *)poll_pkt + OFF_FW_VER, LEN_FW_VER);
143	devc->dev_info.serial_num = RL32(poll_pkt + OFF_SERIAL);
144
145	return SR_OK;
146	}
147
148	SR_PRIV int rdtech_tc_poll(const struct sr_dev_inst *sdi)
149	{
150	struct dev_context *devc = sdi->priv;
151	struct sr_serial_dev_inst *serial = sdi->conn;
152
153	if (serial_write_blocking(serial, &POLL_CMD, sizeof(POLL_CMD) - 1,
154	SERIAL_WRITE_TIMEOUT_MS) < 0) {
155	sr_err("Unable to send poll request.");
156	return SR_ERR;
157	}
158
159	devc->cmd_sent_at = g_get_monotonic_time() / 1000;
160
161	return SR_OK;
162	}
163
164	static void handle_poll_data(const struct sr_dev_inst *sdi)
165	{
166	struct dev_context *devc = sdi->priv;
167	uint8_t poll_pkt[TC_POLL_LEN];
168	int i;
169	GSList *ch;
170
171	sr_spew("Received poll packet (len: %d).", devc->buflen);
172	if (devc->buflen != TC_POLL_LEN) {
173	sr_err("Unexpected poll packet length: %i", devc->buflen);
174	return;
175	}
176
177	if (process_poll_pkt(devc, poll_pkt) != SR_OK) {
178	sr_err("Failed to process poll packet.");
179	return;
180	}
181
182	for (ch = sdi->channels, i = 0; ch; ch = g_slist_next(ch), i++) {
183	bv_send_analog_channel(sdi, ch->data,
184	&devc->channels[i], poll_pkt, TC_POLL_LEN);
185	}
186
187	sr_sw_limits_update_samples_read(&devc->limits, 1);
188	}
189
190	static void recv_poll_data(struct sr_dev_inst sdi, struct sr_serial_dev_inst serial)
191	{
192	struct dev_context *devc = sdi->priv;
193	int len;
194
195	/* Serial data arrived. */
196	while (devc->buflen < TC_POLL_LEN) {
197	len = serial_read_nonblocking(serial, devc->buf + devc->buflen, 1);
198	if (len < 1)
199	return;
200
201	devc->buflen++;
202	}
203
204	if (devc->buflen == TC_POLL_LEN)
205	handle_poll_data(sdi);
206
207	devc->buflen = 0;
208	}
209
210	SR_PRIV int rdtech_tc_receive_data(int fd, int revents, void *cb_data)
211	{
212	struct sr_dev_inst *sdi;
213	struct dev_context *devc;
214	struct sr_serial_dev_inst *serial;
215	int64_t now, elapsed;
216
217	(void)fd;
218
219	if (!(sdi = cb_data))
220	return TRUE;
221
222	if (!(devc = sdi->priv))
223	return TRUE;
224
225	serial = sdi->conn;
226	if (revents == G_IO_IN)
227	recv_poll_data(sdi, serial);
228
229	if (sr_sw_limits_check(&devc->limits)) {
230	sr_dev_acquisition_stop(sdi);
231	return TRUE;
232	}
233
234	now = g_get_monotonic_time() / 1000;
235	elapsed = now - devc->cmd_sent_at;
236
237	if (elapsed > TC_POLL_PERIOD_MS)
238	rdtech_tc_poll(sdi);
239
240	return TRUE;
241	}