src/hardware/rdtech-tc/protocol.c

   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
  22 #include <glib.h>
  23 #include <libsigrok/libsigrok.h>
  24 #include <math.h>
  25 #include <nettle/aes.h>
  26 #include <stdlib.h>
  27 #include <string.h>
  28
  29 #include "libsigrok-internal.h"
  30 #include "protocol.h"
  31
  32 #define PROBE_TO_MS     1000
  33 #define WRITE_TO_MS     1
  34 #define POLL_PERIOD_MS  100
  35
  36 static const char *poll_cmd = "getva";
  37
  38 /*
  39  * Response data (raw sample data) consists of three adjacent chunks
  40  * of 64 bytes each. These chunks start with their magic string, and
  41  * end in a 32bit checksum field. Measurement values are scattered
  42  * across these 192 bytes total size. All multi-byte integer values
  43  * are represented in little endian format. Typical size is 32 bits.
  44  */
  45
  46 #define MAGIC_PAC1      0x70616331      /* 'pac1' */
  47 #define MAGIC_PAC2      0x70616332      /* 'pac2' */
  48 #define MAGIC_PAC3      0x70616333      /* 'pac3' */
  49
  50 #define PAC_LEN 64
  51 #define PAC_CRC_POS (PAC_LEN - sizeof(uint32_t))
  52
  53 /* Offset to PAC block from start of poll data */
  54 #define OFF_PAC1 (0 * PAC_LEN)
  55 #define OFF_PAC2 (1 * PAC_LEN)
  56 #define OFF_PAC3 (2 * PAC_LEN)
  57 #define TC_POLL_LEN (3 * PAC_LEN)
  58
  59 #define OFF_MODEL 4
  60 #define LEN_MODEL 4
  61
  62 #define OFF_FW_VER 8
  63 #define LEN_FW_VER 4
  64
  65 #define OFF_SERIAL 12
  66
  67 static const uint8_t AES_KEY[] = {
  68         0x58, 0x21, 0xfa, 0x56, 0x01, 0xb2, 0xf0, 0x26,
  69         0x87, 0xff, 0x12, 0x04, 0x62, 0x2a, 0x4f, 0xb0,
  70         0x86, 0xf4, 0x02, 0x60, 0x81, 0x6f, 0x9a, 0x0b,
  71         0xa7, 0xf1, 0x06, 0x61, 0x9a, 0xb8, 0x72, 0x88,
  72 };
  73
  74 static const struct rdtech_tc_channel_desc rdtech_tc_channels[] = {
  75         { "V",  {   0 + 48, BVT_LE_UINT32, 1, }, { 100, 1e6, }, 4, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
  76         { "I",  {   0 + 52, BVT_LE_UINT32, 1, }, {  10, 1e6, }, 5, SR_MQ_CURRENT, SR_UNIT_AMPERE },
  77         { "D+", {  64 + 32, BVT_LE_UINT32, 1, }, {  10, 1e3, }, 2, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
  78         { "D-", {  64 + 36, BVT_LE_UINT32, 1, }, {  10, 1e3, }, 2, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
  79         { "E0", {  64 + 12, BVT_LE_UINT32, 1, }, {   1, 1e3, }, 3, SR_MQ_ENERGY, SR_UNIT_WATT_HOUR },
  80         { "E1", {  64 + 20, BVT_LE_UINT32, 1, }, {   1, 1e3, }, 3, SR_MQ_ENERGY, SR_UNIT_WATT_HOUR },
  81 };
  82
  83 static gboolean check_pac_crc(uint8_t *data)
  84 {
  85         uint16_t crc;
  86         uint32_t crc_field;
  87
  88         crc = sr_crc16(SR_CRC16_DEFAULT_INIT, data, PAC_CRC_POS);
  89         crc_field = read_u32le(&data[PAC_CRC_POS]);
  90         if (crc != crc_field) {
  91                 sr_spew("CRC error. Calculated: %0x" PRIx16 ", expected: %0x" PRIx32,
  92                         crc, crc_field);
  93                 return FALSE;
  94         }
  95
  96         return TRUE;
  97 }
  98
  99 static int process_poll_pkt(struct dev_context *devc, uint8_t *dst)
 100 {
 101         struct aes256_ctx ctx;
 102         gboolean ok;
 103
 104         aes256_set_decrypt_key(&ctx, AES_KEY);
 105         aes256_decrypt(&ctx, TC_POLL_LEN, dst, devc->buf);
 106
 107         ok = TRUE;
 108         ok &= read_u32be(&dst[OFF_PAC1]) == MAGIC_PAC1;
 109         ok &= read_u32be(&dst[OFF_PAC2]) == MAGIC_PAC2;
 110         ok &= read_u32be(&dst[OFF_PAC3]) == MAGIC_PAC3;
 111         if (!ok) {
 112                 sr_err("Invalid poll response packet (magic values).");
 113                 return SR_ERR_DATA;
 114         }
 115
 116         ok &= check_pac_crc(&dst[OFF_PAC1]);
 117         ok &= check_pac_crc(&dst[OFF_PAC2]);
 118         ok &= check_pac_crc(&dst[OFF_PAC3]);
 119         if (!ok) {
 120                 sr_err("Invalid poll response packet (checksum).");
 121                 return SR_ERR_DATA;
 122         }
 123
 124         return SR_OK;
 125 }
 126
 127 SR_PRIV int rdtech_tc_probe(struct sr_serial_dev_inst *serial, struct dev_context *devc)
 128 {
 129         int len;
 130         uint8_t poll_pkt[TC_POLL_LEN];
 131
 132         len = serial_write_blocking(serial,
 133                 poll_cmd, strlen(poll_cmd), WRITE_TO_MS);
 134         if (len < 0) {
 135                 sr_err("Failed to send probe request.");
 136                 return SR_ERR;
 137         }
 138
 139         len = serial_read_blocking(serial, devc->buf, TC_POLL_LEN, PROBE_TO_MS);
 140         if (len != TC_POLL_LEN) {
 141                 sr_err("Failed to read probe response.");
 142                 return SR_ERR;
 143         }
 144
 145         if (process_poll_pkt(devc, poll_pkt) != SR_OK) {
 146                 sr_err("Unrecognized TC device!");
 147                 return SR_ERR;
 148         }
 149
 150         devc->channels = rdtech_tc_channels;
 151         devc->channel_count = ARRAY_SIZE(rdtech_tc_channels);
 152         devc->dev_info.model_name = g_strndup((const char *)&poll_pkt[OFF_MODEL], LEN_MODEL);
 153         devc->dev_info.fw_ver = g_strndup((const char *)&poll_pkt[OFF_FW_VER], LEN_FW_VER);
 154         devc->dev_info.serial_num = read_u32le(&poll_pkt[OFF_SERIAL]);
 155
 156         return SR_OK;
 157 }
 158
 159 SR_PRIV int rdtech_tc_poll(const struct sr_dev_inst *sdi, gboolean force)
 160 {
 161         struct dev_context *devc;
 162         int64_t now, elapsed;
 163         struct sr_serial_dev_inst *serial;
 164         int len;
 165
 166         /*
 167          * Don't send the request while receive data is being accumulated.
 168          */
 169         devc = sdi->priv;
 170         if (!force && devc->buflen)
 171                 return SR_OK;
 172
 173         /*
 174          * Send the request when the transmit interval was reached. Or
 175          * when the caller forced the transmission.
 176          */
 177         now = g_get_monotonic_time() / 1000;
 178         elapsed = now - devc->cmd_sent_at;
 179         if (!force && elapsed < POLL_PERIOD_MS)
 180                 return SR_OK;
 181
 182         /*
 183          * Transmit another measurement request. Only advance the
 184          * interval after successful transmission.
 185          */
 186         serial = sdi->conn;
 187         len = serial_write_blocking(serial,
 188                 poll_cmd, strlen(poll_cmd), WRITE_TO_MS);
 189         if (len < 0) {
 190                 sr_err("Unable to send poll request.");
 191                 return SR_ERR;
 192         }
 193         devc->cmd_sent_at = now;
 194
 195         return SR_OK;
 196 }
 197
 198 static int handle_poll_data(struct sr_dev_inst *sdi)
 199 {
 200         struct dev_context *devc;
 201         uint8_t poll_pkt[TC_POLL_LEN];
 202         size_t ch_idx;
 203         const struct rdtech_tc_channel_desc *pch;
 204         int ret;
 205         float v;
 206
 207         devc = sdi->priv;
 208         sr_spew("Received poll packet (len: %zu).", devc->buflen);
 209         if (devc->buflen < TC_POLL_LEN) {
 210                 sr_err("Insufficient poll packet length: %zu", devc->buflen);
 211                 return SR_ERR_DATA;
 212         }
 213
 214         if (process_poll_pkt(devc, poll_pkt) != SR_OK) {
 215                 sr_err("Failed to process poll packet.");
 216                 return SR_ERR_DATA;
 217         }
 218
 219         ret = SR_OK;
 220         std_session_send_df_frame_begin(sdi);
 221         for (ch_idx = 0; ch_idx < devc->channel_count; ch_idx++) {
 222                 pch = &devc->channels[ch_idx];
 223                 ret = bv_get_value(&v, &pch->spec, poll_pkt, TC_POLL_LEN);
 224                 if (ret != SR_OK)
 225                         break;
 226                 ret = feed_queue_analog_submit(devc->feeds[ch_idx], v, 1);
 227                 if (ret != SR_OK)
 228                         break;
 229         }
 230         std_session_send_df_frame_end(sdi);
 231
 232         sr_sw_limits_update_frames_read(&devc->limits, 1);
 233         if (sr_sw_limits_check(&devc->limits))
 234                 sr_dev_acquisition_stop(sdi);
 235
 236         return ret;
 237 }
 238
 239 static int recv_poll_data(struct sr_dev_inst *sdi, struct sr_serial_dev_inst *serial)
 240 {
 241         struct dev_context *devc;
 242         size_t space;
 243         int len;
 244         int ret;
 245
 246         /* Receive data became available. Drain the transport layer. */
 247         devc = sdi->priv;
 248         while (devc->buflen < TC_POLL_LEN) {
 249                 space = sizeof(devc->buf) - devc->buflen;
 250                 len = serial_read_nonblocking(serial,
 251                         &devc->buf[devc->buflen], space);
 252                 if (len < 0)
 253                         return SR_ERR_IO;
 254                 if (len == 0)
 255                         return SR_OK;
 256                 devc->buflen += len;
 257         }
 258
 259         /*
 260          * TODO Want to (re-)synchronize to the packet stream? The
 261          * 'pac1' string literal would be a perfect match for that.
 262          */
 263
 264         /* Process packets when their reception has completed. */
 265         while (devc->buflen >= TC_POLL_LEN) {
 266                 ret = handle_poll_data(sdi);
 267                 if (ret != SR_OK)
 268                         return ret;
 269                 devc->buflen -= TC_POLL_LEN;
 270                 if (devc->buflen)
 271                         memmove(&devc->buf[0], &devc->buf[TC_POLL_LEN], devc->buflen);
 272         }
 273
 274         return SR_OK;
 275 }
 276
 277 SR_PRIV int rdtech_tc_receive_data(int fd, int revents, void *cb_data)
 278 {
 279         struct sr_dev_inst *sdi;
 280         struct dev_context *devc;
 281         struct sr_serial_dev_inst *serial;
 282         int ret;
 283
 284         (void)fd;
 285
 286         if (!(sdi = cb_data))
 287                 return TRUE;
 288         if (!(devc = sdi->priv))
 289                 return TRUE;
 290
 291         /* Handle availability of receive data. */
 292         serial = sdi->conn;
 293         if (revents == G_IO_IN) {
 294                 ret = recv_poll_data(sdi, serial);
 295                 if (ret != SR_OK)
 296                         sr_dev_acquisition_stop(sdi);
 297         }
 298
 299         /* Check configured acquisition limits. */
 300         if (sr_sw_limits_check(&devc->limits)) {
 301                 sr_dev_acquisition_stop(sdi);
 302                 return TRUE;
 303         }
 304
 305         /* Periodically retransmit measurement requests. */
 306         (void)rdtech_tc_poll(sdi, FALSE);
 307
 308         return TRUE;
 309 }