src/hardware/testo/protocol.c

   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
  20 #include <config.h>
  21 #include <string.h>
  22 #include "protocol.h"
  23
  24 SR_PRIV int testo_set_serial_params(struct sr_usb_dev_inst *usb)
  25 {
  26         int ret;
  27
  28         if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_BAUDRATE,
  29                         FTDI_BAUDRATE_115200, FTDI_INDEX, NULL, 0, 10)) < 0) {
  30                 sr_err("Failed to set baudrate: %s", libusb_error_name(ret));
  31                 return SR_ERR;
  32         }
  33
  34         if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_PARAMS,
  35                         FTDI_PARAMS_8N1, FTDI_INDEX, NULL, 0, 10)) < 0) {
  36                 sr_err("Failed to set comm parameters: %s", libusb_error_name(ret));
  37                 return SR_ERR;
  38         }
  39
  40         if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_FLOWCTRL,
  41                         FTDI_FLOW_NONE, FTDI_INDEX, NULL, 0, 10)) < 0) {
  42                 sr_err("Failed to set flow control: %s", libusb_error_name(ret));
  43                 return SR_ERR;
  44         }
  45
  46         if ((ret = libusb_control_transfer(usb->devhdl, 0x40, FTDI_SET_MODEMCTRL,
  47                         FTDI_MODEM_ALLHIGH, FTDI_INDEX, NULL, 0, 10)) < 0) {
  48                 sr_err("Failed to set modem control: %s", libusb_error_name(ret));
  49                 return SR_ERR;
  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)
  59 {
  60         struct dev_context *devc;
  61         struct sr_usb_dev_inst *usb;
  62         int unit, packet_len, len, i;
  63         unsigned char packet[MAX_REPLY_SIZE], buf[MAX_REPLY_SIZE];
  64         const char *probe_name;
  65
  66         devc = sdi->priv;
  67         usb = sdi->conn;
  68
  69         sr_dbg("Probing for channels.");
  70         if (sdi->driver->dev_open(sdi) != SR_OK)
  71                 return SR_ERR;
  72         if (testo_set_serial_params(usb) != SR_OK)
  73                 return SR_ERR;
  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
  80         if (libusb_bulk_transfer(usb->devhdl, EP_OUT, (unsigned char *)devc->model->request,
  81                         devc->model->request_size, &devc->reply_size, 10) < 0)
  82                 return SR_ERR;
  83
  84         packet_len = 0;
  85         while (TRUE) {
  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;
  92                 if (packet_len + len - 2 > MAX_REPLY_SIZE)
  93                         return SR_ERR;
  94
  95                 memcpy(packet + packet_len, buf + 2, len - 2);
  96                 packet_len += len - 2;
  97                 if (packet_len < 5)
  98                         /* Not even enough to check prefix yet. */
  99                         continue;
 100
 101                 if (!testo_check_packet_prefix(packet, packet_len)) {
 102                         /* Tail end of some previous data, drop it. */
 103                         packet_len = 0;
 104                         continue;
 105                 }
 106
 107                 if (packet_len >= 7 + packet[6] * 7 + 2)
 108                         /* Got a complete packet. */
 109                         break;
 110         }
 111         sr_dev_close(sdi);
 112
 113         if (packet[6] > MAX_CHANNELS) {
 114                 sr_err("Device says it has %d channels!", packet[6]);
 115                 return SR_ERR;
 116         }
 117
 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                 }
 138                 sr_channel_new(sdi, i, SR_CHANNEL_ANALOG, TRUE, probe_name);
 139         }
 140         devc->num_channels = packet[6];
 141         sr_dbg("Found %d channel%s.", devc->num_channels,
 142                         devc->num_channels > 1 ? "s" : "");
 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,
 157                         (unsigned char *)devc->model->request, devc->model->request_size,
 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));
 161                 sr_dev_acquisition_stop((struct sr_dev_inst *)sdi);
 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. */
 171 SR_PRIV gboolean testo_check_packet_prefix(unsigned char *buf, int len)
 172 {
 173         static const unsigned char check[] = { 0x21, 0, 0, 0, 1 };
 174         int i;
 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
 186         return TRUE;
 187 }
 188
 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
 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;
 225         struct sr_datafeed_analog analog;
 226         struct sr_analog_encoding encoding;
 227         struct sr_analog_meaning meaning;
 228         struct sr_analog_spec spec;
 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
 247         packet.type = SR_DF_ANALOG;
 248         packet.payload = &analog;
 249         /* TODO: Use proper 'digits' value for this device (and its modes). */
 250         sr_analog_init(&analog, &encoding, &meaning, &spec, 2);
 251         analog.num_samples = 1;
 252         analog.meaning->mqflags = 0;
 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);
 258                 switch (buf[4]) {
 259                 case 1:
 260                         analog.meaning->mq = SR_MQ_TEMPERATURE;
 261                         analog.meaning->unit = SR_UNIT_CELSIUS;
 262                         break;
 263                 case 3:
 264                         analog.meaning->mq = SR_MQ_RELATIVE_HUMIDITY;
 265                         analog.meaning->unit = SR_UNIT_HUMIDITY_293K;
 266                         break;
 267                 case 5:
 268                         analog.meaning->mq = SR_MQ_WIND_SPEED;
 269                         analog.meaning->unit = SR_UNIT_METER_SECOND;
 270                         break;
 271                 case 24:
 272                         analog.meaning->mq = SR_MQ_PRESSURE;
 273                         analog.meaning->unit = SR_UNIT_HECTOPASCAL;
 274                         break;
 275                 default:
 276                         sr_dbg("Unsupported measurement unit %d.", buf[4]);
 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);
 291                 analog.meaning->channels = g_slist_append(NULL, ch);
 292                 sr_session_send(sdi, &packet);
 293                 g_slist_free(analog.meaning->channels);
 294         }
 295 }