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, }, { 100, 1e6, }, 4, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
  77         { "I",  {   0 + 52, BVT_LE_UINT32, }, {  10, 1e6, }, 5, SR_MQ_CURRENT, SR_UNIT_AMPERE },
  78         { "D+", {  64 + 32, BVT_LE_UINT32, }, {  10, 1e3, }, 2, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
  79         { "D-", {  64 + 36, BVT_LE_UINT32, }, {  10, 1e3, }, 2, SR_MQ_VOLTAGE, SR_UNIT_VOLT },
  80         { "E0", {  64 + 12, BVT_LE_UINT32, }, {   1, 1e3, }, 3, SR_MQ_ENERGY, SR_UNIT_WATT_HOUR },
  81         { "E1", {  64 + 20, BVT_LE_UINT32, }, {   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_calc;
  87         uint32_t crc_recv;
  88
  89         crc_calc = sr_crc16(SR_CRC16_DEFAULT_INIT, data, PAC_CRC_POS);
  90         crc_recv = read_u32le(&data[PAC_CRC_POS]);
  91         if (crc_calc != crc_recv) {
  92                 sr_spew("CRC error. Calculated: %0x" PRIx16 ", expected: %0x" PRIx32,
  93                         crc_calc, crc_recv);
  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          * Defer request transmission when a previous request has not yet
 206          * seen any response data at all (more probable to happen shortly
 207          * after connecting to the peripheral).
 208          */
 209         devc = sdi->priv;
 210         if (!force) {
 211                 if (devc->rdlen)
 212                         return SR_OK;
 213                 if (!devc->rx_after_tx)
 214                         return SR_OK;
 215         }
 216
 217         /*
 218          * Send the request when the transmit interval was reached. Or
 219          * when the caller forced the transmission.
 220          */
 221         now = g_get_monotonic_time() / 1000;
 222         elapsed = now - devc->cmd_sent_at;
 223         if (!force && elapsed < POLL_PERIOD_MS)
 224                 return SR_OK;
 225
 226         /*
 227          * Transmit another measurement request. Only advance the
 228          * interval after successful transmission.
 229          */
 230         serial = sdi->conn;
 231         len = serial_write_blocking(serial,
 232                 devc->req_text, strlen(devc->req_text), WRITE_TO_MS);
 233         if (len < 0) {
 234                 sr_err("Unable to send poll request.");
 235                 return SR_ERR;
 236         }
 237         devc->cmd_sent_at = now;
 238         devc->rx_after_tx = 0;
 239
 240         return SR_OK;
 241 }
 242
 243 static int handle_poll_data(struct sr_dev_inst *sdi)
 244 {
 245         struct dev_context *devc;
 246         uint8_t poll_pkt[TC_POLL_LEN];
 247         size_t ch_idx;
 248         const struct rdtech_tc_channel_desc *pch;
 249         int ret;
 250         float v;
 251
 252         devc = sdi->priv;
 253         sr_spew("Received poll packet (len: %zu).", devc->rdlen);
 254         if (devc->rdlen < TC_POLL_LEN) {
 255                 sr_err("Insufficient poll packet length: %zu", devc->rdlen);
 256                 return SR_ERR_DATA;
 257         }
 258
 259         if (process_poll_pkt(devc, poll_pkt) != SR_OK) {
 260                 sr_err("Failed to process poll packet.");
 261                 return SR_ERR_DATA;
 262         }
 263
 264         ret = SR_OK;
 265         std_session_send_df_frame_begin(sdi);
 266         for (ch_idx = 0; ch_idx < devc->channel_count; ch_idx++) {
 267                 pch = &devc->channels[ch_idx];
 268                 ret = bv_get_value_len(&v, &pch->spec, poll_pkt, TC_POLL_LEN);
 269                 if (ret != SR_OK)
 270                         break;
 271                 ret = feed_queue_analog_submit_one(devc->feeds[ch_idx], v, 1);
 272                 if (ret != SR_OK)
 273                         break;
 274         }
 275         std_session_send_df_frame_end(sdi);
 276
 277         sr_sw_limits_update_frames_read(&devc->limits, 1);
 278         if (sr_sw_limits_check(&devc->limits))
 279                 sr_dev_acquisition_stop(sdi);
 280
 281         return ret;
 282 }
 283
 284 static int recv_poll_data(struct sr_dev_inst *sdi, struct sr_serial_dev_inst *serial)
 285 {
 286         struct dev_context *devc;
 287         size_t space;
 288         int len;
 289         int ret;
 290
 291         /* Receive data became available. Drain the transport layer. */
 292         devc = sdi->priv;
 293         while (devc->rdlen < TC_POLL_LEN) {
 294                 space = sizeof(devc->buf) - devc->rdlen;
 295                 len = serial_read_nonblocking(serial,
 296                         &devc->buf[devc->rdlen], space);
 297                 if (len < 0)
 298                         return SR_ERR_IO;
 299                 if (len == 0)
 300                         return SR_OK;
 301                 devc->rdlen += len;
 302                 devc->rx_after_tx += len;
 303         }
 304
 305         /*
 306          * TODO Want to (re-)synchronize to the packet stream? The
 307          * 'pac1' string literal would be a perfect match for that.
 308          */
 309
 310         /* Process packets when their reception has completed. */
 311         while (devc->rdlen >= TC_POLL_LEN) {
 312                 ret = handle_poll_data(sdi);
 313                 if (ret != SR_OK)
 314                         return ret;
 315                 devc->rdlen -= TC_POLL_LEN;
 316                 if (devc->rdlen)
 317                         memmove(devc->buf, &devc->buf[TC_POLL_LEN], devc->rdlen);
 318         }
 319
 320         return SR_OK;
 321 }
 322
 323 SR_PRIV int rdtech_tc_receive_data(int fd, int revents, void *cb_data)
 324 {
 325         struct sr_dev_inst *sdi;
 326         struct dev_context *devc;
 327         struct sr_serial_dev_inst *serial;
 328         int ret;
 329
 330         (void)fd;
 331
 332         if (!(sdi = cb_data))
 333                 return TRUE;
 334         if (!(devc = sdi->priv))
 335                 return TRUE;
 336
 337         /* Handle availability of receive data. */
 338         serial = sdi->conn;
 339         if (revents == G_IO_IN) {
 340                 ret = recv_poll_data(sdi, serial);
 341                 if (ret != SR_OK)
 342                         sr_dev_acquisition_stop(sdi);
 343         }
 344
 345         /* Check configured acquisition limits. */
 346         if (sr_sw_limits_check(&devc->limits)) {
 347                 sr_dev_acquisition_stop(sdi);
 348                 return TRUE;
 349         }
 350
 351         /* Periodically retransmit measurement requests. */
 352         (void)rdtech_tc_poll(sdi, FALSE);
 353
 354         return TRUE;
 355 }