forked from Archive/PX4-Autopilot
Add 'show' and 'test' verbs to the boardinfo command. Teach rcS how to use the new version.
This commit is contained in:
parent
7203ba797e
commit
0616d58340
|
@ -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 ]
|
||||
|
|
|
@ -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");
|
||||
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");
|
||||
}
|
||||
warnx("data length 0x%08x", size);
|
||||
|
||||
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'");
|
||||
}
|
||||
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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.
|
||||
|
|
Loading…
Reference in New Issue