PX4-Autopilot/src/modules/systemlib/hx_stream.c

/****************************************************************************
 *
 *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file hx_stream.c
 *
 * A simple serial line framing protocol based on HDLC
 * with 32-bit CRC protection.
 */

#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <crc32.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>

#include "perf_counter.h"

#include "hx_stream.h"


struct hx_stream {
	/* 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;
	bool			tx_error;
	const 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;
	perf_counter_t		pc_rx_errors;
};

/*
 * Protocol magic numbers, straight out of HDLC.
 */
#define FBO	0x7e	/**< Frame Boundary Octet */
#define CEO	0x7c	/**< Control Escape Octet */

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 void
hx_tx_raw(hx_stream_t stream, uint8_t c)
{
	if (write(stream->fd, &c, 1) != 1) {
		stream->tx_error = true;
	}
}

static int
hx_rx_frame(hx_stream_t stream)
{
	union {
		uint8_t	b[4];
		uint32_t w;
	} u;
	unsigned length = stream->rx_frame_bytes;

	/* reset the stream */
	stream->rx_frame_bytes = 0;
	stream->rx_escaped = false;

	/* not a real frame - too short */
	if (length < 4) {
		if (length > 1) {
			perf_count(stream->pc_rx_errors);
		}

		return 0;
	}

	length -= 4;

	/* compute expected CRC */
	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->rx_buf[length + i]) {
			perf_count(stream->pc_rx_errors);
			return 0;
		}
	}

	/* frame is good */
	perf_count(stream->pc_rx_frames);
	stream->rx_callback(stream->rx_callback_arg, &stream->rx_buf[0], length);
	return 1;
}

hx_stream_t
hx_stream_init(int fd,
	       hx_stream_rx_callback callback,
	       void *arg)
{
	hx_stream_t stream;

	stream = (hx_stream_t)malloc(sizeof(struct hx_stream));

	if (stream != NULL) {
		memset(stream, 0, sizeof(struct hx_stream));
		stream->fd = fd;
		stream->rx_callback = callback;
		stream->rx_callback_arg = arg;
	}

	return stream;
}

void
hx_stream_free(hx_stream_t stream)
{
	/* free perf counters (OK if they are NULL) */
	perf_free(stream->pc_tx_frames);
	perf_free(stream->pc_rx_frames);
	perf_free(stream->pc_rx_errors);

	free(stream);
}

void
hx_stream_set_counters(hx_stream_t stream,
		       perf_counter_t tx_frames,
		       perf_counter_t rx_frames,
		       perf_counter_t rx_errors)
{
	stream->pc_tx_frames = tx_frames;
	stream->pc_rx_frames = rx_frames;
	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)
{
	int result;

	result = hx_stream_start(stream, data, count);

	if (result != OK) {
		return result;
	}

	int c;

	while ((c = hx_stream_send_next(stream)) >= 0) {
		hx_tx_raw(stream, c);
	}

	/* check for transmit error */
	if (stream->tx_error) {
		stream->tx_error = false;
		return -EIO;
	}

	perf_count(stream->pc_tx_frames);
	return OK;
}

void
hx_stream_rx(hx_stream_t stream, uint8_t c)
{
	/* frame end? */
	if (c == FBO) {
		hx_rx_frame(stream);
		return;
	}

	/* escaped? */
	if (stream->rx_escaped) {
		stream->rx_escaped = false;
		c ^= 0x20;

	} else if (c == CEO) {
		/* now rx_escaped, ignore the byte */
		stream->rx_escaped = true;
		return;
	}

	/* save for later */
	if (stream->rx_frame_bytes < sizeof(stream->rx_buf)) {
		stream->rx_buf[stream->rx_frame_bytes++] = c;
	}
}