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 binary_analog_channel rdtech_tc_channels[] = {
  75         { "V",  {   0 + 48, BVT_LE_UINT32, 1e-4, }, 4, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
  76         { "I",  {   0 + 52, BVT_LE_UINT32, 1e-5, }, 5, SR_MQ_CURRENT, SR_UNIT_AMPERE },
  77         { "D+", {  64 + 32, BVT_LE_UINT32, 1e-2, }, 2, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
  78         { "D-", {  64 + 36, BVT_LE_UINT32, 1e-2, }, 2, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
  79         { "E0", {  64 + 12, BVT_LE_UINT32, 1e-3, }, 3, SR_MQ_ENERGY, SR_UNIT_WATT_HOUR },
  80         { "E1", {  64 + 20, BVT_LE_UINT32, 1e-3, }, 3, SR_MQ_ENERGY, SR_UNIT_WATT_HOUR },
  81         ALL_ZERO,
  82 };
  83
  84 static gboolean check_pac_crc(uint8_t *data)
  85 {
  86         uint16_t crc;
  87         uint32_t crc_field;
  88
  89         crc = sr_crc16(SR_CRC16_DEFAULT_INIT, data, PAC_CRC_POS);
  90         crc_field = read_u32le(&data[PAC_CRC_POS]);
  91         if (crc != crc_field) {
  92                 sr_spew("CRC error. Calculated: %0x" PRIx16 ", expected: %0x" PRIx32,
  93                         crc, crc_field);
  94                 return FALSE;
  95         }
  96
  97         return TRUE;
  98 }
  99
 100 static int process_poll_pkt(struct dev_context *devc, uint8_t *dst)
 101 {
 102         struct aes256_ctx ctx;
 103         gboolean ok;
 104
 105         aes256_set_decrypt_key(&ctx, AES_KEY);
 106         aes256_decrypt(&ctx, TC_POLL_LEN, dst, devc->buf);
 107
 108         ok = TRUE;
 109         ok &= read_u32be(&dst[OFF_PAC1]) == MAGIC_PAC1;
 110         ok &= read_u32be(&dst[OFF_PAC2]) == MAGIC_PAC2;
 111         ok &= read_u32be(&dst[OFF_PAC3]) == MAGIC_PAC3;
 112         if (!ok) {
 113                 sr_err("Invalid poll response packet (magic values).");
 114                 return SR_ERR_DATA;
 115         }
 116
 117         ok &= check_pac_crc(&dst[OFF_PAC1]);
 118         ok &= check_pac_crc(&dst[OFF_PAC2]);
 119         ok &= check_pac_crc(&dst[OFF_PAC3]);
 120         if (!ok) {
 121                 sr_err("Invalid poll response packet (checksum).");
 122                 return SR_ERR_DATA;
 123         }
 124
 125         return SR_OK;
 126 }
 127
 128 SR_PRIV int rdtech_tc_probe(struct sr_serial_dev_inst *serial, struct dev_context *devc)
 129 {
 130         int len;
 131         uint8_t poll_pkt[TC_POLL_LEN];
 132
 133         len = serial_write_blocking(serial,
 134                 poll_cmd, strlen(poll_cmd), WRITE_TO_MS);
 135         if (len < 0) {
 136                 sr_err("Failed to send probe request.");
 137                 return SR_ERR;
 138         }
 139
 140         len = serial_read_blocking(serial, devc->buf, TC_POLL_LEN, PROBE_TO_MS);
 141         if (len != TC_POLL_LEN) {
 142                 sr_err("Failed to read probe response.");
 143                 return SR_ERR;
 144         }
 145
 146         if (process_poll_pkt(devc, poll_pkt) != SR_OK) {
 147                 sr_err("Unrecognized TC device!");
 148                 return SR_ERR;
 149         }
 150
 151         devc->channels = 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         devc = sdi->priv;
 167         now = g_get_monotonic_time() / 1000;
 168         elapsed = now - devc->cmd_sent_at;
 169         if (!force && elapsed < POLL_PERIOD_MS)
 170                 return SR_OK;
 171
 172         serial = sdi->conn;
 173         len = serial_write_blocking(serial,
 174                 poll_cmd, strlen(poll_cmd), WRITE_TO_MS);
 175         if (len < 0) {
 176                 sr_err("Unable to send poll request.");
 177                 return SR_ERR;
 178         }
 179         devc->cmd_sent_at = now;
 180
 181         return SR_OK;
 182 }
 183
 184 static int handle_poll_data(struct sr_dev_inst *sdi)
 185 {
 186         struct dev_context *devc;
 187         uint8_t poll_pkt[TC_POLL_LEN];
 188         size_t i;
 189         GSList *ch;
 190         int ret;
 191
 192         devc = sdi->priv;
 193         sr_spew("Received poll packet (len: %zu).", devc->buflen);
 194         if (devc->buflen != TC_POLL_LEN) {
 195                 sr_err("Unexpected poll packet length: %zu", devc->buflen);
 196                 return SR_ERR_DATA;
 197         }
 198
 199         if (process_poll_pkt(devc, poll_pkt) != SR_OK) {
 200                 sr_err("Failed to process poll packet.");
 201                 return SR_ERR_DATA;
 202         }
 203
 204         i = 0;
 205         for (ch = sdi->channels; ch; ch = g_slist_next(ch)) {
 206                 ret = bv_send_analog_channel(sdi, ch->data,
 207                         &devc->channels[i], poll_pkt, TC_POLL_LEN);
 208                 i++;
 209                 if (ret != SR_OK)
 210                         return ret;
 211         }
 212
 213         sr_sw_limits_update_samples_read(&devc->limits, 1);
 214         if (sr_sw_limits_check(&devc->limits))
 215                 sr_dev_acquisition_stop(sdi);
 216
 217         return SR_OK;
 218 }
 219
 220 static int recv_poll_data(struct sr_dev_inst *sdi, struct sr_serial_dev_inst *serial)
 221 {
 222         struct dev_context *devc;
 223         int len;
 224         int ret;
 225
 226         /* Serial data arrived. */
 227         devc = sdi->priv;
 228         while (devc->buflen < TC_POLL_LEN) {
 229                 len = serial_read_nonblocking(serial, devc->buf + devc->buflen, 1);
 230                 if (len < 0)
 231                         return SR_ERR_IO;
 232                 if (len == 0)
 233                         return SR_OK;
 234                 devc->buflen += len;
 235         }
 236
 237         if (devc->buflen == TC_POLL_LEN) {
 238                 ret = handle_poll_data(sdi);
 239                 if (ret != SR_OK)
 240                         return ret;
 241         }
 242         devc->buflen = 0;
 243
 244         return SR_OK;
 245 }
 246
 247 SR_PRIV int rdtech_tc_receive_data(int fd, int revents, void *cb_data)
 248 {
 249         struct sr_dev_inst *sdi;
 250         struct dev_context *devc;
 251         struct sr_serial_dev_inst *serial;
 252         int ret;
 253
 254         (void)fd;
 255
 256         if (!(sdi = cb_data))
 257                 return TRUE;
 258         if (!(devc = sdi->priv))
 259                 return TRUE;
 260
 261         /* Handle availability of receive data. */
 262         serial = sdi->conn;
 263         if (revents == G_IO_IN) {
 264                 ret = recv_poll_data(sdi, serial);
 265                 if (ret != SR_OK)
 266                         sr_dev_acquisition_stop(sdi);
 267         }
 268
 269         /* Check configured acquisition limits. */
 270         if (sr_sw_limits_check(&devc->limits)) {
 271                 sr_dev_acquisition_stop(sdi);
 272                 return TRUE;
 273         }
 274
 275         /* Periodically retransmit measurement requests. */
 276         (void)rdtech_tc_poll(sdi, FALSE);
 277
 278         return TRUE;
 279 }