src/hardware/mooshimeter-dmm/protocol.c

   1 /*
   2  * This file is part of the libsigrok project.
   3  *
   4  * Copyright (C) 2019 Derek Hageman <hageman@inthat.cloud>
   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 <gio/gio.h>
  22 #include <math.h>
  23 #include <string.h>
  24 #include "protocol.h"
  25
  26 /*
  27  * The Mooshimeter protocol is broken down into several layers in a
  28  * communication stack.
  29  *
  30  * The lowest layer is the BLE GATT stack, which provides two characteristics:
  31  * one to write packets to the meter and one to receive them from it. The
  32  * MTU for a packet in either direction is 20 bytes. This is implemented
  33  * in the GATT abstraction, so we can talk to it via simple write commands
  34  * and a read callback.
  35  *
  36  *
  37  * The next layer is the serial stream: each BLE packet in either direction
  38  * has a 1-byte header of a sequence number. Despite what the documentation
  39  * says, this is present in both directions (not just meter output) and is
  40  * NOT reset on the meter output on BLE connection. So the implementation
  41  * here needs to provide an output sequence number and incoming reassembly
  42  * for out of order packets (I haven't actually observed this, but
  43  * supposedly it happens, which is why the sequence number is present).
  44  * So the structure of packets received looks like:
  45  *
  46  * | 1 byte | 1-19 bytes  |
  47  * |--------|-------------|
  48  * | SeqNum | Serial Data |
  49  *
  50  *
  51  * On top of the serial layer is the "config tree" layer. This is how
  52  * the meter actually exposes data and configuration. The tree itself
  53  * is composed of nodes, each with a string name, data type, and a list
  54  * of children (zero or more). For value containing (non-informational)
  55  * nodes, they also contain a 7-bit unique identifier. Access to the
  56  * config tree is provided by packets on the serial stream, each packet
  57  * has a 1-byte header, where the uppermost bit (0x80) is set when writing
  58  * (i.e. never by the meter) and the remaining 7 bits are the node identifier.
  59  * The length of the packets varies based on the datatype of the tree node.
  60  * This means that any lost/dropped packets can make the stream unrecoverable
  61  * (i.e. there's no defined sync method other than reconnection). Packets
  62  * are emitted by the meter in response to a read or write command (write
  63  * commands simply back the value) and at unsolicited times by the meter
  64  * (e.g. continuous sampling and periodic battery voltage). A read packet
  65  * send to the meter looks like:
  66  *
  67  * | 1 bit | 7 bits |
  68  * |-------|--------|
  69  * |   0   | NodeID |
  70  *
  71  * In response to the read, the meter will send:
  72  *
  73  * | 1 bit | 7 bits | 1-N bytes |
  74  * |-------|--------|-----------|
  75  * |   0   | NodeID | NodeValue |
  76  *
  77  * A write packet sent to the meter:
  78  *
  79  * | 1 bit | 7 bits | 1-N bytes |
  80  * |-------|--------|-----------|
  81  * |   1   | NodeID | NodeValue |
  82  *
  83  * In response to the write, the meter will send a read response:
  84  *
  85  * | 1 bit | 7 bits | 1-N bytes |
  86  * |-------|--------|-----------|
  87  * |   0   | NodeID | NodeValue |
  88  *
  89  *
  90  * For the data in the tree, all values are little endian (least significant
  91  * bytes first). The supported type codes are:
  92  *
  93  * | Code | Description | Wire Format                            |
  94  * |------|-------------|----------------------------------------|
  95  * |  0   | Plain       |                                        |
  96  * |  1   | Link        |                                        |
  97  * |  2   | Chooser     | uint8_t                                |
  98  * |  3   | U8          | uint8_t                                |
  99  * |  4   | U16         | uint16_t                               |
 100  * |  5   | U32         | uint32_t                               |
 101  * |  6   | S8          | int8_t                                 |
 102  * |  7   | S16         | int16_t                                |
 103  * |  8   | S32         | int32_t                                |
 104  * |  9   | String      | uint16_t length; char value[length]    |
 105  * |  10  | Binary      | uint16_t length; uint8_t value[length] |
 106  * |  11  | Float       | float                                  |
 107  *
 108  * Plain and Link nodes are present to provide information and/or choices
 109  * but do not provide commands codes for direct access (see serialization
 110  * below). Chooser nodes are written with indices described by their Plain
 111  * type children (e.g. to select a choice identified by the second child
 112  * of a chooser, write 1 to the chooser node itself).
 113  *
 114  * On initial connection only three nodes at fixed identifiers are available:
 115  *
 116  * | Node             | ID | Type   |
 117  * |------------------|----|--------|
 118  * | ADMIN:CRC32      | 0  | U32    |
 119  * | ADMIN:TREE       | 1  | Binary |
 120  * | ADMIN:DIAGNOSTIC | 2  | String |
 121  *
 122  *
 123  * The handshake sequence is to read the contents of ADMIN:TREE, which contains
 124  * the zlib compressed tree serialization, then write the CRC of the compressed
 125  * data back to ADMIN:CRC32 (which the meter will echo back). Only after
 126  * that is done will the meter accept access to the rest of the tree.
 127  *
 128  * After zlib decompression the tree serialization is as follows:
 129  *
 130  * | Type         | Description                         |
 131  * |--------------|-------------------------------------|
 132  * | uint8_t      | The node data type code from above  |
 133  * | uint8_t      | Name length                         |
 134  * | char[length] | Node name (e.g. "ADMIN" or "CRC32") |
 135  * | uint8_t      | Number of children                  |
 136  * | Node[count]  | Child serialization (length varies) |
 137  *
 138  * Once the tree has been deserialized, each node needs its identifier
 139  * assigned. This is a depth first tree walk, assigning sequential identifiers
 140  * first the the current node (if it needs one), then repeating recursively
 141  * for each of its children. Plain and Link nodes are skipped in assignment
 142  * but not the walk (so the recursion still happens, but the identifier
 143  * is not incremented).
 144  *
 145  *
 146  * So, for example a write to the ADMIN:CRC32 as part of the handshake would
 147  * be a write by us (the host):
 148  *
 149  * | SerSeq | NodeID | U32 (CRC)  |
 150  * | 1 byte | 1 byte |   4 bytes  |
 151  * ---------|--------|------------|
 152  * |  0x01  |  0x80  | 0xDEADBEEF |
 153  *
 154  * The meter will respond with a packet like:
 155  *
 156  * | SerSeq | NodeID | U32 (CRC)  |
 157  * | 1 byte | 1 byte |   4 bytes  |
 158  * ---------|--------|------------|
 159  * |  0x42  |  0x00  | 0xDEADBEEF |
 160  *
 161  * A spontaneous error from the meter (e.g. in response to a bad packet)
 162  * can be emitted like:
 163  *
 164  * | SerSeq | NodeID | U16 (len)  |      String      |
 165  * | 1 byte | 1 byte |   2 bytes  |  len (=8) bytes  |
 166  * ---------|--------|------------|------------------|
 167  * |  0xAB  |  0x20  |   0x0008   |    BAD\x20DATA   |
 168  *
 169  *
 170  * The config tree at the time of writing looks like:
 171  *
 172  *  <ROOT> (PLAIN)
 173  *      ADMIN (PLAIN)
 174  *              CRC32 (U32) = 0
 175  *              TREE (BIN) = 1
 176  *              DIAGNOSTIC (STR) = 2
 177  *      PCB_VERSION (U8) = 3
 178  *      NAME (STR) = 4
 179  *      TIME_UTC (U32) = 5
 180  *      TIME_UTC_MS (U16) = 6
 181  *      BAT_V (FLT) = 7
 182  *      REBOOT (CHOOSER) = 8
 183  *              NORMAL (PLAIN)
 184  *              SHIPMODE (PLAIN)
 185  *      SAMPLING (PLAIN)
 186  *              RATE (CHOOSER) = 9
 187  *                      125 (PLAIN)
 188  *                      250 (PLAIN)
 189  *                      500 (PLAIN)
 190  *                      1000 (PLAIN)
 191  *                      2000 (PLAIN)
 192  *                      4000 (PLAIN)
 193  *                      8000 (PLAIN)
 194  *              DEPTH (CHOOSER) = 10
 195  *                      32 (PLAIN)
 196  *                      64 (PLAIN)
 197  *                      128 (PLAIN)
 198  *                      256 (PLAIN)
 199  *              TRIGGER (CHOOSER) = 11
 200  *                      OFF (PLAIN)
 201  *                      SINGLE (PLAIN)
 202  *                      CONTINUOUS (PLAIN)
 203  *      LOG (PLAIN)
 204  *              ON (U8) = 12
 205  *              INTERVAL (U16) = 13
 206  *              STATUS (U8) = 14
 207  *              POLLDIR (U8) = 15
 208  *              INFO (PLAIN)
 209  *                      INDEX (U16) = 16
 210  *                      END_TIME (U32) = 17
 211  *                      N_BYTES (U32) = 18
 212  *              STREAM (PLAIN)
 213  *                      INDEX (U16) = 19
 214  *                      OFFSET (U32) = 20
 215  *                      DATA (BIN) = 21
 216  *      CH1 (PLAIN)
 217  *              MAPPING (CHOOSER) = 22
 218  *                      CURRENT (PLAIN)
 219  *                              10 (PLAIN)
 220  *                      TEMP (PLAIN)
 221  *                              350 (PLAIN)
 222  *                      SHARED (LINK)
 223  *              RANGE_I (U8) = 23
 224  *              ANALYSIS (CHOOSER) = 24
 225  *                      MEAN (PLAIN)
 226  *                      RMS (PLAIN)
 227  *                      BUFFER (PLAIN)
 228  *              VALUE (FLT) = 25
 229  *              OFFSET (FLT) = 26
 230  *              BUF (BIN) = 27
 231  *              BUF_BPS (U8) = 28
 232  *              BUF_LSB2NATIVE (FLT) = 29
 233  *      CH2 (PLAIN)
 234  *              MAPPING (CHOOSER) = 30
 235  *                      VOLTAGE (PLAIN)
 236  *                              60 (PLAIN)
 237  *                              600 (PLAIN)
 238  *                      TEMP (PLAIN)
 239  *                              350 (PLAIN)
 240  *                      SHARED (LINK)
 241  *              RANGE_I (U8) = 31
 242  *              ANALYSIS (CHOOSER) = 32
 243  *                      MEAN (PLAIN)
 244  *                      RMS (PLAIN)
 245  *                      BUFFER (PLAIN)
 246  *              VALUE (FLT) = 33
 247  *              OFFSET (FLT) = 34
 248  *              BUF (BIN) = 35
 249  *              BUF_BPS (U8) = 36
 250  *              BUF_LSB2NATIVE (FLT) = 37
 251  *      SHARED (CHOOSER) = 38
 252  *              AUX_V (PLAIN)
 253  *                      0.1 (PLAIN)
 254  *                      0.3 (PLAIN)
 255  *                      1.2 (PLAIN)
 256  *              RESISTANCE (PLAIN)
 257  *                      1000.0 (PLAIN)
 258  *                      10000.0 (PLAIN)
 259  *                      100000.0 (PLAIN)
 260  *                      1000000.0 (PLAIN)
 261  *                      10000000.0 (PLAIN)
 262  *              DIODE (PLAIN)
 263  *                      1.2 (PLAIN)
 264  *      REAL_PWR (FLT) = 39
 265  */
 266
 267 static struct config_tree_node *lookup_tree_path(struct dev_context *devc,
 268         const char *path)
 269 {
 270         struct config_tree_node *current = &devc->tree_root;
 271         struct config_tree_node *next;
 272         const char *end;
 273         size_t length;
 274
 275         for (;;) {
 276                 end = strchr(path, ':');
 277                 if (!end)
 278                         length = strlen(path);
 279                 else
 280                         length = end - path;
 281
 282                 next = NULL;
 283                 for (size_t i = 0; i < current->count_children; i++) {
 284                         if (!current->children[i].name)
 285                                 continue;
 286                         if (strlen(current->children[i].name) != length)
 287                                 continue;
 288                         if (g_ascii_strncasecmp(path,
 289                                 current->children[i].name,
 290                                 length)) {
 291                                 continue;
 292                         }
 293
 294                         next = &current->children[i];
 295                 }
 296                 if (!next)
 297                         return NULL;
 298                 if (!end)
 299                         return next;
 300
 301                 path = end + 1;
 302                 current = next;
 303         }
 304 }
 305
 306 static int lookup_chooser_index(struct dev_context *devc, const char *path)
 307 {
 308         struct config_tree_node *node;
 309
 310         node = lookup_tree_path(devc, path);
 311         if (!node)
 312                 return -1;
 313
 314         return (int)node->index_in_parent;
 315 }
 316
 317 static gboolean update_tree_data(struct config_tree_node *node,
 318         GByteArray *contents)
 319 {
 320         guint len;
 321         switch (node->type) {
 322         case TREE_NODE_DATATYPE_PLAIN:
 323         case TREE_NODE_DATATYPE_LINK:
 324                 sr_err("Update for dataless node.");
 325                 g_byte_array_remove_range(contents, 0, 2);
 326                 return TRUE;
 327         case TREE_NODE_DATATYPE_CHOOSER:
 328         case TREE_NODE_DATATYPE_U8:
 329                 node->value.i = R8(contents->data + 1);
 330                 g_byte_array_remove_range(contents, 0, 2);
 331                 break;
 332         case TREE_NODE_DATATYPE_U16:
 333                 if (contents->len < 3)
 334                         return FALSE;
 335                 node->value.i = RL16(contents->data + 1);
 336                 g_byte_array_remove_range(contents, 0, 3);
 337                 break;
 338         case TREE_NODE_DATATYPE_U32:
 339                 if (contents->len < 5)
 340                         return FALSE;
 341                 node->value.i = RL32(contents->data + 1);
 342                 g_byte_array_remove_range(contents, 0, 5);
 343                 break;
 344         case TREE_NODE_DATATYPE_S8:
 345                 node->value.i = (int8_t)R8(contents->data + 1);
 346                 g_byte_array_remove_range(contents, 0, 2);
 347                 break;
 348         case TREE_NODE_DATATYPE_S16:
 349                 if (contents->len < 3)
 350                         return FALSE;
 351                 node->value.i = RL16S(contents->data + 1);
 352                 g_byte_array_remove_range(contents, 0, 3);
 353                 break;
 354         case TREE_NODE_DATATYPE_S32:
 355                 if (contents->len < 5)
 356                         return FALSE;
 357                 node->value.i = RL32S(contents->data + 1);
 358                 g_byte_array_remove_range(contents, 0, 5);
 359                 break;
 360         case TREE_NODE_DATATYPE_STRING:
 361         case TREE_NODE_DATATYPE_BINARY:
 362                 if (contents->len < 3)
 363                         return FALSE;
 364                 len = RL16(contents->data + 1);
 365                 if (contents->len < 3 + len)
 366                         return FALSE;
 367                 g_byte_array_set_size(node->value.b, len);
 368                 memcpy(node->value.b->data, contents->data + 3, len);
 369                 g_byte_array_remove_range(contents, 0, 3 + len);
 370                 break;
 371         case TREE_NODE_DATATYPE_FLOAT:
 372                 if (contents->len < 5)
 373                         return FALSE;
 374                 node->value.f = RLFL(contents->data + 1);
 375                 g_byte_array_remove_range(contents, 0, 5);
 376                 break;
 377         }
 378
 379         node->update_number++;
 380
 381         if (node->on_update)
 382                 (*node->on_update)(node, node->on_update_param);
 383
 384         return TRUE;
 385 }
 386
 387 static gboolean incoming_frame(struct packet_rx *rx,
 388         const void *data, guint count)
 389 {
 390         const guint8 *bytes = data;
 391         int seq, ahead;
 392         GByteArray *ba;
 393         GSList *target = NULL;
 394
 395         if (!count)
 396                 return FALSE;
 397
 398         seq = bytes[0];
 399         if (rx->sequence_number < 0) {
 400                 rx->sequence_number = (seq + 1) & 0xFF;
 401                 g_byte_array_append(rx->contents, bytes + 1, count - 1);
 402                 return TRUE;
 403         } else if (rx->sequence_number == seq) {
 404                 rx->sequence_number = (seq + 1) & 0xFF;
 405                 g_byte_array_append(rx->contents, data + 1, count - 1);
 406
 407                 while (rx->reorder_buffer && rx->reorder_buffer->data) {
 408                         rx->sequence_number = (rx->sequence_number + 1) & 0xFF;
 409
 410                         ba = rx->reorder_buffer->data;
 411                         g_byte_array_append(rx->contents, ba->data, ba->len);
 412                         g_byte_array_free(ba, TRUE);
 413                         target = rx->reorder_buffer;
 414                         rx->reorder_buffer = rx->reorder_buffer->next;
 415                         g_slist_free_1(target);
 416                 }
 417                 return TRUE;
 418         } else {
 419                 ahead = seq - rx->sequence_number;
 420                 if (ahead < 0)
 421                         ahead += 256;
 422                 if (!rx->reorder_buffer)
 423                         rx->reorder_buffer = g_slist_alloc();
 424                 target = rx->reorder_buffer;
 425                 for (--ahead; ahead > 0; --ahead) {
 426                         if (!target->next)
 427                                 target->next = g_slist_alloc();
 428                         target = target->next;
 429                 }
 430                 if (target->data)
 431                         g_byte_array_free(target->data, TRUE);
 432                 target->data = g_byte_array_sized_new(count);
 433                 g_byte_array_append(target->data, data + 1, count - 1);
 434                 return TRUE;
 435         }
 436 }
 437
 438 static void consume_packets(struct dev_context *devc)
 439 {
 440         uint8_t id;
 441         struct config_tree_node *target;
 442
 443         if (devc->rx.contents->len < 2)
 444                 return;
 445
 446         id = devc->rx.contents->data[0];
 447         id &= 0x7F;
 448         target = devc->tree_id_lookup[id];
 449
 450         if (!target) {
 451                 sr_err("Command %hhu code does not map to a known node.", id);
 452                 g_byte_array_remove_index(devc->rx.contents, 0);
 453                 return consume_packets(devc);
 454         }
 455
 456         if (!update_tree_data(target, devc->rx.contents))
 457                 return;
 458
 459         return consume_packets(devc);
 460 }
 461
 462 static int notify_cb(void *cb_data, uint8_t *data, size_t dlen)
 463 {
 464         const struct sr_dev_inst *sdi = cb_data;
 465         struct dev_context *devc = sdi->priv;
 466
 467         if (!incoming_frame(&devc->rx, data, (guint)dlen))
 468                 return -1;
 469
 470         consume_packets(devc);
 471
 472         return 0;
 473 }
 474
 475 static int write_frame(const struct sr_dev_inst *sdi,
 476         const void *frame, size_t length)
 477 {
 478         struct sr_bt_desc *desc = sdi->conn;
 479
 480         if (sr_bt_write(desc, frame, length) != (ssize_t)length)
 481                 return SR_ERR;
 482
 483         return SR_OK;
 484 }
 485
 486 static int poll_tree_value(const struct sr_dev_inst *sdi,
 487         struct config_tree_node *node)
 488 {
 489         struct dev_context *devc = sdi->priv;
 490
 491         uint8_t frame[2];
 492         W8(&frame[0], devc->tx.sequence_number);
 493         W8(&frame[1], node->id);
 494
 495         devc->tx.sequence_number = (devc->tx.sequence_number + 1) & 0xFF;
 496
 497         return write_frame(sdi, frame, 2);
 498 }
 499
 500 static void set_tree_integer(const struct sr_dev_inst *sdi,
 501         struct config_tree_node *node, int32_t value)
 502 {
 503         struct dev_context *devc = sdi->priv;
 504         uint8_t frame[20];
 505         size_t length;
 506
 507         W8(&frame[0], devc->tx.sequence_number);
 508         W8(&frame[1], 0x80 | node->id);
 509
 510         length = 2;
 511
 512         switch (node->type) {
 513         case TREE_NODE_DATATYPE_PLAIN:
 514         case TREE_NODE_DATATYPE_LINK:
 515                 sr_err("Set attempted for dataless node.");
 516                 return;
 517         case TREE_NODE_DATATYPE_CHOOSER:
 518         case TREE_NODE_DATATYPE_U8:
 519                 node->value.i = value;
 520                 W8(&frame[length], value);
 521                 length += 1;
 522                 break;
 523         case TREE_NODE_DATATYPE_U16:
 524                 node->value.i = value;
 525                 WL16(&frame[length], value);
 526                 length += 2;
 527                 break;
 528         case TREE_NODE_DATATYPE_U32:
 529                 node->value.i = value;
 530                 WL32(&frame[length], value);
 531                 length += 4;
 532                 break;
 533         case TREE_NODE_DATATYPE_S8:
 534                 node->value.i = value;
 535                 W8(&frame[length], value);
 536                 length += 1;
 537                 break;
 538         case TREE_NODE_DATATYPE_S16:
 539                 node->value.i = value;
 540                 WL16(&frame[length], value);
 541                 length += 2;
 542                 break;
 543         case TREE_NODE_DATATYPE_S32:
 544                 node->value.i = value;
 545                 WL32(&frame[length], value);
 546                 length += 4;
 547                 break;
 548         case TREE_NODE_DATATYPE_STRING:
 549         case TREE_NODE_DATATYPE_BINARY:
 550         case TREE_NODE_DATATYPE_FLOAT:
 551                 return;
 552         }
 553
 554         devc->tx.sequence_number = (devc->tx.sequence_number + 1) & 0xFF;
 555         write_frame(sdi, frame, length);
 556 }
 557
 558 static int32_t get_tree_integer(struct config_tree_node *node)
 559 {
 560         switch (node->type) {
 561         case TREE_NODE_DATATYPE_PLAIN:
 562         case TREE_NODE_DATATYPE_LINK:
 563                 sr_err("Read attempted for dataless node.");
 564                 return 0;
 565         case TREE_NODE_DATATYPE_CHOOSER:
 566         case TREE_NODE_DATATYPE_U8:
 567         case TREE_NODE_DATATYPE_U16:
 568         case TREE_NODE_DATATYPE_U32:
 569         case TREE_NODE_DATATYPE_S8:
 570         case TREE_NODE_DATATYPE_S16:
 571         case TREE_NODE_DATATYPE_S32:
 572                 return node->value.i;
 573         case TREE_NODE_DATATYPE_FLOAT:
 574                 return (int)node->value.f;
 575         default:
 576                 break;
 577         }
 578
 579         return 0;
 580 }
 581
 582 static void tree_diagnostic_updated(struct config_tree_node *node, void *param)
 583 {
 584         (void)param;
 585
 586         if (!node->value.b->len) {
 587                 sr_warn("Mooshimeter error with no information.");
 588                 return;
 589         }
 590
 591         if (node->value.b->data[node->value.b->len]) {
 592                 g_byte_array_set_size(node->value.b, node->value.b->len + 1);
 593                 node->value.b->data[node->value.b->len - 1] = 0;
 594         }
 595
 596         sr_warn("Mooshimeter error: %s.", node->value.b->data);
 597 }
 598
 599 static void chX_value_update(struct config_tree_node *node,
 600         struct sr_dev_inst *sdi, int channel)
 601 {
 602         struct dev_context *devc = sdi->priv;
 603         float value;
 604         struct sr_datafeed_packet packet;
 605         struct sr_datafeed_analog analog;
 606         struct sr_analog_encoding encoding;
 607         struct sr_analog_meaning meaning;
 608         struct sr_analog_spec spec;
 609
 610         if (!devc->enable_value_stream)
 611                 return;
 612
 613         if (!((struct sr_channel *)devc->channel_meaning[channel].
 614                 channels->data)->enabled) {
 615                 return;
 616         }
 617
 618         if (node->type != TREE_NODE_DATATYPE_FLOAT)
 619                 return;
 620         value = node->value.f;
 621
 622         sr_spew("Received value for channel %d = %g.", channel, value);
 623
 624         /*
 625          * Could do significant digit calculations based on the
 626          * effective number of effective bits (sample rate, buffer size, etc),
 627          * but does it matter?
 628          * (see https://github.com/mooshim/Mooshimeter-AndroidApp/blob/94a20a2d42f6af9975ad48591caa6a17130ca53b/app/src/main/java/com/mooshim/mooshimeter/devices/MooshimeterDevice.java#L691 )
 629          */
 630         sr_analog_init(&analog, &encoding, &meaning, &spec, 2);
 631
 632         memcpy(analog.meaning, &devc->channel_meaning[channel],
 633                 sizeof(struct sr_analog_meaning));
 634         analog.num_samples = 1;
 635         analog.data = &value;
 636         packet.type = SR_DF_ANALOG;
 637         packet.payload = &analog;
 638         sr_session_send(sdi, &packet);
 639
 640         if (devc->channel_autorange[channel])
 641                 (*devc->channel_autorange[channel])(sdi, value);
 642
 643         sr_sw_limits_update_samples_read(&devc->limits, 1);
 644         if (sr_sw_limits_check(&devc->limits))
 645                 sr_dev_acquisition_stop(sdi);
 646 }
 647
 648 static void chX_buffer_update(struct config_tree_node *node,
 649         struct sr_dev_inst *sdi, int channel)
 650 {
 651         struct dev_context *devc = sdi->priv;
 652         uint32_t bits_per_sample = devc->buffer_bps[channel];
 653         float output_scalar = devc->buffer_lsb2native[channel];
 654         uint32_t bytes_per_sample;
 655         const uint8_t *raw;
 656         size_t size;
 657         size_t number_of_samples;
 658         int32_t unscaled = 0;
 659         int32_t sign_bit;
 660         int32_t sign_mask;
 661         float converted_value = 0;
 662         float maximum_value = 0;
 663         float *values;
 664         float *output_value;
 665         struct sr_datafeed_packet packet;
 666         struct sr_datafeed_analog analog;
 667         struct sr_analog_encoding encoding;
 668         struct sr_analog_meaning meaning;
 669         struct sr_analog_spec spec;
 670
 671         if (!devc->enable_value_stream)
 672                 return;
 673
 674         if (!((struct sr_channel *)devc->channel_meaning[channel].
 675                 channels->data)->enabled) {
 676                 return;
 677         }
 678
 679         if (!bits_per_sample)
 680                 return;
 681         if (node->type != TREE_NODE_DATATYPE_BINARY)
 682                 return;
 683         raw = node->value.b->data;
 684         size = node->value.b->len;
 685         if (!size)
 686                 return;
 687
 688         bytes_per_sample = bits_per_sample / 8;
 689         if (bits_per_sample % 8 != 0)
 690                 bytes_per_sample++;
 691         if (bytes_per_sample > 4)
 692                 return;
 693         number_of_samples = size / bytes_per_sample;
 694         if (!number_of_samples)
 695                 return;
 696
 697         sr_analog_init(&analog, &encoding, &meaning, &spec, 0);
 698
 699         values = g_new0(float, number_of_samples);
 700         output_value = values;
 701
 702         memcpy(analog.meaning, &devc->channel_meaning[channel],
 703                 sizeof(struct sr_analog_meaning));
 704         analog.num_samples = number_of_samples;
 705         analog.data = output_value;
 706         packet.type = SR_DF_ANALOG;
 707         packet.payload = &analog;
 708
 709         sr_spew("Received buffer for channel %d with %u bytes (%u samples).",
 710                 channel, (unsigned int)size, (unsigned int)number_of_samples);
 711
 712         sign_bit = 1 << (bits_per_sample - 1);
 713         sign_mask = sign_bit - 1;
 714         for (; size >= bytes_per_sample; size -= bytes_per_sample,
 715                 raw += bytes_per_sample, output_value++) {
 716                 switch (bytes_per_sample) {
 717                 case 1:
 718                         unscaled = R8(raw);
 719                         break;
 720                 case 2:
 721                         unscaled = RL16(raw);
 722                         break;
 723                 case 3:
 724                         unscaled = ((uint32_t)raw[0]) |
 725                                 (((uint32_t)raw[1]) << 8) |
 726                                 (((uint32_t)raw[2]) << 16);
 727                         break;
 728                 case 4:
 729                         unscaled = RL32(raw);
 730                         break;
 731                 default:
 732                         break;
 733                 }
 734
 735                 unscaled = (unscaled & sign_mask) - (unscaled & sign_bit);
 736                 converted_value = (float)unscaled * output_scalar;
 737                 *output_value = converted_value;
 738                 if (fabsf(converted_value) > maximum_value)
 739                         maximum_value = fabsf(maximum_value);
 740         }
 741
 742         sr_session_send(sdi, &packet);
 743
 744         g_free(values);
 745
 746         if (devc->channel_autorange[channel])
 747                 (*devc->channel_autorange[channel])(sdi, maximum_value);
 748
 749         sr_sw_limits_update_samples_read(&devc->limits, number_of_samples);
 750         if (sr_sw_limits_check(&devc->limits))
 751                 sr_dev_acquisition_stop(sdi);
 752 }
 753
 754 static void ch1_value_update(struct config_tree_node *node, void *param)
 755 {
 756         chX_value_update(node, param, 0);
 757 }
 758
 759 static void ch2_value_update(struct config_tree_node *node, void *param)
 760 {
 761         chX_value_update(node, param, 1);
 762 }
 763
 764 static void power_value_update(struct config_tree_node *node, void *param)
 765 {
 766         chX_value_update(node, param, 2);
 767 }
 768
 769 static void ch1_buffer_update(struct config_tree_node *node, void *param)
 770 {
 771         chX_buffer_update(node, param, 0);
 772 }
 773
 774 static void ch2_buffer_update(struct config_tree_node *node, void *param)
 775 {
 776         chX_buffer_update(node, param, 1);
 777 }
 778
 779 static void ch1_buffer_bps_update(struct config_tree_node *node, void *param)
 780 {
 781         const struct sr_dev_inst *sdi = param;
 782         struct dev_context *devc = sdi->priv;
 783         devc->buffer_bps[0] = (uint32_t)get_tree_integer(node);
 784 }
 785
 786 static void ch2_buffer_bps_update(struct config_tree_node *node, void *param)
 787 {
 788         const struct sr_dev_inst *sdi = param;
 789         struct dev_context *devc = sdi->priv;
 790         devc->buffer_bps[1] = (uint32_t)get_tree_integer(node);
 791 }
 792
 793 static void ch1_buffer_lsb2native_update(struct config_tree_node *node,
 794         void *param)
 795 {
 796         const struct sr_dev_inst *sdi = param;
 797         struct dev_context *devc = sdi->priv;
 798         if (node->type != TREE_NODE_DATATYPE_BINARY)
 799                 return;
 800         devc->buffer_lsb2native[0] = node->value.f;
 801 }
 802
 803 static void ch2_buffer_lsb2native_update(struct config_tree_node *node,
 804         void *param)
 805 {
 806         const struct sr_dev_inst *sdi = param;
 807         struct dev_context *devc = sdi->priv;
 808         if (node->type != TREE_NODE_DATATYPE_BINARY)
 809                 return;
 810         devc->buffer_lsb2native[1] = node->value.f;
 811 }
 812
 813 static void release_tree_node(struct config_tree_node *node)
 814 {
 815         g_free(node->name);
 816
 817         switch (node->type) {
 818         case TREE_NODE_DATATYPE_STRING:
 819         case TREE_NODE_DATATYPE_BINARY:
 820                 g_byte_array_free(node->value.b, TRUE);
 821                 break;
 822         default:
 823                 break;
 824         }
 825
 826         for (size_t i = 0; i < node->count_children; i++)
 827                 release_tree_node(node->children + i);
 828         g_free(node->children);
 829 }
 830
 831 static void allocate_startup_tree(struct dev_context *devc)
 832 {
 833         struct config_tree_node *node;
 834
 835         node = &devc->tree_root;
 836         node->name = g_strdup("ADMIN");
 837         node->type = TREE_NODE_DATATYPE_PLAIN;
 838         node->count_children = 3;
 839         node->children = g_new0(struct config_tree_node, node->count_children);
 840
 841         node = &devc->tree_root.children[0];
 842         node->name = g_strdup("CRC");
 843         node->type = TREE_NODE_DATATYPE_U32;
 844         node->id = 0;
 845         devc->tree_id_lookup[node->id] = node;
 846
 847         node = &devc->tree_root.children[1];
 848         node->name = g_strdup("TREE");
 849         node->type = TREE_NODE_DATATYPE_BINARY;
 850         node->value.b = g_byte_array_new();
 851         node->id = 1;
 852         devc->tree_id_lookup[node->id] = node;
 853
 854         node = &devc->tree_root.children[2];
 855         node->name = g_strdup("DIAGNOSTIC");
 856         node->type = TREE_NODE_DATATYPE_STRING;
 857         node->value.b = g_byte_array_new();
 858         node->id = 2;
 859         devc->tree_id_lookup[node->id] = node;
 860 }
 861
 862 static gboolean tree_node_has_id(struct config_tree_node *node)
 863 {
 864         switch (node->type) {
 865         case TREE_NODE_DATATYPE_PLAIN:
 866         case TREE_NODE_DATATYPE_LINK:
 867                 return FALSE;
 868         default:
 869                 break;
 870         }
 871
 872         return TRUE;
 873 }
 874
 875 static int deserialize_tree(struct dev_context *devc,
 876         struct config_tree_node *node,
 877         int *id, const uint8_t **data, size_t *size)
 878 {
 879         size_t n;
 880         int res;
 881
 882         if (*size < 2)
 883                 return SR_ERR_DATA;
 884
 885         n = R8(*data);
 886         *data += 1;
 887         *size -= 1;
 888         if (n > TREE_NODE_DATATYPE_FLOAT)
 889                 return SR_ERR_DATA;
 890         node->type = n;
 891
 892         switch (node->type) {
 893         case TREE_NODE_DATATYPE_STRING:
 894         case TREE_NODE_DATATYPE_BINARY:
 895                 node->value.b = g_byte_array_new();
 896                 break;
 897         default:
 898                 break;
 899         }
 900
 901         n = R8(*data);
 902         *data += 1;
 903         *size -= 1;
 904         if (n > *size)
 905                 return SR_ERR_DATA;
 906         node->name = g_strndup((const char *)(*data), n);
 907         *data += n;
 908         *size -= n;
 909
 910         if (!(*size))
 911                 return SR_ERR_DATA;
 912
 913         if (tree_node_has_id(node)) {
 914                 node->id = *id;
 915                 (*id)++;
 916                 devc->tree_id_lookup[node->id] = node;
 917         }
 918
 919         n = R8(*data);
 920         *data += 1;
 921         *size -= 1;
 922
 923         if (n) {
 924                 node->count_children = n;
 925                 node->children = g_new0(struct config_tree_node, n);
 926
 927                 for (size_t i = 0; i < n; i++) {
 928                         if ((res = deserialize_tree(devc,
 929                                 node->children + i, id,
 930                                 data, size)) != SR_OK) {
 931                                 return res;
 932                         }
 933                         node->children[i].index_in_parent = i;
 934                 }
 935         }
 936
 937         return SR_OK;
 938 }
 939
 940 static int wait_for_update(const struct sr_dev_inst *sdi,
 941         struct config_tree_node *node,
 942         uint32_t original_update_number)
 943 {
 944         struct sr_bt_desc *desc = sdi->conn;
 945         int ret;
 946         gint64 start_time;
 947
 948         start_time = g_get_monotonic_time();
 949         for (;;) {
 950                 ret = sr_bt_check_notify(desc);
 951                 if (ret < 0)
 952                         return SR_ERR;
 953
 954                 if (node->update_number != original_update_number)
 955                         return SR_OK;
 956
 957                 if (g_get_monotonic_time() - start_time > 5 * 1000 * 1000)
 958                         break;
 959
 960                 if (ret > 0)
 961                         continue;
 962
 963                 /* Nothing pollable, so just sleep a bit and try again. */
 964                 g_usleep(50 * 1000);
 965         }
 966
 967         return SR_ERR_TIMEOUT;
 968 }
 969
 970 static void install_update_handlers(struct sr_dev_inst *sdi)
 971 {
 972         struct dev_context *devc = sdi->priv;
 973         struct config_tree_node *target;
 974
 975         target = lookup_tree_path(devc, "CH1:VALUE");
 976         if (target) {
 977                 target->on_update = ch1_value_update;
 978                 target->on_update_param = sdi;
 979         } else {
 980                 sr_warn("No tree path for channel 1 values.");
 981         }
 982
 983         target = lookup_tree_path(devc, "CH1:BUF");
 984         if (target) {
 985                 target->on_update = ch1_buffer_update;
 986                 target->on_update_param = sdi;
 987         } else {
 988                 sr_warn("No tree path for channel 1 buffer.");
 989         }
 990
 991         target = lookup_tree_path(devc, "CH1:BUF_BPS");
 992         if (target) {
 993                 target->on_update = ch1_buffer_bps_update;
 994                 target->on_update_param = sdi;
 995         } else {
 996                 sr_warn("No tree path for channel 1 buffer BPS.");
 997         }
 998
 999         target = lookup_tree_path(devc, "CH1:BUF_LSB2NATIVE");
1000         if (target) {
1001                 target->on_update = ch1_buffer_lsb2native_update;
1002                 target->on_update_param = sdi;
1003         } else {
1004                 sr_warn("No tree path for channel 1 buffer conversion factor.");
1005         }
1006
1007         target = lookup_tree_path(devc, "CH2:VALUE");
1008         if (target) {
1009                 target->on_update = ch2_value_update;
1010                 target->on_update_param = sdi;
1011         } else {
1012                 sr_warn("No tree path for channel 2 values.");
1013         }
1014
1015         target = lookup_tree_path(devc, "CH2:BUF");
1016         if (target) {
1017                 target->on_update = ch2_buffer_update;
1018                 target->on_update_param = sdi;
1019         } else {
1020                 sr_warn("No tree path for channel 2 buffer.");
1021         }
1022
1023         target = lookup_tree_path(devc, "CH2:BUF_BPS");
1024         if (target) {
1025                 target->on_update = ch2_buffer_bps_update;
1026                 target->on_update_param = sdi;
1027         } else {
1028                 sr_warn("No tree path for channel 2 buffer BPS.");
1029         }
1030
1031         target = lookup_tree_path(devc, "CH2:BUF_LSB2NATIVE");
1032         if (target) {
1033                 target->on_update = ch2_buffer_lsb2native_update;
1034                 target->on_update_param = sdi;
1035         } else {
1036                 sr_warn("No tree path for channel 2 buffer conversion factor.");
1037         }
1038
1039         target = lookup_tree_path(devc, "REAL_PWR");
1040         if (target) {
1041                 target->on_update = power_value_update;
1042                 target->on_update_param = sdi;
1043         } else {
1044                 sr_warn("No tree path for real power.");
1045         }
1046 }
1047
1048 struct startup_context {
1049         struct sr_dev_inst *sdi;
1050         uint32_t crc;
1051         int result;
1052         gboolean running;
1053 };
1054
1055 static void startup_failed(struct startup_context *ctx, int err)
1056 {
1057         sr_dbg("Startup handshake failed: %s.", sr_strerror(err));
1058
1059         ctx->result = err;
1060         ctx->running = FALSE;
1061 }
1062
1063 static void startup_complete(struct startup_context *ctx)
1064 {
1065         sr_dbg("Startup handshake completed.");
1066
1067         install_update_handlers(ctx->sdi);
1068
1069         ctx->running = FALSE;
1070 }
1071
1072 static int startup_run(struct startup_context *ctx)
1073 {
1074         struct sr_bt_desc *desc = ctx->sdi->conn;
1075         int ret;
1076         gint64 start_time;
1077
1078         ctx->result = SR_OK;
1079         ctx->running = TRUE;
1080
1081         start_time = g_get_monotonic_time();
1082         for (;;) {
1083                 ret = sr_bt_check_notify(desc);
1084                 if (ret < 0)
1085                         return SR_ERR;
1086
1087                 if (!ctx->running)
1088                         return ctx->result;
1089
1090                 if (g_get_monotonic_time() - start_time > 30 * 1000 * 1000)
1091                         break;
1092
1093                 if (ret > 0)
1094                         continue;
1095
1096                 /* Nothing pollable, so just sleep a bit and try again. */
1097                 g_usleep(50 * 1000);
1098         }
1099
1100         return SR_ERR_TIMEOUT;
1101 }
1102
1103 static void startup_tree_crc_updated(struct config_tree_node *node, void *param)
1104 {
1105         struct startup_context *ctx = param;
1106         uint32_t result;
1107
1108         node->on_update = NULL;
1109
1110         result = (uint32_t)get_tree_integer(node);
1111         if (result != ctx->crc) {
1112                 sr_err("Tree CRC mismatch, expected %08X but received %08X.",
1113                         ctx->crc, result);
1114                 startup_failed(ctx, SR_ERR_DATA);
1115                 return;
1116         }
1117
1118         startup_complete(ctx);
1119 }
1120
1121 static void startup_send_tree_crc(struct startup_context *ctx)
1122 {
1123         struct dev_context *devc = ctx->sdi->priv;
1124         struct config_tree_node *target;
1125
1126         if (!(target = lookup_tree_path(devc, "ADMIN:CRC32"))) {
1127                 sr_err("ADMIN:CRC32 node not found in received startup tree.");
1128                 startup_failed(ctx, SR_ERR_DATA);
1129                 return;
1130         }
1131
1132         target->on_update = startup_tree_crc_updated;
1133         target->on_update_param = ctx;
1134
1135         set_tree_integer(ctx->sdi, target, ctx->crc);
1136 }
1137
1138 static uint32_t crc32(const uint8_t *ptr, size_t size)
1139 {
1140         uint32_t result = 0xFFFFFFFF;
1141         uint32_t t;
1142         for (; size; size--, ptr++) {
1143                 result ^= *ptr;
1144                 for (int i = 0; i < 8; i++) {
1145                         t = result & 1;
1146                         result >>= 1;
1147                         if (t)
1148                                 result ^= 0xEDB88320;
1149                 }
1150         }
1151
1152         return ~result;
1153 }
1154
1155 static void startup_tree_updated(struct config_tree_node *node, void *param)
1156 {
1157         struct startup_context *ctx = param;
1158         struct dev_context *devc = ctx->sdi->priv;
1159
1160         GConverter *decompressor;
1161         GConverterResult decompress_result;
1162         GByteArray *tree_data;
1163         gsize input_read;
1164         gsize output_size;
1165         GError *err = NULL;
1166         int res;
1167         int id;
1168         const uint8_t *data;
1169         size_t size;
1170         struct config_tree_node *target;
1171
1172         ctx->crc = crc32(node->value.b->data, node->value.b->len);
1173
1174         tree_data = g_byte_array_new();
1175         g_byte_array_set_size(tree_data, 4096);
1176         decompressor = (GConverter *)g_zlib_decompressor_new(
1177                 G_ZLIB_COMPRESSOR_FORMAT_ZLIB);
1178         for (;;) {
1179                 g_converter_reset(decompressor);
1180                 decompress_result = g_converter_convert(decompressor,
1181                         node->value.b->data,
1182                         node->value.b->len,
1183                         tree_data->data,
1184                         tree_data->len,
1185                         G_CONVERTER_INPUT_AT_END,
1186                         &input_read,
1187                         &output_size,
1188                         &err);
1189                 if (decompress_result == G_CONVERTER_FINISHED)
1190                         break;
1191                 if (decompress_result == G_CONVERTER_ERROR) {
1192                         if (err->code == G_IO_ERROR_NO_SPACE &&
1193                                 tree_data->len < 1024 * 1024) {
1194                                 g_byte_array_set_size(tree_data,
1195                                         tree_data->len * 2);
1196                                 continue;
1197                         }
1198                         sr_err("Tree decompression failed: %s.", err->message);
1199                 } else {
1200                         sr_err("Tree decompression error %d.",
1201                                 (int)decompress_result);
1202                 }
1203                 startup_failed(ctx, SR_ERR_DATA);
1204                 return;
1205         }
1206         g_object_unref(decompressor);
1207
1208         sr_dbg("Config tree received (%d -> %d bytes) with CRC %08X.",
1209                 node->value.b->len, (int)output_size,
1210                 ctx->crc);
1211
1212         release_tree_node(&devc->tree_root);
1213         memset(&devc->tree_root, 0, sizeof(struct config_tree_node));
1214         memset(devc->tree_id_lookup, 0, sizeof(devc->tree_id_lookup));
1215
1216         id = 0;
1217         data = tree_data->data;
1218         size = output_size;
1219         res = deserialize_tree(devc, &devc->tree_root, &id, &data, &size);
1220         g_byte_array_free(tree_data, TRUE);
1221
1222         if (res != SR_OK) {
1223                 sr_err("Tree deserialization failed.");
1224                 startup_failed(ctx, res);
1225                 return;
1226         }
1227
1228         if ((target = lookup_tree_path(devc, "ADMIN:DIAGNOSTIC"))) {
1229                 target->on_update = tree_diagnostic_updated;
1230                 target->on_update_param = ctx->sdi;
1231         }
1232
1233         startup_send_tree_crc(ctx);
1234 }
1235
1236 static void release_rx_buffer(void *data)
1237 {
1238         GByteArray *ba = data;
1239         if (!ba)
1240                 return;
1241         g_byte_array_free(ba, TRUE);
1242 }
1243
1244 SR_PRIV int mooshimeter_dmm_open(const struct sr_dev_inst *sdi)
1245 {
1246         struct dev_context *devc = sdi->priv;
1247         struct sr_bt_desc *desc = sdi->conn;
1248         struct startup_context ctx;
1249         int ret;
1250
1251         release_tree_node(&devc->tree_root);
1252         memset(&devc->tree_root, 0, sizeof(struct config_tree_node));
1253         memset(devc->tree_id_lookup, 0, sizeof(devc->tree_id_lookup));
1254
1255         g_slist_free_full(devc->rx.reorder_buffer, release_rx_buffer);
1256         devc->rx.reorder_buffer = NULL;
1257         if (devc->rx.contents)
1258                 devc->rx.contents->len = 0;
1259         else
1260                 devc->rx.contents = g_byte_array_new();
1261         devc->rx.sequence_number = -1;
1262         devc->tx.sequence_number = 0;
1263
1264         ret = sr_bt_config_cb_data(desc, notify_cb, (void *)sdi);
1265         if (ret < 0)
1266                 return SR_ERR;
1267
1268         ret = sr_bt_connect_ble(desc);
1269         if (ret < 0)
1270                 return SR_ERR;
1271
1272         ret = sr_bt_start_notify(desc);
1273         if (ret < 0)
1274                 return SR_ERR;
1275
1276         memset(&ctx, 0, sizeof(ctx));
1277         ctx.sdi = (struct sr_dev_inst *)sdi;
1278
1279         allocate_startup_tree(devc);
1280         devc->tree_id_lookup[1]->on_update = startup_tree_updated;
1281         devc->tree_id_lookup[1]->on_update_param = &ctx;
1282         devc->tree_id_lookup[2]->on_update = tree_diagnostic_updated;
1283         devc->tree_id_lookup[2]->on_update_param = (struct sr_dev_inst *)sdi;
1284
1285         sr_spew("Initiating startup handshake.");
1286
1287         ret = poll_tree_value(sdi, devc->tree_id_lookup[1]);
1288         if (ret != SR_OK)
1289                 return ret;
1290
1291         return startup_run(&ctx);
1292 }
1293
1294 SR_PRIV int mooshimeter_dmm_close(const struct sr_dev_inst *sdi)
1295 {
1296         struct dev_context *devc = sdi->priv;
1297         struct sr_bt_desc *desc = sdi->conn;
1298
1299         sr_bt_disconnect(desc);
1300
1301         release_tree_node(&devc->tree_root);
1302         memset(&devc->tree_root, 0, sizeof(struct config_tree_node));
1303         memset(devc->tree_id_lookup, 0, sizeof(devc->tree_id_lookup));
1304
1305         g_slist_free_full(devc->rx.reorder_buffer, release_rx_buffer);
1306         devc->rx.reorder_buffer = NULL;
1307         if (devc->rx.contents)
1308                 g_byte_array_free(devc->rx.contents, TRUE);
1309         devc->rx.contents = NULL;
1310
1311         return SR_OK;
1312 }
1313
1314 SR_PRIV int mooshimeter_dmm_set_chooser(const struct sr_dev_inst *sdi,
1315         const char *path, const char *choice)
1316 {
1317         struct dev_context *devc = sdi->priv;
1318         struct config_tree_node *target;
1319         int value;
1320         uint32_t original_update_number;
1321
1322         value = lookup_chooser_index(devc, choice);
1323         if (value == -1) {
1324                 sr_err("Value %s not found for chooser %s.", choice, path);
1325                 return SR_ERR_DATA;
1326         }
1327
1328         target = lookup_tree_path(devc, path);
1329         if (!target) {
1330                 sr_err("Tree path %s not found.", path);
1331                 return SR_ERR_DATA;
1332         }
1333
1334         sr_spew("Setting chooser %s to %s (%d).", path, choice, value);
1335
1336         original_update_number = target->update_number;
1337         set_tree_integer(sdi, target, value);
1338         return wait_for_update(sdi, target, original_update_number);
1339 }
1340
1341 SR_PRIV int mooshimeter_dmm_set_integer(const struct sr_dev_inst *sdi,
1342         const char *path, int value)
1343 {
1344         struct dev_context *devc = sdi->priv;
1345         struct config_tree_node *target;
1346         uint32_t original_update_number;
1347
1348         target = lookup_tree_path(devc, path);
1349         if (!target) {
1350                 sr_err("Tree path %s not found.", path);
1351                 return SR_ERR_DATA;
1352         }
1353
1354         sr_spew("Setting integer %s to %d.", path, value);
1355
1356         original_update_number = target->update_number;
1357         set_tree_integer(sdi, target, value);
1358         return wait_for_update(sdi, target, original_update_number);
1359 }
1360
1361 static struct config_tree_node *select_next_largest_in_tree(
1362         struct dev_context *devc,
1363         const char *parent, float number)
1364 {
1365         float node_value;
1366         float distance;
1367         float best_distance = 0;
1368         struct config_tree_node *choice_parent;
1369         struct config_tree_node *selected_choice = NULL;
1370
1371         choice_parent = lookup_tree_path(devc, parent);
1372         if (!choice_parent) {
1373                 sr_err("Tree path %s not found.", parent);
1374                 return NULL;
1375         }
1376         if (!choice_parent->count_children) {
1377                 sr_err("Tree path %s has no children.", parent);
1378                 return NULL;
1379         }
1380
1381         for (size_t i = 0; i < choice_parent->count_children; i++) {
1382                 node_value = strtof(choice_parent->children[i].name, NULL);
1383                 if (node_value <= 0)
1384                         continue;
1385                 distance = node_value - number;
1386                 if (!selected_choice) {
1387                         selected_choice = &choice_parent->children[i];
1388                         best_distance = distance;
1389                         continue;
1390                 }
1391                 /* Select the one that's the least below it, if all
1392                  * are below the target */
1393                 if (distance < 0) {
1394                         if (best_distance > 0)
1395                                 continue;
1396                         if (distance > best_distance) {
1397                                 selected_choice = &choice_parent->children[i];
1398                                 best_distance = distance;
1399                         }
1400                         continue;
1401                 }
1402                 if (best_distance < 0 || distance < best_distance) {
1403                         selected_choice = &choice_parent->children[i];
1404                         best_distance = distance;
1405                 }
1406         }
1407
1408         return selected_choice;
1409 }
1410
1411 SR_PRIV int mooshimeter_dmm_set_larger_number(const struct sr_dev_inst *sdi,
1412         const char *path, const char *parent, float number)
1413 {
1414         struct dev_context *devc = sdi->priv;
1415         struct config_tree_node *selected_choice;
1416         struct config_tree_node *target;
1417         uint32_t original_update_number;
1418
1419         selected_choice = select_next_largest_in_tree(devc, parent, number);
1420         if (!selected_choice) {
1421                 sr_err("No choice available for %f at %s.", number, parent);
1422                 return SR_ERR_NA;
1423         }
1424
1425         target = lookup_tree_path(devc, path);
1426         if (!target) {
1427                 sr_err("Tree path %s not found.", path);
1428                 return SR_ERR_DATA;
1429         }
1430
1431         sr_spew("Setting number choice %s to index %d for requested %g.", path,
1432                 (int)selected_choice->index_in_parent, number);
1433
1434         original_update_number = target->update_number;
1435         set_tree_integer(sdi, target, selected_choice->index_in_parent);
1436         return wait_for_update(sdi, target, original_update_number);
1437 }
1438
1439 SR_PRIV gboolean mooshimeter_dmm_set_autorange(const struct sr_dev_inst *sdi,
1440         const char *path, const char *parent, float latest)
1441 {
1442         struct dev_context *devc = sdi->priv;
1443         struct config_tree_node *selected_choice;
1444         struct config_tree_node *target;
1445
1446         selected_choice = select_next_largest_in_tree(devc, parent,
1447                 fabsf(latest));
1448         if (!selected_choice) {
1449                 sr_err("No choice available for %f at %s.", latest, parent);
1450                 return FALSE;
1451         }
1452
1453         target = lookup_tree_path(devc, path);
1454         if (!target) {
1455                 sr_err("Tree path %s not found.", path);
1456                 return FALSE;
1457         }
1458
1459         if (get_tree_integer(target) == (int)selected_choice->index_in_parent)
1460                 return FALSE;
1461
1462         sr_spew("Changing autorange %s to index %d for %g.", path,
1463                 (int)selected_choice->index_in_parent, latest);
1464
1465         set_tree_integer(sdi, target, selected_choice->index_in_parent);
1466
1467         return TRUE;
1468 }
1469
1470 SR_PRIV int mooshimeter_dmm_get_chosen_number(const struct sr_dev_inst *sdi,
1471         const char *path, const char *parent, float *number)
1472 {
1473         struct dev_context *devc = sdi->priv;
1474         struct config_tree_node *value_node;
1475         struct config_tree_node *available;
1476         int32_t selected;
1477
1478         value_node = lookup_tree_path(devc, path);
1479         if (!value_node) {
1480                 sr_err("Tree path %s not found.", path);
1481                 return SR_ERR_DATA;
1482         }
1483
1484         available = lookup_tree_path(devc, parent);
1485         if (!available) {
1486                 sr_err("Tree path %s not found.", path);
1487                 return SR_ERR_DATA;
1488         }
1489
1490         selected = get_tree_integer(value_node);
1491         if (selected < 0 || selected >= (int32_t)available->count_children)
1492                 return SR_ERR_DATA;
1493
1494         *number = g_ascii_strtod(available->children[selected].name, NULL);
1495
1496         return SR_OK;
1497 }
1498
1499 SR_PRIV int mooshimeter_dmm_get_available_number_choices(
1500         const struct sr_dev_inst *sdi, const char *path,
1501         float **numbers, size_t *count)
1502 {
1503         struct dev_context *devc = sdi->priv;
1504         struct config_tree_node *available;
1505
1506         available = lookup_tree_path(devc, path);
1507         if (!available) {
1508                 sr_err("Tree path %s not found.", path);
1509                 return SR_ERR_NA;
1510         }
1511
1512         *numbers = g_malloc(sizeof(float) * available->count_children);
1513         *count = available->count_children;
1514
1515         for (size_t i = 0; i < available->count_children; i++) {
1516                 (*numbers)[i] = g_ascii_strtod(available->children[i].name,
1517                         NULL);
1518         }
1519
1520         return SR_OK;
1521 }
1522
1523 SR_PRIV int mooshimeter_dmm_poll(int fd, int revents, void *cb_data)
1524 {
1525         struct sr_dev_inst *sdi;
1526         struct sr_bt_desc *desc;
1527
1528         (void)fd;
1529         (void)revents;
1530
1531         if (!(sdi = cb_data))
1532                 return TRUE;
1533
1534         desc = sdi->conn;
1535
1536         while (sr_bt_check_notify(desc) > 0);
1537
1538         return TRUE;
1539 }
1540
1541 /*
1542  * The meter will disconnect if it doesn't receive a host command for 30 (?)
1543  * seconds, so periodically poll a trivial value to keep it alive.
1544  */
1545 SR_PRIV int mooshimeter_dmm_heartbeat(int fd, int revents, void *cb_data)
1546 {
1547         struct sr_dev_inst *sdi;
1548         struct dev_context *devc;
1549         struct config_tree_node *target;
1550
1551         (void)fd;
1552         (void)revents;
1553
1554         if (!(sdi = cb_data))
1555                 return TRUE;
1556
1557         if (!(devc = sdi->priv))
1558                 return TRUE;
1559
1560         target = lookup_tree_path(devc, "PCB_VERSION");
1561         if (!target) {
1562                 sr_err("Tree for PCB_VERSION not found.");
1563                 return FALSE;
1564         }
1565
1566         sr_spew("Sending heartbeat request.");
1567         poll_tree_value(sdi, target);
1568
1569         return TRUE;
1570 }