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 /*
  37  * Response data (raw sample data) consists of three adjacent chunks
  38  * of 64 bytes each. These chunks start with their magic string, and
  39  * end in a 32bit checksum field. Measurement values are scattered
  40  * across these 192 bytes total size. All multi-byte integer values
  41  * are represented in little endian format. Typical size is 32 bits.
  42  */
  43
  44 #define MAGIC_PAC1      0x70616331      /* 'pac1' */
  45 #define MAGIC_PAC2      0x70616332      /* 'pac2' */
  46 #define MAGIC_PAC3      0x70616333      /* 'pac3' */
  47
  48 #define PAC_LEN 64
  49 #define PAC_CRC_POS (PAC_LEN - sizeof(uint32_t))
  50
  51 /* Offset to PAC block from start of poll data */
  52 #define OFF_PAC1 (0 * PAC_LEN)
  53 #define OFF_PAC2 (1 * PAC_LEN)
  54 #define OFF_PAC3 (2 * PAC_LEN)
  55 #define TC_POLL_LEN (3 * PAC_LEN)
  56 #if TC_POLL_LEN > RDTECH_TC_RSPBUFSIZE
  57 #  error "response length exceeds receive buffer space"
  58 #endif
  59
  60 #define OFF_MODEL 4
  61 #define LEN_MODEL 4
  62
  63 #define OFF_FW_VER 8
  64 #define LEN_FW_VER 4
  65
  66 #define OFF_SERIAL 12
  67
  68 static const uint8_t AES_KEY[] = {
  69         0x58, 0x21, 0xfa, 0x56, 0x01, 0xb2, 0xf0, 0x26,
  70         0x87, 0xff, 0x12, 0x04, 0x62, 0x2a, 0x4f, 0xb0,
  71         0x86, 0xf4, 0x02, 0x60, 0x81, 0x6f, 0x9a, 0x0b,
  72         0xa7, 0xf1, 0x06, 0x61, 0x9a, 0xb8, 0x72, 0x88,
  73 };
  74
  75 static const struct rdtech_tc_channel_desc rdtech_tc_channels[] = {
  76         { "V",  {   0 + 48, BVT_LE_UINT32, 1, }, { 100, 1e6, }, 4, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
  77         { "I",  {   0 + 52, BVT_LE_UINT32, 1, }, {  10, 1e6, }, 5, SR_MQ_CURRENT, SR_UNIT_AMPERE },
  78         { "D+", {  64 + 32, BVT_LE_UINT32, 1, }, {  10, 1e3, }, 2, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
  79         { "D-", {  64 + 36, BVT_LE_UINT32, 1, }, {  10, 1e3, }, 2, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
  80         { "E0", {  64 + 12, BVT_LE_UINT32, 1, }, {   1, 1e3, }, 3, SR_MQ_ENERGY, SR_UNIT_WATT_HOUR },
  81         { "E1", {  64 + 20, BVT_LE_UINT32, 1, }, {   1, 1e3, }, 3, SR_MQ_ENERGY, SR_UNIT_WATT_HOUR },
  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         if (sr_log_loglevel_get() >= SR_LOG_SPEW) {
 126                 static const size_t chunk_max = 32;
 127
 128                 const uint8_t *rdptr;
 129                 size_t rdlen, chunk_addr, chunk_len;
 130                 GString *txt;
 131
 132                 sr_spew("check passed on decrypted receive data");
 133                 rdptr = dst;
 134                 rdlen = TC_POLL_LEN;
 135                 chunk_addr = 0;
 136                 while (rdlen) {
 137                         chunk_len = rdlen;
 138                         if (chunk_len > chunk_max)
 139                                 chunk_len = chunk_max;
 140                         txt = sr_hexdump_new(rdptr, chunk_len);
 141                         sr_spew("%04zx  %s", chunk_addr, txt->str);
 142                         sr_hexdump_free(txt);
 143                         chunk_addr += chunk_len;
 144                         rdptr += chunk_len;
 145                         rdlen -= chunk_len;
 146                 }
 147         }
 148
 149         return SR_OK;
 150 }
 151
 152 SR_PRIV int rdtech_tc_probe(struct sr_serial_dev_inst *serial, struct dev_context *devc)
 153 {
 154         static const char *poll_cmd_cdc = "getva";
 155         static const char *poll_cmd_ble = "bgetva\r\n";
 156
 157         int len;
 158         uint8_t poll_pkt[TC_POLL_LEN];
 159
 160         /* Construct the request text. Which differs across transports. */
 161         devc->is_bluetooth = ser_name_is_bt(serial);
 162         snprintf(devc->req_text, sizeof(devc->req_text), "%s",
 163                 devc->is_bluetooth ? poll_cmd_ble : poll_cmd_cdc);
 164         sr_dbg("is bluetooth %d -> poll request '%s'.",
 165                 devc->is_bluetooth, devc->req_text);
 166
 167         /* Transmit the request. */
 168         len = serial_write_blocking(serial,
 169                 devc->req_text, strlen(devc->req_text), WRITE_TO_MS);
 170         if (len < 0) {
 171                 sr_err("Failed to send probe request.");
 172                 return SR_ERR;
 173         }
 174
 175         /* Receive a response. */
 176         len = serial_read_blocking(serial, devc->buf, TC_POLL_LEN, PROBE_TO_MS);
 177         if (len != TC_POLL_LEN) {
 178                 sr_err("Failed to read probe response.");
 179                 return SR_ERR;
 180         }
 181
 182         if (process_poll_pkt(devc, poll_pkt) != SR_OK) {
 183                 sr_err("Unrecognized TC device!");
 184                 return SR_ERR;
 185         }
 186
 187         devc->channels = rdtech_tc_channels;
 188         devc->channel_count = ARRAY_SIZE(rdtech_tc_channels);
 189         devc->dev_info.model_name = g_strndup((const char *)&poll_pkt[OFF_MODEL], LEN_MODEL);
 190         devc->dev_info.fw_ver = g_strndup((const char *)&poll_pkt[OFF_FW_VER], LEN_FW_VER);
 191         devc->dev_info.serial_num = read_u32le(&poll_pkt[OFF_SERIAL]);
 192
 193         return SR_OK;
 194 }
 195
 196 SR_PRIV int rdtech_tc_poll(const struct sr_dev_inst *sdi, gboolean force)
 197 {
 198         struct dev_context *devc;
 199         int64_t now, elapsed;
 200         struct sr_serial_dev_inst *serial;
 201         int len;
 202
 203         /*
 204          * Don't send the request while receive data is being accumulated.
 205          */
 206         devc = sdi->priv;
 207         if (!force && devc->rdlen)
 208                 return SR_OK;
 209
 210         /*
 211          * Send the request when the transmit interval was reached. Or
 212          * when the caller forced the transmission.
 213          */
 214         now = g_get_monotonic_time() / 1000;
 215         elapsed = now - devc->cmd_sent_at;
 216         if (!force && elapsed < POLL_PERIOD_MS)
 217                 return SR_OK;
 218
 219         /*
 220          * Transmit another measurement request. Only advance the
 221          * interval after successful transmission.
 222          */
 223         serial = sdi->conn;
 224         len = serial_write_blocking(serial,
 225                 devc->req_text, strlen(devc->req_text), WRITE_TO_MS);
 226         if (len < 0) {
 227                 sr_err("Unable to send poll request.");
 228                 return SR_ERR;
 229         }
 230         devc->cmd_sent_at = now;
 231
 232         return SR_OK;
 233 }
 234
 235 static int handle_poll_data(struct sr_dev_inst *sdi)
 236 {
 237         struct dev_context *devc;
 238         uint8_t poll_pkt[TC_POLL_LEN];
 239         size_t ch_idx;
 240         const struct rdtech_tc_channel_desc *pch;
 241         int ret;
 242         float v;
 243
 244         devc = sdi->priv;
 245         sr_spew("Received poll packet (len: %zu).", devc->rdlen);
 246         if (devc->rdlen < TC_POLL_LEN) {
 247                 sr_err("Insufficient poll packet length: %zu", devc->rdlen);
 248                 return SR_ERR_DATA;
 249         }
 250
 251         if (process_poll_pkt(devc, poll_pkt) != SR_OK) {
 252                 sr_err("Failed to process poll packet.");
 253                 return SR_ERR_DATA;
 254         }
 255
 256         ret = SR_OK;
 257         std_session_send_df_frame_begin(sdi);
 258         for (ch_idx = 0; ch_idx < devc->channel_count; ch_idx++) {
 259                 pch = &devc->channels[ch_idx];
 260                 ret = bv_get_value(&v, &pch->spec, poll_pkt, TC_POLL_LEN);
 261                 if (ret != SR_OK)
 262                         break;
 263                 ret = feed_queue_analog_submit(devc->feeds[ch_idx], v, 1);
 264                 if (ret != SR_OK)
 265                         break;
 266         }
 267         std_session_send_df_frame_end(sdi);
 268
 269         sr_sw_limits_update_frames_read(&devc->limits, 1);
 270         if (sr_sw_limits_check(&devc->limits))
 271                 sr_dev_acquisition_stop(sdi);
 272
 273         return ret;
 274 }
 275
 276 static int recv_poll_data(struct sr_dev_inst *sdi, struct sr_serial_dev_inst *serial)
 277 {
 278         struct dev_context *devc;
 279         size_t space;
 280         int len;
 281         int ret;
 282
 283         /* Receive data became available. Drain the transport layer. */
 284         devc = sdi->priv;
 285         while (devc->rdlen < TC_POLL_LEN) {
 286                 space = sizeof(devc->buf) - devc->rdlen;
 287                 len = serial_read_nonblocking(serial,
 288                         &devc->buf[devc->rdlen], space);
 289                 if (len < 0)
 290                         return SR_ERR_IO;
 291                 if (len == 0)
 292                         return SR_OK;
 293                 devc->rdlen += len;
 294         }
 295
 296         /*
 297          * TODO Want to (re-)synchronize to the packet stream? The
 298          * 'pac1' string literal would be a perfect match for that.
 299          */
 300
 301         /* Process packets when their reception has completed. */
 302         while (devc->rdlen >= TC_POLL_LEN) {
 303                 ret = handle_poll_data(sdi);
 304                 if (ret != SR_OK)
 305                         return ret;
 306                 devc->rdlen -= TC_POLL_LEN;
 307                 if (devc->rdlen)
 308                         memmove(devc->buf, &devc->buf[TC_POLL_LEN], devc->rdlen);
 309         }
 310
 311         return SR_OK;
 312 }
 313
 314 SR_PRIV int rdtech_tc_receive_data(int fd, int revents, void *cb_data)
 315 {
 316         struct sr_dev_inst *sdi;
 317         struct dev_context *devc;
 318         struct sr_serial_dev_inst *serial;
 319         int ret;
 320
 321         (void)fd;
 322
 323         if (!(sdi = cb_data))
 324                 return TRUE;
 325         if (!(devc = sdi->priv))
 326                 return TRUE;
 327
 328         /* Handle availability of receive data. */
 329         serial = sdi->conn;
 330         if (revents == G_IO_IN) {
 331                 ret = recv_poll_data(sdi, serial);
 332                 if (ret != SR_OK)
 333                         sr_dev_acquisition_stop(sdi);
 334         }
 335
 336         /* Check configured acquisition limits. */
 337         if (sr_sw_limits_check(&devc->limits)) {
 338                 sr_dev_acquisition_stop(sdi);
 339                 return TRUE;
 340         }
 341
 342         /* Periodically retransmit measurement requests. */
 343         (void)rdtech_tc_poll(sdi, FALSE);
 344
 345         return TRUE;
 346 }