From 0616d5834039cc08057b862f80f8129a7b4948af Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 29 Oct 2012 21:46:56 -0700 Subject: Add 'show' and 'test' verbs to the boardinfo command. Teach rcS how to use the new version. --- ROMFS/scripts/rcS | 4 +- apps/systemcmds/boardinfo/boardinfo.c | 156 +++++++++++++++++++++++++++++++--- apps/systemlib/bson/tinybson.c | 11 ++- apps/systemlib/bson/tinybson.h | 4 +- 4 files changed, 159 insertions(+), 16 deletions(-) diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS index b5fbfe0f5..4152494e0 100755 --- a/ROMFS/scripts/rcS +++ b/ROMFS/scripts/rcS @@ -83,7 +83,7 @@ else # # Are we attached to a PX4IOAR (AR.Drone carrier board)? # - if boardinfo -t 7 + if boardinfo test name PX4IOAR then set BOARD PX4IOAR if [ -f /etc/init.d/rc.PX4IOAR ] @@ -99,7 +99,7 @@ else # # Are we attached to a PX4IO? # - if boardinfo -t 6 + if boardinfo test name PX4IO then set BOARD PX4IO if [ -f /etc/init.d/rc.PX4IO ] diff --git a/apps/systemcmds/boardinfo/boardinfo.c b/apps/systemcmds/boardinfo/boardinfo.c index 9e6189fec..fb9c0b3a4 100644 --- a/apps/systemcmds/boardinfo/boardinfo.c +++ b/apps/systemcmds/boardinfo/boardinfo.c @@ -118,7 +118,6 @@ eeprom_write(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) } }; - warnx("write 0x%02x/%u", address, count); result = I2C_TRANSFER(dev, msgv, 1); if (result != OK) { warnx("EEPROM write failed: %d", result); @@ -172,7 +171,6 @@ eeprom_read(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) } }; - warnx("read 0x%02x/%u", address, count); result = I2C_TRANSFER(dev, msgv, 2); if (result != OK) { warnx("EEPROM read failed: %d", result); @@ -187,6 +185,41 @@ out: return result; } +static void * +idrom_read(const struct eeprom_info_s *eeprom) +{ + uint32_t size = 0xffffffff; + int result; + void *buf = NULL; + + result = eeprom_read(eeprom, (uint8_t *)&size, sizeof(size)); + if (result != 0) { + warnx("failed reading ID ROM length"); + goto fail; + } + if (size > (eeprom->page_size * eeprom->page_count)) { + warnx("ID ROM not programmed"); + goto fail; + } + + buf = malloc(size); + if (buf == NULL) { + warnx("could not allocate %d bytes for ID ROM", size); + goto fail; + } + result = eeprom_read(eeprom, buf, size); + if (result != 0) { + warnx("failed reading ID ROM"); + goto fail; + } + return buf; + +fail: + if (buf != NULL) + free(buf); + return NULL; +} + static void boardinfo_set(const struct eeprom_info_s *eeprom, char *spec) { @@ -228,7 +261,7 @@ boardinfo_set(const struct eeprom_info_s *eeprom, char *spec) result = 1; goto out; } - if (*(uint32_t *)bson_encoder_buf_data(&encoder) != bson_encoder_buf_size(&encoder)) { + if ((int)*(uint32_t *)bson_encoder_buf_data(&encoder) != bson_encoder_buf_size(&encoder)) { warnx("buffer length mismatch"); result = 1; goto out; @@ -249,21 +282,121 @@ out: exit(result); } +static int +boardinfo_print(bson_decoder_t decoder, void *private, bson_node_t node) +{ + switch (node->type) { + case BSON_INT32: + printf("%s: %d / 0x%08x\n", node->name, (int)node->i, (unsigned)node->i); + break; + case BSON_STRING: { + char buf[bson_decoder_data_pending(decoder)]; + bson_decoder_copy_data(decoder, buf); + printf("%s: %s\n", node->name, buf); + break; + } + case BSON_EOO: + break; + default: + warnx("unexpected node type %d", node->type); + break; + } + return 1; +} + static void boardinfo_show(const struct eeprom_info_s *eeprom) { - uint32_t size = 0xffffffff; - int result; + struct bson_decoder_s decoder; + void *buf; - result = eeprom_read(eeprom, (uint8_t *)&size, sizeof(size)); - if (result != 0) { - warnx("failed reading ID ROM length"); - } - warnx("data length 0x%08x", size); + buf = idrom_read(eeprom); + if (buf == NULL) + errx(1, "ID ROM read failed"); + if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_print, NULL) == 0) { + while (bson_decoder_next(&decoder) > 0) + ; + } else { + warnx("failed to init decoder"); + } + free(buf); exit(0); } +struct { + const char *property; + const char *value; +} test_args; + +static int +boardinfo_test_callback(bson_decoder_t decoder, void *private, bson_node_t node) +{ + /* reject nodes with non-matching names */ + if (strcmp(node->name, test_args.property)) + return 1; + + /* compare node values to check for a match */ + switch (node->type) { + case BSON_STRING: { + char buf[bson_decoder_data_pending(decoder)]; + bson_decoder_copy_data(decoder, buf); + + /* check for a match */ + if (!strcmp(test_args.value, buf)) { + return 2; + } + break; + } + + case BSON_INT32: { + int32_t val = strtol(test_args.value, NULL, 0); + + /* check for a match */ + if (node->i == val) { + return 2; + } + break; + } + + default: + break; + } + + return 1; +} + +static void +boardinfo_test(const struct eeprom_info_s *eeprom, const char *property, const char *value) +{ + struct bson_decoder_s decoder; + void *buf; + int result = -1; + + if ((property == NULL) || (strlen(property) == 0) || + (value == NULL) || (strlen(value) == 0)) + errx(1, "missing property name or value"); + + test_args.property = property; + test_args.value = value; + + buf = idrom_read(eeprom); + if (buf == NULL) + errx(1, "ID ROM read failed"); + + if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_test_callback, NULL) == 0) { + do { + result = bson_decoder_next(&decoder); + } while (result == 1); + } else { + warnx("failed to init decoder"); + } + free(buf); + + /* if we matched, we exit with zero success */ + exit((result == 2) ? 0 : 1); +} + int boardinfo_main(int argc, char *argv[]) { @@ -273,6 +406,9 @@ boardinfo_main(int argc, char *argv[]) if (!strcmp(argv[1], "show")) boardinfo_show(&eeprom_info[0]); + if (!strcmp(argv[1], "test")) + boardinfo_test(&eeprom_info[0], argv[2], argv[3]); + errx(1, "missing/unrecognised command, try one of 'set', 'show', 'test'"); } diff --git a/apps/systemlib/bson/tinybson.c b/apps/systemlib/bson/tinybson.c index 1b5a1e1ee..321466f87 100644 --- a/apps/systemlib/bson/tinybson.c +++ b/apps/systemlib/bson/tinybson.c @@ -127,13 +127,18 @@ bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_ int32_t len; /* argument sanity */ - if ((buf == NULL) || (bufsize < 5) || (callback == NULL)) + if ((buf == NULL) || (callback == NULL)) return -1; decoder->fd = -1; decoder->buf = (uint8_t *)buf; decoder->dead = false; - decoder->bufsize = bufsize; + if (bufsize == 0) { + decoder->bufsize = *(uint32_t *)buf; + debug("auto-detected %u byte object", decoder->bufsize); + } else { + decoder->bufsize = bufsize; + } decoder->bufpos = 0; decoder->callback = callback; decoder->private = private; @@ -144,7 +149,7 @@ bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_ /* read and discard document size */ if (read_int32(decoder, &len)) CODER_KILL(decoder, "failed reading length"); - if ((len > 0) && (len > (int)bufsize)) + if ((len > 0) && (len > (int)decoder->bufsize)) CODER_KILL(decoder, "document length larger than buffer"); /* ready for decoding */ diff --git a/apps/systemlib/bson/tinybson.h b/apps/systemlib/bson/tinybson.h index d820aa7b9..666f8191a 100644 --- a/apps/systemlib/bson/tinybson.h +++ b/apps/systemlib/bson/tinybson.h @@ -134,7 +134,9 @@ __EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder * * @param decoder Decoder state structure to be initialised. * @param buf Buffer to read from. - * @param bufsize Size of the buffer (BSON object may be smaller). + * @param bufsize Size of the buffer (BSON object may be smaller). May be + * passed as zero if the buffer size should be extracted from the + * BSON header only. * @param callback Callback to be invoked by bson_decoder_next * @param private Callback private data, stored in node. * @return Zero on success. -- cgit v1.2.3