BLAHAJ
/
libsigrok
10
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

testo: More robust probing and packet checking. 

The CRC in every packet is now also checked. Thanks to Aurelien
Jacobs for the CRC function.
This commit is contained in:
Bert Vermeulen 2014-07-07 01:14:06 +02:00
parent 6dcb97230e
commit 8789564070
3 changed files with 72 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
hardware/testo/api.c
View File

@ -320,6 +320,7 @@ static void receive_data(struct sr_dev_inst *sdi, unsigned char *data, int len)
{ {
struct dev_context *devc; struct dev_context *devc;
int packet_size; int packet_size;
uint16_t crc;
devc = sdi->priv; 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); memcpy(devc->reply + devc->reply_size, data, len);
devc->reply_size += len; devc->reply_size += len;
/* Sixth byte contains the length of the packet. */ /* Sixth byte contains the length of the packet. */
if (devc->reply_size >= 7) { if (devc->reply_size < 7)
packet_size = 7 + devc->reply[6] * 7 + 2; return;
if (devc->reply_size >= packet_size) {
testo_receive_packet(sdi); packet_size = 7 + devc->reply[6] * 7 + 2;
devc->num_samples++; if (devc->reply_size < packet_size)
devc->reply_size = 0; return;
if (devc->limit_samples && devc->num_samples >= devc->limit_samples)
dev_acquisition_stop(sdi, devc->cb_data); if (!testo_check_packet_prefix(devc->reply, devc->reply_size))
else return;
testo_request_packet(sdi);
} 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) SR_PRIV void receive_transfer(struct libusb_transfer *transfer)

64
hardware/testo/protocol.c
View File

@ -51,6 +51,7 @@ SR_PRIV int testo_set_serial_params(struct sr_usb_dev_inst *usb)
return SR_OK; return SR_OK;
} }
/* Due to the modular nature of the Testo hardware, you can't assume /* Due to the modular nature of the Testo hardware, you can't assume
* which measurements the device will supply. Fetch a single result * which measurements the device will supply. Fetch a single result
* set synchronously to see which measurements it has. */ * 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; devc = sdi->priv;
usb = sdi->conn; usb = sdi->conn;
sr_dbg("Probing for channels.");
if (sdi->driver->dev_open(sdi) != SR_OK) if (sdi->driver->dev_open(sdi) != SR_OK)
return SR_ERR; return SR_ERR;
if (testo_set_serial_params(usb) != SR_OK) if (testo_set_serial_params(usb) != SR_OK)
return SR_ERR; 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, if (libusb_bulk_transfer(usb->devhdl, EP_OUT, devc->model->request,
devc->model->request_size, &devc->reply_size, 10) < 0) devc->model->request_size, &devc->reply_size, 10) < 0)
return SR_ERR; return SR_ERR;
packet_len = 0; packet_len = 0;
while(TRUE) { while(TRUE) {
if (libusb_bulk_transfer(usb->devhdl, EP_IN, buf, MAX_REPLY_SIZE, 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) if (len == 2)
/* FTDI cruft */ /* FTDI cruft */
continue; continue;
sr_dbg("got %d", len);
if (packet_len + len - 2 > MAX_REPLY_SIZE) if (packet_len + len - 2 > MAX_REPLY_SIZE)
return SR_ERR; return SR_ERR;
memcpy(packet + packet_len, buf + 2, len - 2); memcpy(packet + packet_len, buf + 2, len - 2);
packet_len += 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_prefix(packet, packet_len)) {
if (!testo_check_packet(packet, packet_len)) /* Tail end of some previous data, drop it. */
return SR_ERR; packet_len = 0;
if (packet_len >= 7 + packet[6] * 7 + 2) continue;
/* Got a complete packet. */
break;
} }
if (packet_len >= 7 + packet[6] * 7 + 2)
/* Got a complete packet. */
break;
} }
sdi->driver->dev_close(sdi);
if (packet[6] > MAX_CHANNELS) { if (packet[6] > MAX_CHANNELS) {
sr_err("Device says it has %d channels!", packet[6]); 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); sdi->channels = g_slist_append(sdi->channels, ch);
} }
devc->num_channels = packet[6]; devc->num_channels = packet[6];
sr_dbg("Found %d channel%s.", devc->num_channels,
sdi->driver->dev_close(sdi); devc->num_channels > 1 ? "s" : "");
return SR_OK; 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 /* 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. */ * 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; int i;
unsigned char check[] = { 0x21, 0, 0, 0, 1 }; 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; 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) static float binary32_le_to_float(unsigned char *buf)
{ {
GFloatIEEE754 f; GFloatIEEE754 f;
@ -211,9 +244,6 @@ SR_PRIV void testo_receive_packet(const struct sr_dev_inst *sdi)
g_string_free(dbg, TRUE); g_string_free(dbg, TRUE);
} }
if (!testo_check_packet(devc->reply, devc->reply_size))
return;
packet.type = SR_DF_ANALOG; packet.type = SR_DF_ANALOG;
packet.payload = &analog; packet.payload = &analog;
analog.num_samples = 1; 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++) { for (i = 0; i < devc->reply[6]; i++) {
buf = devc->reply + 7 + i * 7; buf = devc->reply + 7 + i * 7;
value = binary32_le_to_float(buf); value = binary32_le_to_float(buf);
sr_dbg("value: %f unit %d exp %d", value, buf[4], buf[6]);
switch (buf[4]) { switch (buf[4]) {
case 1: case 1:
analog.mq = SR_MQ_TEMPERATURE; 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; analog.unit = SR_UNIT_HECTOPASCAL;
break; break;
default: default:
sr_dbg("Unsupported measurement unit %d", buf[4]); sr_dbg("Unsupported measurement unit %d.", buf[4]);
return; return;
} }
@ -257,7 +286,6 @@ SR_PRIV void testo_receive_packet(const struct sr_dev_inst *sdi)
return; return;
} }
ch = g_slist_nth_data(sdi->channels, i); 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); analog.channels = g_slist_append(NULL, ch);
sr_session_send(sdi, &packet); sr_session_send(sdi, &packet);
g_slist_free(analog.channels); g_slist_free(analog.channels);

3
hardware/testo/protocol.h
View File

@ -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 int testo_probe_channels(struct sr_dev_inst *sdi);
SR_PRIV void receive_transfer(struct libusb_transfer *transfer); SR_PRIV void receive_transfer(struct libusb_transfer *transfer);
SR_PRIV int testo_request_packet(const struct sr_dev_inst *sdi); 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); SR_PRIV void testo_receive_packet(const struct sr_dev_inst *sdi);
#endif #endif