]>
Commit | Line | Data |
---|---|---|
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 = ¤t->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 | } |