Add serial read-length handling.

2 files changed, 13 insertions, 6 deletions

diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 29107f79f..6cdf86b2b 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -36,7 +36,8 @@
 /**
  * @file protocol.h
  *
- * PX4IO interface protocol.
+ * PX4IO interface protocol
+ * ========================
  *
  * Communication is performed via writes to and reads from 16-bit virtual
  * registers organised into pages of 255 registers each.
@@ -63,19 +64,21 @@
  * readable pages to be densely packed. Page numbers do not need to be
  * packed.
  *
- * PX4IO I2C interface notes:
+ * PX4IO I2C interface notes
+ * -------------------------
  *
  * Register read/write operations are mapped directly to PX4IO register
  * read/write operations.
  *
- * PX4IO Serial interface notes:
+ * PX4IO Serial interface notes
+ * ----------------------------
  *
  * The MSB of the page number is used to distinguish between read and
  * write operations. If set, the operation is a write and additional 
  * data is expected to follow in the packet as for I2C writes.
  *
  * If clear, the packet is expected to contain a single byte giving the
- * number of bytes to be read. PX4IO will respond with a packet containing
+ * number of registers to be read. PX4IO will respond with a packet containing
  * the same header (page, offset) and the requested data.
  *
  * If a read is requested when PX4IO does not have buffer space to store
diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c
index a12d58aca..bf9456e94 100644
--- a/src/modules/px4iofirmware/serial.c
+++ b/src/modules/px4iofirmware/serial.c
@@ -107,7 +107,9 @@ serial_callback(void *arg, const void *data, unsigned length)
 		return;
 	}
 
-	/* it's a read */
+	/* it's a read - must contain length byte */
+	if (length != 3)
+		return;
 	uint16_t *registers;
 	unsigned count;
 
@@ -118,10 +120,12 @@ serial_callback(void *arg, const void *data, unsigned length)
 	if (registers_get(message[0], message[1], &registers, &count) < 0)
 		count = 0;
 
-	/* fill buffer with message */
+	/* fill buffer with message, limited by length */
 #define TX_MAX		((sizeof(tx_buf) - 2) / 2)
 	if (count > TX_MAX)
 		count = TX_MAX;
+	if (count > message[2])
+		count = message[2];
 	memcpy(&tx_buf[2], registers, count * 2);
 
 	/* try to send the message */