Checkpoint: interface abstraction for px4io driver

src/modules/px4iofirmware/protocol.h

@@ -36,8 +36,7 @@
 /**
  * @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.
@@ -46,7 +45,7 @@
  * respectively. Subsequent reads and writes increment the offset within
  * the page. 
  *
- * Most pages are readable or writable but not both.
+ * Some pages are read- or write-only.
  *
  * Note that some pages may permit offset values greater than 255, which
  * can only be achieved by long writes. The offset does not wrap.
@@ -63,29 +62,6 @@
  * Note that the implementation of readable pages prefers registers within
  * readable pages to be densely packed. Page numbers do not need to be
  * packed.
- *
- * PX4IO I2C interface notes
- * -------------------------
- *
- * Register read/write operations are mapped directly to PX4IO register
- * read/write operations.
- *
- * 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 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
- * the reply, the request will be dropped. PX4IO is always configured with
- * enough space to receive one full-sized write and one read request, and
- * to send one full-sized read response.
- *
  */
 
 #define PX4IO_CONTROL_CHANNELS			8
@@ -99,14 +75,12 @@
 #define REG_TO_FLOAT(_reg)	((float)REG_TO_SIGNED(_reg) / 10000.0f)
 #define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
 
-#define PX4IO_PAGE_WRITE		(1<<7)
-
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG		0
 #define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_SOFTWARE_VERSION		1	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_BOOTLOADER_VERSION	2	/* get this how? */
-#define PX4IO_P_CONFIG_MAX_TRANSFER		3	/* maximum packet transfer size */
+#define PX4IO_P_CONFIG_MAX_TRANSFER		3	/* maximum I2C transfer size */
 #define PX4IO_P_CONFIG_CONTROL_COUNT		4	/* hardcoded max control count supported */
 #define PX4IO_P_CONFIG_ACTUATOR_COUNT		5	/* hardcoded max actuator output count */
 #define PX4IO_P_CONFIG_RC_INPUT_COUNT		6	/* hardcoded max R/C input count supported */
@@ -168,7 +142,7 @@
 #define PX4IO_RATE_MAP_BASE			0	/* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
 
 /* setup page */
-#define PX4IO_PAGE_SETUP		64
+#define PX4IO_PAGE_SETUP			100
 #define PX4IO_P_SETUP_FEATURES			0
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
@@ -186,13 +160,13 @@
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
 /* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS		65		/* 0..CONFIG_CONTROL_COUNT */
+#define PX4IO_PAGE_CONTROLS			101	/* 0..CONFIG_CONTROL_COUNT */
 
 /* raw text load to the mixer parser - ignores offset */
-#define PX4IO_PAGE_MIXERLOAD		66		/* see px4io_mixdata structure below */
+#define PX4IO_PAGE_MIXERLOAD			102
 
 /* R/C channel config */
-#define PX4IO_PAGE_RC_CONFIG		67		/* R/C input configuration */
+#define PX4IO_PAGE_RC_CONFIG			103	/* R/C input configuration */
 #define PX4IO_P_RC_CONFIG_MIN			0	/* lowest input value */
 #define PX4IO_P_RC_CONFIG_CENTER		1	/* center input value */
 #define PX4IO_P_RC_CONFIG_MAX			2	/* highest input value */
@@ -204,10 +178,10 @@
 #define PX4IO_P_RC_CONFIG_STRIDE		6	/* spacing between channel config data */
 
 /* PWM output - overrides mixer */
-#define PX4IO_PAGE_DIRECT_PWM		68		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DIRECT_PWM			104	/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* PWM failsafe values - zero disables the output */
-#define PX4IO_PAGE_FAILSAFE_PWM		69		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_FAILSAFE_PWM			105	/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /**
  * As-needed mixer data upload.

src/modules/systemlib/hx_stream.c

@@ -53,14 +53,26 @@
 
 
 struct hx_stream {
-	uint8_t			buf[HX_STREAM_MAX_FRAME + 4];
-	unsigned		frame_bytes;
-	bool			escaped;
-	bool			txerror;
+	/* RX state */
+	uint8_t			rx_buf[HX_STREAM_MAX_FRAME + 4];
+	unsigned		rx_frame_bytes;
+	bool			rx_escaped;
+	hx_stream_rx_callback	rx_callback;
+	void			*rx_callback_arg;
 
+	/* TX state */
 	int			fd;
-	hx_stream_rx_callback	callback;
-	void			*callback_arg;
+	bool			tx_error;
+	uint8_t			*tx_buf;
+	unsigned		tx_resid;
+	uint32_t		tx_crc;
+	enum {
+		TX_IDLE = 0,
+		TX_SEND_START,
+		TX_SEND_DATA,
+		TX_SENT_ESCAPE,
+		TX_SEND_END
+	}			tx_state;
 
 	perf_counter_t		pc_tx_frames;
 	perf_counter_t		pc_rx_frames;
@@ -76,13 +88,12 @@ struct hx_stream {
 static void	hx_tx_raw(hx_stream_t stream, uint8_t c);
 static void	hx_tx_raw(hx_stream_t stream, uint8_t c);
 static int	hx_rx_frame(hx_stream_t stream);
-static bool	hx_rx_char(hx_stream_t stream, uint8_t c);
 
 static void
 hx_tx_raw(hx_stream_t stream, uint8_t c)
 {
 	if (write(stream->fd, &c, 1) != 1)
-		stream->txerror = true;
+		stream->tx_error = true;
 }
 
 static void
@@ -106,11 +117,11 @@ hx_rx_frame(hx_stream_t stream)
 		uint8_t	b[4];
 		uint32_t w;
 	} u;
-	unsigned length = stream->frame_bytes;
+	unsigned length = stream->rx_frame_bytes;
 
 	/* reset the stream */
-	stream->frame_bytes = 0;
-	stream->escaped = false;
+	stream->rx_frame_bytes = 0;
+	stream->rx_escaped = false;
 
 	/* not a real frame - too short */
 	if (length < 4) {
@@ -123,11 +134,11 @@ hx_rx_frame(hx_stream_t stream)
 	length -= 4;
 
 	/* compute expected CRC */
-	u.w = crc32(&stream->buf[0], length);
+	u.w = crc32(&stream->rx_buf[0], length);
 
 	/* compare computed and actual CRC */
 	for (unsigned i = 0; i < 4; i++) {
-		if (u.b[i] != stream->buf[length + i]) {
+		if (u.b[i] != stream->rx_buf[length + i]) {
 			perf_count(stream->pc_rx_errors);
 			return 0;
 		}
@@ -135,7 +146,7 @@ hx_rx_frame(hx_stream_t stream)
 
 	/* frame is good */
 	perf_count(stream->pc_rx_frames);
-	stream->callback(stream->callback_arg, &stream->buf[0], length);
+	stream->rx_callback(stream->rx_callback_arg, &stream->rx_buf[0], length);
 	return 1;
 }
 
@@ -151,8 +162,8 @@ hx_stream_init(int fd,
 	if (stream != NULL) {
 		memset(stream, 0, sizeof(struct hx_stream));
 		stream->fd = fd;
-		stream->callback = callback;
-		stream->callback_arg = arg;
+		stream->rx_callback = callback;
+		stream->rx_callback_arg = arg;
 	}
 
 	return stream;
@@ -180,105 +191,135 @@ hx_stream_set_counters(hx_stream_t stream,
 	stream->pc_rx_errors = rx_errors;
 }
 
+void
+hx_stream_reset(hx_stream_t stream)
+{
+	stream->rx_frame_bytes = 0;
+	stream->rx_escaped = false;
+
+	stream->tx_buf = NULL;
+	stream->tx_resid = 0;
+	stream->tx_state = TX_IDLE;
+}
+
+int
+hx_stream_start(hx_stream_t stream,
+	       const void *data,
+	       size_t count)
+{
+	if (count > HX_STREAM_MAX_FRAME)
+		return -EINVAL;
+
+	stream->tx_buf = data;
+	stream->tx_resid = count;
+	stream->tx_state = TX_SEND_START;
+	stream->tx_crc = crc32(data, count);
+	return OK;
+}
+
+int
+hx_stream_send_next(hx_stream_t stream)
+{
+	int c;
+
+	/* sort out what we're going to send */
+	switch (stream->tx_state) {
+
+	case TX_SEND_START:
+		stream->tx_state = TX_SEND_DATA;
+		return FBO;
+
+	case TX_SEND_DATA:
+		c = *stream->tx_buf;
+
+		switch (c) {
+		case FBO:
+		case CEO:
+			stream->tx_state = TX_SENT_ESCAPE;
+			return CEO;
+		}
+		break;
+
+	case TX_SENT_ESCAPE:
+		c = *stream->tx_buf ^ 0x20;
+		stream->tx_state = TX_SEND_DATA;
+		break;
+
+	case TX_SEND_END:
+		stream->tx_state = TX_IDLE;
+		return FBO;
+
+	case TX_IDLE:
+	default:
+		return -1;
+	}
+
+	/* if we are here, we have consumed a byte from the buffer */
+	stream->tx_resid--;
+	stream->tx_buf++;
+
+	/* buffer exhausted */
+	if (stream->tx_resid == 0) {
+		uint8_t *pcrc = (uint8_t *)&stream->tx_crc;
+
+		/* was the buffer the frame CRC? */
+		if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) {
+			stream->tx_state = TX_SEND_END;
+		} else {
+			/* no, it was the payload - switch to sending the CRC */
+			stream->tx_buf = pcrc;
+			stream->tx_resid = sizeof(stream->tx_crc);
+		}
+	}
+	return c;
+}
+
 int
 hx_stream_send(hx_stream_t stream,
 	       const void *data,
 	       size_t count)
 {
-	union {
-		uint8_t	b[4];
-		uint32_t w;
-	} u;
-	const uint8_t *p = (const uint8_t *)data;
-	unsigned resid = count;
+	int result;
 
-	if (resid > HX_STREAM_MAX_FRAME)
-		return -EINVAL;
+	result = hx_start(stream, data, count);
+	if (result != OK)
+		return result;
 
-	/* start the frame */
-	hx_tx_raw(stream, FBO);
-
-	/* transmit the data */
-	while (resid--)
-		hx_tx_byte(stream, *p++);
-
-	/* compute the CRC */
-	u.w = crc32(data, count);
-
-	/* send the CRC */
-	p = &u.b[0];
-	resid = 4;
-
-	while (resid--)
-		hx_tx_byte(stream, *p++);
-
-	/* and the trailing frame separator */
-	hx_tx_raw(stream, FBO);
+	int c;
+	while ((c = hx_send_next(stream)) >= 0)
+		hx_tx_raw(stream, c);
 
 	/* check for transmit error */
-	if (stream->txerror) {
-		stream->txerror = false;
+	if (stream->tx_error) {
+		stream->tx_error = false;
 		return -EIO;
 	}
 
 	perf_count(stream->pc_tx_frames);
-	return 0;
+	return OK;
 }
 
-static bool
-hx_rx_char(hx_stream_t stream, uint8_t c)
+void
+hx_stream_rx(hx_stream_t stream, uint8_t c)
 {
 	/* frame end? */
 	if (c == FBO) {
 		hx_rx_frame(stream);
-		return true;
+		return;
 	}
 
 	/* escaped? */
-	if (stream->escaped) {
-		stream->escaped = false;
+	if (stream->rx_escaped) {
+		stream->rx_escaped = false;
 		c ^= 0x20;
 
 	} else if (c == CEO) {
-		/* now escaped, ignore the byte */
-		stream->escaped = true;
-		return false;
+		/* now rx_escaped, ignore the byte */
+		stream->rx_escaped = true;
+		return;
 	}
 
 	/* save for later */
-	if (stream->frame_bytes < sizeof(stream->buf))
-		stream->buf[stream->frame_bytes++] = c;
-
-	return false;
-}
-
-void
-hx_stream_rx_char(hx_stream_t stream, uint8_t c)
-{
-	hx_rx_char(stream, c);
-}
-
-int
-hx_stream_rx(hx_stream_t stream)
-{
-	uint16_t	buf[16];
-	ssize_t		len;
-
-	/* read bytes */
-	len = read(stream->fd, buf, sizeof(buf));
-	if (len <= 0) {
-
-		/* nonblocking stream and no data */
-		if (errno == EWOULDBLOCK)
-			return 0;
-
-		/* error or EOF */
-		return -errno;
-	}
-
-	/* process received characters */
-	for (int i = 0; i < len; i++)
-		hx_rx_char(stream, buf[i]);
-
-	return 0;
+	if (stream->rx_frame_bytes < sizeof(stream->rx_buf))
+		stream->rx_buf[stream->rx_frame_bytes++] = c;
 }

src/modules/systemlib/hx_stream.h

@@ -58,7 +58,8 @@ __BEGIN_DECLS
  * Allocate a new hx_stream object.
  *
  * @param fd		The file handle over which the protocol will
- *			communicate.
+ *			communicate, or -1 if the protocol will use
+ *			hx_stream_start/hx_stream_send_next.
  * @param callback	Called when a frame is received.
  * @param callback_arg	Passed to the callback.
  * @return		A handle to the stream, or NULL if memory could
@@ -80,6 +81,7 @@ __EXPORT extern void		hx_stream_free(hx_stream_t stream);
  *
  * Any counter may be set to NULL to disable counting that datum.
  *
+ * @param stream	A handle returned from hx_stream_init.
  * @param tx_frames	Counter for transmitted frames.
  * @param rx_frames	Counter for received frames.
  * @param rx_errors	Counter for short and corrupt received frames.
@@ -89,6 +91,44 @@ __EXPORT extern void		hx_stream_set_counters(hx_stream_t stream,
 		perf_counter_t rx_frames,
 		perf_counter_t rx_errors);
 
+/**
+ * Reset a stream.
+ *
+ * Forces the local stream state to idle.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ */
+__EXPORT extern void		hx_stream_reset(hx_stream_t stream);
+
+/**
+ * Prepare to send a frame.
+ *
+ * Use this in conjunction with hx_stream_send_next to 
+ * set the frame to be transmitted.
+ *
+ * Use hx_stream_send() to write to the stream fd directly.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @param data		Pointer to the data to send.
+ * @param count		The number of bytes to send.
+ * @return		Zero on success, -errno on error.
+ */
+__EXPORT extern int		hx_stream_start(hx_stream_t stream,
+		const void *data,
+		size_t count);
+
+/**
+ * Get the next byte to send for a stream.
+ *
+ * This requires that the stream be prepared for sending by
+ * calling hx_stream_start first.
+ *
+ * @param stream	A handle returned from hx_stream_init.
+ * @return		The byte to send, or -1 if there is 
+ *			nothing left to send.
+ */
+__EXPORT extern int		hx_stream_send_next(hx_stream_t stream);
+
 /**
  * Send a frame.
  *
@@ -114,25 +154,9 @@ __EXPORT extern int		hx_stream_send(hx_stream_t stream,
  * @param stream	A handle returned from hx_stream_init.
  * @param c		The character to process.
  */
-__EXPORT extern void		hx_stream_rx_char(hx_stream_t stream,
+__EXPORT extern void		hx_stream_rx(hx_stream_t stream,
 		uint8_t c);
 
-/**
- * Handle received bytes from the stream.
- *
- * Note that this interface should only be used with blocking streams
- * when it is OK for the call to block until a frame is received.
- *
- * When used with a non-blocking stream, it will typically return
- * immediately, or after handling a received frame.
- *
- * @param stream	A handle returned from hx_stream_init.
- * @return		-errno on error, nonzero if a frame
- *			has been received, or if not enough
- *			bytes are available to complete a frame.
- */
-__EXPORT extern int		hx_stream_rx(hx_stream_t stream);
-
 __END_DECLS
 
 #endif