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