From 1dc9569e31717aefab8e05b858122f433dab1698 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 13 Oct 2013 11:44:26 +0200
Subject: [PATCH] Fixed mixer chunk load and line ending detection for good.
---
src/modules/systemlib/mixer/mixer.cpp | 27 ++++
src/modules/systemlib/mixer/mixer.h | 23 ++++
src/modules/systemlib/mixer/mixer_group.cpp | 15 ++
src/modules/systemlib/mixer/mixer_load.c | 15 +-
src/modules/systemlib/mixer/mixer_load.h | 2 +-
.../systemlib/mixer/mixer_multirotor.cpp | 10 +-
src/modules/systemlib/mixer/mixer_simple.cpp | 62 +++------
src/systemcmds/mixer/mixer.cpp | 2 +-
src/systemcmds/tests/test_mixer.cpp | 129 +++++++++++++++++-
9 files changed, 229 insertions(+), 56 deletions(-)
diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp
index b4b82c539f..cce46bf5fc 100644
--- a/src/modules/systemlib/mixer/mixer.cpp
+++ b/src/modules/systemlib/mixer/mixer.cpp
@@ -116,6 +116,33 @@ Mixer::scale_check(struct mixer_scaler_s &scaler)
return 0;
}
+const char *
+Mixer::findtag(const char *buf, unsigned &buflen, char tag)
+{
+ while (buflen >= 2) {
+ if ((buf[0] == tag) && (buf[1] == ':'))
+ return buf;
+ buf++;
+ buflen--;
+ }
+ return nullptr;
+}
+
+const char *
+Mixer::skipline(const char *buf, unsigned &buflen)
+{
+ const char *p;
+
+ /* if we can find a CR or NL in the buffer, skip up to it */
+ if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) {
+ /* skip up to it AND one beyond - could be on the NUL symbol now */
+ buflen -= (p - buf) + 1;
+ return p + 1;
+ }
+
+ return nullptr;
+}
+
/****************************************************************************/
NullMixer::NullMixer() :
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index 6c322ff92e..723bf9f3b7 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -212,6 +212,24 @@ protected:
*/
static int scale_check(struct mixer_scaler_s &scaler);
+ /**
+ * Find a tag
+ *
+ * @param buf The buffer to operate on.
+ * @param buflen length of the buffer.
+ * @param tag character to search for.
+ */
+ static const char * findtag(const char *buf, unsigned &buflen, char tag);
+
+ /**
+ * Skip a line
+ *
+ * @param buf The buffer to operate on.
+ * @param buflen length of the buffer.
+ * @return 0 / OK if a line could be skipped, 1 else
+ */
+ static const char * skipline(const char *buf, unsigned &buflen);
+
private:
};
@@ -240,6 +258,11 @@ public:
*/
void reset();
+ /**
+ * Count the mixers in the group.
+ */
+ unsigned count();
+
/**
* Adds mixers to the group based on a text description in a buffer.
*
diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp
index 1dbc512cdb..3ed99fba09 100644
--- a/src/modules/systemlib/mixer/mixer_group.cpp
+++ b/src/modules/systemlib/mixer/mixer_group.cpp
@@ -111,6 +111,20 @@ MixerGroup::mix(float *outputs, unsigned space)
return index;
}
+unsigned
+MixerGroup::count()
+{
+ Mixer *mixer = _first;
+ unsigned index = 0;
+
+ while ((mixer != nullptr)) {
+ mixer = mixer->_next;
+ index++;
+ }
+
+ return index;
+}
+
void
MixerGroup::groups_required(uint32_t &groups)
{
@@ -170,6 +184,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
/* only adjust buflen if parsing was successful */
buflen = resid;
+ debug("SUCCESS - buflen: %d", buflen);
} else {
/*
diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c
index 18c4e474a6..a55ddf8a35 100644
--- a/src/modules/systemlib/mixer/mixer_load.c
+++ b/src/modules/systemlib/mixer/mixer_load.c
@@ -38,22 +38,22 @@
*/
#include <nuttx/config.h>
-#include <systemlib/err.h>
#include <string.h>
#include <stdio.h>
#include <ctype.h>
#include "mixer_load.h"
-int load_mixer_file(const char *fname, char *buf)
+int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
{
FILE *fp;
char line[120];
/* open the mixer definition file */
fp = fopen(fname, "r");
- if (fp == NULL)
- err(1, "can't open %s", fname);
+ if (fp == NULL) {
+ return 1;
+ }
/* read valid lines from the file into a buffer */
buf[0] = '\0';
@@ -70,7 +70,7 @@ int load_mixer_file(const char *fname, char *buf)
/* compact whitespace in the buffer */
char *t, *f;
- for (f = buf; *f != '\0'; f++) {
+ for (f = line; *f != '\0'; f++) {
/* scan for space characters */
if (*f == ' ') {
/* look for additional spaces */
@@ -87,8 +87,9 @@ int load_mixer_file(const char *fname, char *buf)
}
/* if the line is too long to fit in the buffer, bail */
- if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf))
- break;
+ if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
+ return 1;
+ }
/* add the line to the buffer */
strcat(buf, line);
diff --git a/src/modules/systemlib/mixer/mixer_load.h b/src/modules/systemlib/mixer/mixer_load.h
index b2327a4f72..4b7091d5b2 100644
--- a/src/modules/systemlib/mixer/mixer_load.h
+++ b/src/modules/systemlib/mixer/mixer_load.h
@@ -44,7 +44,7 @@
__BEGIN_DECLS
-__EXPORT int load_mixer_file(const char *fname, char *buf);
+__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen);
__END_DECLS
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 89afc272c0..b89f341b60 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -205,11 +205,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
}
if (used > (int)buflen) {
- debug("multirotor spec used %d of %u", used, buflen);
+ debug("OVERFLOW: multirotor spec used %d of %u", used, buflen);
return nullptr;
}
- buflen -= used;
+ buf = skipline(buf, buflen);
+ if (buf == nullptr) {
+ debug("no line ending, line is incomplete");
+ return nullptr;
+ }
+
+ debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
if (!strcmp(geomname, "4+")) {
geometry = MultirotorMixer::QUAD_PLUS;
diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp
index c8434f991b..c3985b5de6 100644
--- a/src/modules/systemlib/mixer/mixer_simple.cpp
+++ b/src/modules/systemlib/mixer/mixer_simple.cpp
@@ -55,7 +55,7 @@
#include "mixer.h"
#define debug(fmt, args...) do { } while(0)
-// #define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
+//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
SimpleMixer::SimpleMixer(ControlCallback control_cb,
uintptr_t cb_handle,
@@ -71,28 +71,6 @@ SimpleMixer::~SimpleMixer()
free(_info);
}
-static const char *
-findtag(const char *buf, unsigned &buflen, char tag)
-{
- while (buflen >= 2) {
- if ((buf[0] == tag) && (buf[1] == ':'))
- return buf;
- buf++;
- buflen--;
- }
- return nullptr;
-}
-
-static void
-skipline(const char *buf, unsigned &buflen)
-{
- const char *p;
-
- /* if we can find a CR or NL in the buffer, skip up to it */
- if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen)))
- buflen -= (p - buf);
-}
-
int
SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
{
@@ -111,7 +89,12 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
return -1;
}
- skipline(buf, buflen);
+
+ buf = skipline(buf, buflen);
+ if (buf == nullptr) {
+ debug("no line ending, line is incomplete");
+ return -1;
+ }
scaler.negative_scale = s[0] / 10000.0f;
scaler.positive_scale = s[1] / 10000.0f;
@@ -130,7 +113,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
buf = findtag(buf, buflen, 'S');
if ((buf == nullptr) || (buflen < 16)) {
- debug("contorl parser failed finding tag, ret: '%s'", buf);
+ debug("control parser failed finding tag, ret: '%s'", buf);
return -1;
}
@@ -139,7 +122,12 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
debug("control parse failed on '%s'", buf);
return -1;
}
- skipline(buf, buflen);
+
+ buf = skipline(buf, buflen);
+ if (buf == nullptr) {
+ debug("no line ending, line is incomplete");
+ return -1;
+ }
control_group = u[0];
control_index = u[1];
@@ -161,29 +149,17 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
int used;
const char *end = buf + buflen;
- /* enforce that the mixer ends with space or a new line */
- for (int i = buflen - 1; i >= 0; i--) {
- if (buf[i] == '\0')
- continue;
-
- /* require a space or newline at the end of the buffer, fail on printable chars */
- if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
- /* found a line ending or space, so no split symbols / numbers. good. */
- break;
- } else {
- debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
- goto out;
- }
-
- }
-
/* get the base info for the mixer */
if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
debug("simple parse failed on '%s'", buf);
goto out;
}
- buflen -= used;
+ buf = skipline(buf, buflen);
+ if (buf == nullptr) {
+ debug("no line ending, line is incomplete");
+ goto out;
+ }
mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp
index b1ebebbb44..6da39d371b 100644
--- a/src/systemcmds/mixer/mixer.cpp
+++ b/src/systemcmds/mixer/mixer.cpp
@@ -102,7 +102,7 @@ load(const char *devname, const char *fname)
if (ioctl(dev, MIXERIOCRESET, 0))
err(1, "can't reset mixers on %s", devname);
- load_mixer_file(fname, &buf[0]);
+ load_mixer_file(fname, &buf[0], sizeof(buf));
/* XXX pass the buffer to the device */
int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index acb7bd88f6..4da86042d2 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -46,6 +46,7 @@
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
+#include <string.h>
#include <time.h>
#include <systemlib/err.h>
@@ -53,6 +54,11 @@
#include "tests.h"
+static int mixer_callback(uintptr_t handle,
+ uint8_t control_group,
+ uint8_t control_index,
+ float &control);
+
int test_mixer(int argc, char *argv[])
{
warnx("testing mixer");
@@ -66,9 +72,128 @@ int test_mixer(int argc, char *argv[])
char buf[2048];
- load_mixer_file(filename, &buf[0]);
+ load_mixer_file(filename, &buf[0], sizeof(buf));
+ unsigned loaded = strlen(buf);
+
+ warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
+
+ /* load the mixer in chunks, like
+ * in the case of a remote load,
+ * e.g. on PX4IO.
+ */
+
+ unsigned nused = 0;
+
+ const unsigned chunk_size = 64;
+
+ MixerGroup mixer_group(mixer_callback, 0);
+
+ /* load at once test */
+ unsigned xx = loaded;
+ mixer_group.load_from_buf(&buf[0], xx);
+ warnx("complete buffer load: loaded %u mixers", mixer_group.count());
+ if (mixer_group.count() != 8)
+ return 1;
+
+ unsigned empty_load = 2;
+ char empty_buf[2];
+ empty_buf[0] = ' ';
+ empty_buf[1] = '\0';
+ mixer_group.reset();
+ mixer_group.load_from_buf(&empty_buf[0], empty_load);
+ warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
+ if (empty_load != 0)
+ return 1;
+
+ /* FIRST mark the mixer as invalid */
+ bool mixer_ok = false;
+ /* THEN actually delete it */
+ mixer_group.reset();
+ char mixer_text[256]; /* large enough for one mixer */
+ unsigned mixer_text_length = 0;
+
+ unsigned transmitted = 0;
+
+ warnx("transmitted: %d, loaded: %d", transmitted, loaded);
+
+ while (transmitted < loaded) {
+
+ unsigned text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted;
+
+ /* check for overflow - this would be really fatal */
+ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
+ bool mixer_ok = false;
+ return 1;
+ }
+
+ /* append mixer text and nul-terminate */
+ memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length);
+ mixer_text_length += text_length;
+ mixer_text[mixer_text_length] = '\0';
+ warnx("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]);
+
+ /* process the text buffer, adding new mixers as their descriptions can be parsed */
+ unsigned resid = mixer_text_length;
+ mixer_group.load_from_buf(&mixer_text[0], resid);
+
+ /* if anything was parsed */
+ if (resid != mixer_text_length) {
+
+ /* only set mixer ok if no residual is left over */
+ if (resid == 0) {
+ mixer_ok = true;
+ } else {
+ /* not yet reached the end of the mixer, set as not ok */
+ mixer_ok = false;
+ }
+
+ warnx("used %u", mixer_text_length - resid);
+
+ /* copy any leftover text to the base of the buffer for re-use */
+ if (resid > 0)
+ memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+
+ mixer_text_length = resid;
+ }
+
+ transmitted += text_length;
+ }
+
+ warnx("chunked load: loaded %u mixers", mixer_group.count());
+
+ if (mixer_group.count() != 8)
+ return 1;
+
+ /* load multirotor at once test */
+ mixer_group.reset();
+
+ if (argc > 2)
+ filename = argv[2];
+ else
+ filename = "/etc/mixers/FMU_quad_w.mix";
+
+ load_mixer_file(filename, &buf[0], sizeof(buf));
+ loaded = strlen(buf);
+
+ warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
+
+ unsigned mc_loaded = loaded;
+ mixer_group.load_from_buf(&buf[0], mc_loaded);
+ warnx("complete buffer load: loaded %u mixers", mixer_group.count());
+ if (mixer_group.count() != 8)
+ return 1;
+}
+
+static int
+mixer_callback(uintptr_t handle,
+ uint8_t control_group,
+ uint8_t control_index,
+ float &control)
+{
+ if (control_group != 0)
+ return -1;
- warnx("loaded: %s", &buf[0]);
+ control = 0.0f;
return 0;
}
--
GitLab