moved all third-party lib to separate folder

thirdparty/libcsp/src/interfaces/CMakeLists.txt

@@ -0,0 +1,7 @@
+target_sources(${LIB_CSP_NAME} PRIVATE
+	csp_if_can_pbuf.c
+	csp_if_can.c
+	csp_if_i2c.c
+	csp_if_kiss.c
+	csp_if_lo.c
+)

thirdparty/libcsp/src/interfaces/csp_if_can.c

@@ -0,0 +1,279 @@
+/*
+Cubesat Space Protocol - A small network-layer protocol designed for Cubesats
+Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com)
+Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk)
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+/* CAN frames contains at most 8 bytes of data, so in order to transmit CSP
+ * packets larger than this, a fragmentation protocol is required. The CAN
+ * Fragmentation Protocol (CFP) header is designed to match the 29 bit CAN
+ * identifier.
+ *
+ * The CAN identifier is divided in these fields:
+ * src:          5 bits
+ * dst:          5 bits
+ * type:         1 bit
+ * remain:       8 bits
+ * identifier:   10 bits
+ *
+ * Source and Destination addresses must match the CSP packet. The type field
+ * is used to distinguish the first and subsequent frames in a fragmented CSP
+ * packet. Type is BEGIN (0) for the first fragment and MORE (1) for all other
+ * fragments. Remain indicates number of remaining fragments, and must be
+ * decremented by one for each fragment sent. The identifier field serves the
+ * same purpose as in the Internet Protocol, and should be an auto incrementing
+ * integer to uniquely separate sessions.
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <inttypes.h>
+
+#include <csp/csp.h>
+#include <csp/csp_interface.h>
+#include <csp/csp_endian.h>
+#include <csp/interfaces/csp_if_can.h>
+
+#include <csp/arch/csp_semaphore.h>
+#include <csp/arch/csp_time.h>
+#include <csp/arch/csp_queue.h>
+#include <csp/arch/csp_thread.h>
+
+#include "csp_if_can_pbuf.h"
+
+/* CFP Frame Types */
+enum cfp_frame_t {
+	CFP_BEGIN = 0,
+	CFP_MORE = 1
+};
+
+int csp_can_rx(csp_iface_t *interface, uint32_t id, const uint8_t *data, uint8_t dlc, CSP_BASE_TYPE *task_woken)
+{
+	csp_can_pbuf_element_t *buf;
+	uint8_t offset;
+
+	/* Random packet loss */
+#if 0
+	int random = rand();
+	if (random < RAND_MAX * 0.00005) {
+		csp_log_warn("Dropping frame");
+		return;
+	}
+#endif
+
+	/* Bind incoming frame to a packet buffer */
+	buf = csp_can_pbuf_find(id, CFP_ID_CONN_MASK);
+
+	/* Check returned buffer */
+	if (buf == NULL) {
+		if (CFP_TYPE(id) == CFP_BEGIN) {
+			buf = csp_can_pbuf_new(id, task_woken);
+			if (buf == NULL) {
+				//csp_log_warn("No available packet buffer for CAN");
+				interface->rx_error++;
+				return CSP_ERR_NOMEM;
+			}
+		} else {
+			//csp_log_warn("Out of order id 0x%X remain %u", CFP_ID(id), CFP_REMAIN(id));
+			interface->frame++;
+			return CSP_ERR_INVAL;
+		}
+	}
+
+	/* Reset frame data offset */
+	offset = 0;
+
+	switch (CFP_TYPE(id)) {
+
+	case CFP_BEGIN:
+
+		/* Discard packet if DLC is less than CSP id + CSP length fields */
+		if (dlc < sizeof(csp_id_t) + sizeof(uint16_t)) {
+			//csp_log_warn("Short BEGIN frame received");
+			interface->frame++;
+			csp_can_pbuf_free(buf, task_woken);
+			break;
+		}
+
+		/* Check for incomplete frame */
+		if (buf->packet != NULL) {
+			/* Reuse the buffer */
+			//csp_log_warn("Incomplete frame");
+			interface->frame++;
+		} else {
+			/* Allocate memory for frame */
+			if (task_woken == NULL) {
+				buf->packet = csp_buffer_get(interface->mtu);
+			} else {
+				buf->packet = csp_buffer_get_isr(interface->mtu);
+			}
+			if (buf->packet == NULL) {
+				//csp_log_error("Failed to get buffer for CSP_BEGIN packet");
+				interface->frame++;
+				csp_can_pbuf_free(buf, task_woken);
+				break;
+			}
+		}
+
+		/* Copy CSP identifier and length*/
+		memcpy(&(buf->packet->id), data, sizeof(csp_id_t));
+		buf->packet->id.ext = csp_ntoh32(buf->packet->id.ext);
+		memcpy(&(buf->packet->length), data + sizeof(csp_id_t), sizeof(uint16_t));
+		buf->packet->length = csp_ntoh16(buf->packet->length);
+
+		/* Reset RX count */
+		buf->rx_count = 0;
+
+		/* Set offset to prevent CSP header from being copied to CSP data */
+		offset = sizeof(csp_id_t) + sizeof(uint16_t);
+
+		/* Set remain field - increment to include begin packet */
+		buf->remain = CFP_REMAIN(id) + 1;
+
+		/* FALLTHROUGH */
+
+	case CFP_MORE:
+
+		/* Check 'remain' field match */
+		if (CFP_REMAIN(id) != buf->remain - 1) {
+			//csp_log_error("CAN frame lost in CSP packet");
+			csp_can_pbuf_free(buf, task_woken);
+			interface->frame++;
+			break;
+		}
+
+		/* Decrement remaining frames */
+		buf->remain--;
+
+		/* Check for overflow */
+		if ((buf->rx_count + dlc - offset) > buf->packet->length) {
+			//csp_log_error("RX buffer overflow");
+			interface->frame++;
+			csp_can_pbuf_free(buf, task_woken);
+			break;
+		}
+
+		/* Copy dlc bytes into buffer */
+		memcpy(&buf->packet->data[buf->rx_count], data + offset, dlc - offset);
+		buf->rx_count += dlc - offset;
+
+		/* Check if more data is expected */
+		if (buf->rx_count != buf->packet->length)
+			break;
+
+		/* Data is available */
+		csp_qfifo_write(buf->packet, interface, task_woken);
+
+		/* Drop packet buffer reference */
+		buf->packet = NULL;
+
+		/* Free packet buffer */
+		csp_can_pbuf_free(buf, task_woken);
+
+		break;
+
+	default:
+		//csp_log_warn("Received unknown CFP message type");
+		csp_can_pbuf_free(buf, task_woken);
+		break;
+
+	}
+
+	return CSP_ERR_NONE;
+}
+
+int csp_can_tx(csp_iface_t *interface, csp_packet_t *packet, uint32_t timeout)
+{
+
+	/* CFP Identification number */
+	static volatile int csp_can_frame_id = 0;
+
+	/* Get local copy of the static frameid */
+	int ident = csp_can_frame_id++;
+
+	uint16_t tx_count;
+	uint8_t bytes, overhead, avail, dest;
+	uint8_t frame_buf[8];
+
+	/* Calculate overhead */
+	overhead = sizeof(csp_id_t) + sizeof(uint16_t);
+
+	/* Insert destination node mac address into the CFP destination field */
+	dest = csp_rtable_find_mac(packet->id.dst);
+	if (dest == CSP_NODE_MAC)
+		dest = packet->id.dst;
+
+	/* Create CAN identifier */
+	uint32_t id = 0;
+	id |= CFP_MAKE_SRC(packet->id.src);
+	id |= CFP_MAKE_DST(dest);
+	id |= CFP_MAKE_ID(ident);
+	id |= CFP_MAKE_TYPE(CFP_BEGIN);
+	id |= CFP_MAKE_REMAIN((packet->length + overhead - 1) / 8);
+
+	/* Calculate first frame data bytes */
+	avail = 8 - overhead;
+	bytes = (packet->length <= avail) ? packet->length : avail;
+
+	/* Copy CSP headers and data */
+	uint32_t csp_id_be = csp_hton32(packet->id.ext);
+	uint16_t csp_length_be = csp_hton16(packet->length);
+
+	memcpy(frame_buf, &csp_id_be, sizeof(csp_id_be));
+	memcpy(frame_buf + sizeof(csp_id_be), &csp_length_be, sizeof(csp_length_be));
+	memcpy(frame_buf + overhead, packet->data, bytes);
+
+	/* Increment tx counter */
+	tx_count = bytes;
+
+	/* Send first frame */
+	if (csp_can_tx_frame(interface, id, frame_buf, overhead + bytes)) {
+		//csp_log_warn("Failed to send CAN frame in csp_tx_can");
+		interface->tx_error++;
+		return CSP_ERR_DRIVER;
+	}
+
+	/* Send next frames if not complete */
+	while (tx_count < packet->length) {
+		/* Calculate frame data bytes */
+		bytes = (packet->length - tx_count >= 8) ? 8 : packet->length - tx_count;
+
+		/* Prepare identifier */
+		id = 0;
+		id |= CFP_MAKE_SRC(packet->id.src);
+		id |= CFP_MAKE_DST(dest);
+		id |= CFP_MAKE_ID(ident);
+		id |= CFP_MAKE_TYPE(CFP_MORE);
+		id |= CFP_MAKE_REMAIN((packet->length - tx_count - bytes + 7) / 8);
+
+		/* Increment tx counter */
+		tx_count += bytes;
+
+		/* Send frame */
+		if (csp_can_tx_frame(interface, id, packet->data + tx_count - bytes, bytes)) {
+			//csp_log_warn("Failed to send CAN frame in Tx callback");
+			interface->tx_error++;
+			return CSP_ERR_DRIVER;
+		}
+	}
+
+	csp_buffer_free(packet);
+
+	return CSP_ERR_NONE;
+}

thirdparty/libcsp/src/interfaces/csp_if_can_pbuf.c

@@ -0,0 +1,77 @@
+/*
+ * csp_if_can_pbuf.c
+ *
+ *  Created on: Feb 3, 2017
+ *      Author: johan
+ */
+
+#include <csp/arch/csp_time.h>
+#include "csp_if_can_pbuf.h"
+
+/* Number of packet buffer elements */
+#define PBUF_ELEMENTS		CSP_CONN_MAX
+
+/* Buffer element timeout in ms */
+#define PBUF_TIMEOUT_MS		1000
+
+static csp_can_pbuf_element_t csp_can_pbuf[PBUF_ELEMENTS] = {};
+
+int csp_can_pbuf_free(csp_can_pbuf_element_t *buf, CSP_BASE_TYPE *task_woken)
+{
+	/* Free CSP packet */
+	if (buf->packet != NULL) {
+		if (task_woken == NULL) {
+			csp_buffer_free(buf->packet);
+		} else {
+			csp_buffer_free_isr(buf->packet);
+		}
+	}
+
+	/* Mark buffer element free */
+	buf->packet = NULL;
+	buf->rx_count = 0;
+	buf->cfpid = 0;
+	buf->last_used = 0;
+	buf->remain = 0;
+	buf->state = BUF_FREE;
+
+	return CSP_ERR_NONE;
+}
+
+csp_can_pbuf_element_t *csp_can_pbuf_new(uint32_t id, CSP_BASE_TYPE *task_woken)
+{
+	uint32_t now = csp_get_ms();
+
+	for (int i = 0; i < PBUF_ELEMENTS; i++) {
+
+		/* Perform cleanup in used pbufs */
+		if (csp_can_pbuf[i].state == BUF_USED) {
+			if (now - csp_can_pbuf[i].last_used > PBUF_TIMEOUT_MS)
+				csp_can_pbuf_free(&csp_can_pbuf[i], task_woken);
+		}
+
+		if (csp_can_pbuf[i].state == BUF_FREE) {
+			csp_can_pbuf[i].state = BUF_USED;
+			csp_can_pbuf[i].cfpid = id;
+			csp_can_pbuf[i].remain = 0;
+			csp_can_pbuf[i].last_used = now;
+			return &csp_can_pbuf[i];
+		}
+
+	}
+
+	return NULL;
+
+}
+
+csp_can_pbuf_element_t *csp_can_pbuf_find(uint32_t id, uint32_t mask)
+{
+	for (int i = 0; i < PBUF_ELEMENTS; i++) {
+		if ((csp_can_pbuf[i].state == BUF_USED) && ((csp_can_pbuf[i].cfpid & mask) == (id & mask))) {
+			csp_can_pbuf[i].last_used = csp_get_ms();
+			return &csp_can_pbuf[i];
+		}
+	}
+	return NULL;
+}
+

thirdparty/libcsp/src/interfaces/csp_if_can_pbuf.h

@@ -0,0 +1,31 @@
+/*
+ * csp_if_can_pbuf.h
+ *
+ *  Created on: Feb 3, 2017
+ *      Author: johan
+ */
+
+#ifndef LIB_CSP_SRC_INTERFACES_CSP_IF_CAN_PBUF_H_
+#define LIB_CSP_SRC_INTERFACES_CSP_IF_CAN_PBUF_H_
+
+/* Packet buffers */
+typedef enum {
+	BUF_FREE = 0,			/* Buffer element free */
+	BUF_USED = 1,			/* Buffer element used */
+} csp_can_pbuf_state_t;
+
+typedef struct {
+	uint16_t rx_count;		/* Received bytes */
+	uint32_t remain;		/* Remaining packets */
+	uint32_t cfpid;			/* Connection CFP identification number */
+	csp_packet_t *packet;		/* Pointer to packet buffer */
+	csp_can_pbuf_state_t state;	/* Element state */
+	uint32_t last_used;		/* Timestamp in ms for last use of buffer */
+} csp_can_pbuf_element_t;
+
+int csp_can_pbuf_free(csp_can_pbuf_element_t *buf, CSP_BASE_TYPE *task_woken);
+csp_can_pbuf_element_t *csp_can_pbuf_new(uint32_t id, CSP_BASE_TYPE *task_woken);
+csp_can_pbuf_element_t *csp_can_pbuf_find(uint32_t id, uint32_t mask);
+void csp_can_pbuf_cleanup(CSP_BASE_TYPE *task_woken);
+
+#endif /* LIB_CSP_SRC_INTERFACES_CSP_IF_CAN_PBUF_H_ */

thirdparty/libcsp/src/interfaces/csp_if_i2c.c

@@ -0,0 +1,116 @@
+/*
+Cubesat Space Protocol - A small network-layer protocol designed for Cubesats
+Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com)
+Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk)
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#include <inttypes.h>
+#include <stdint.h>
+
+#include <csp/csp.h>
+#include <csp/csp_endian.h>
+#include <csp/csp_interface.h>
+#include <csp/csp_error.h>
+#include <csp/interfaces/csp_if_i2c.h>
+#include <csp/drivers/i2c.h>
+
+static int csp_i2c_handle = 0;
+
+int csp_i2c_tx(csp_iface_t * interface, csp_packet_t * packet, uint32_t timeout) {
+
+	/* Cast the CSP packet buffer into an i2c frame */
+	i2c_frame_t * frame = (i2c_frame_t *) packet;
+
+	/* Insert destination node into the i2c destination field */
+	if (csp_rtable_find_mac(packet->id.dst) == CSP_NODE_MAC) {
+		frame->dest = packet->id.dst;
+	} else {
+		frame->dest = csp_rtable_find_mac(packet->id.dst);
+	}
+
+	/* Save the outgoing id in the buffer */
+	packet->id.ext = csp_hton32(packet->id.ext);
+
+	/* Add the CSP header to the I2C length field */
+	frame->len += sizeof(packet->id);
+	frame->len_rx = 0;
+
+	/* Some I2C drivers support X number of retries
+	 * CSP don't care about this. If it doesn't work the first
+	 * time, don'y use time on it.
+	 */
+	frame->retries = 0;
+
+	/* enqueue the frame */
+	if (i2c_send(csp_i2c_handle, frame, timeout) != E_NO_ERR)
+		return CSP_ERR_DRIVER;
+
+	return CSP_ERR_NONE;
+
+}
+
+/**
+ * When a frame is received, cast it to a csp_packet
+ * and send it directly to the CSP new packet function.
+ * Context: ISR only
+ * @param frame
+ */
+void csp_i2c_rx(i2c_frame_t * frame, void * pxTaskWoken) {
+
+	static csp_packet_t * packet;
+
+	/* Validate input */
+	if (frame == NULL)
+		return;
+
+	if ((frame->len < 4) || (frame->len > I2C_MTU)) {
+		csp_if_i2c.frame++;
+		csp_buffer_free_isr(frame);
+		return;
+	}
+
+	/* Strip the CSP header off the length field before converting to CSP packet */
+	frame->len -= sizeof(csp_id_t);
+
+	/* Convert the packet from network to host order */
+	packet = (csp_packet_t *) frame;
+	packet->id.ext = csp_ntoh32(packet->id.ext);
+
+	/* Receive the packet in CSP */
+	csp_qfifo_write(packet, &csp_if_i2c, pxTaskWoken);
+
+}
+
+int csp_i2c_init(uint8_t addr, int handle, int speed) {
+
+	/* Create i2c_handle */
+	csp_i2c_handle = handle;
+	if (i2c_init(csp_i2c_handle, I2C_MASTER, addr, speed, 10, 10, csp_i2c_rx) != E_NO_ERR)
+		return CSP_ERR_DRIVER;
+
+	/* Register interface */
+	csp_iflist_add(&csp_if_i2c);
+
+	return CSP_ERR_NONE;
+
+}
+
+/** Interface definition */
+csp_iface_t csp_if_i2c = {
+	.name = "I2C",
+	.nexthop = csp_i2c_tx,
+};

thirdparty/libcsp/src/interfaces/csp_if_kiss.c

@@ -0,0 +1,260 @@
+/*
+Cubesat Space Protocol - A small network-layer protocol designed for Cubesats
+Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com)
+Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk)
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <csp/csp.h>
+#include <csp/csp_endian.h>
+#include <csp/csp_platform.h>
+#include <csp/csp_interface.h>
+#include <csp/interfaces/csp_if_kiss.h>
+#include <csp/arch/csp_semaphore.h>
+#include <csp/csp_crc32.h>
+
+#define KISS_MTU				256
+
+#define FEND  					0xC0
+#define FESC  					0xDB
+#define TFEND 					0xDC
+#define TFESC 					0xDD
+
+#define TNC_DATA				0x00
+#define TNC_SET_HARDWARE		0x06
+#define TNC_RETURN				0xFF
+
+static int kiss_lock_init = 0;
+static csp_bin_sem_handle_t kiss_lock;
+
+/* Send a CSP packet over the KISS RS232 protocol */
+static int csp_kiss_tx(csp_iface_t * interface, csp_packet_t * packet, uint32_t timeout) {
+
+	if (interface == NULL || interface->driver == NULL)
+		return CSP_ERR_DRIVER;
+
+	/* Add CRC32 checksum */
+	csp_crc32_append(packet, false);
+
+	/* Save the outgoing id in the buffer */
+	packet->id.ext = csp_hton32(packet->id.ext);
+	packet->length += sizeof(packet->id.ext);
+
+	/* Lock */
+	csp_bin_sem_wait(&kiss_lock, 1000);
+
+	/* Transmit data */
+	csp_kiss_handle_t * driver = interface->driver;
+	driver->kiss_putc(FEND);
+	driver->kiss_putc(TNC_DATA);
+	for (unsigned int i = 0; i < packet->length; i++) {
+		if (((unsigned char *) &packet->id.ext)[i] == FEND) {
+			((unsigned char *) &packet->id.ext)[i] = TFEND;
+			driver->kiss_putc(FESC);
+		} else if (((unsigned char *) &packet->id.ext)[i] == FESC) {
+			((unsigned char *) &packet->id.ext)[i] = TFESC;
+			driver->kiss_putc(FESC);
+		}
+		driver->kiss_putc(((unsigned char *) &packet->id.ext)[i]);
+	}
+	driver->kiss_putc(FEND);
+
+	/* Free data */
+	csp_buffer_free(packet);
+
+	/* Unlock */
+	csp_bin_sem_post(&kiss_lock);
+
+	return CSP_ERR_NONE;
+}
+
+/**
+ * When a frame is received, decode the kiss-stuff
+ * and eventually send it directly to the CSP new packet function.
+ */
+void csp_kiss_rx(csp_iface_t * interface, uint8_t * buf, int len, void * pxTaskWoken) {
+
+	/* Driver handle */
+	csp_kiss_handle_t * driver = interface->driver;
+
+	while (len--) {
+
+		/* Input */
+		unsigned char inputbyte = *buf++;
+
+		/* If packet was too long */
+		if (driver->rx_length > interface->mtu) {
+			//csp_log_warn("KISS RX overflow");
+			interface->rx_error++;
+			driver->rx_mode = KISS_MODE_NOT_STARTED;
+			driver->rx_length = 0;
+		}
+
+		switch (driver->rx_mode) {
+
+		case KISS_MODE_NOT_STARTED:
+
+			/* Send normal chars back to usart driver */
+			if (inputbyte != FEND) {
+				if (driver->kiss_discard != NULL)
+					driver->kiss_discard(inputbyte, pxTaskWoken);
+				break;
+			}
+
+			/* Try to allocate new buffer */
+			if (driver->rx_packet == NULL) {
+				if (pxTaskWoken == NULL) {
+					driver->rx_packet = csp_buffer_get(interface->mtu);
+				} else {
+					driver->rx_packet = csp_buffer_get_isr(interface->mtu);
+				}
+			}
+
+			/* If no more memory, skip frame */
+			if (driver->rx_packet == NULL) {
+				driver->rx_mode = KISS_MODE_SKIP_FRAME;
+				break;
+			}
+
+			/* Start transfer */
+			driver->rx_length = 0;
+			driver->rx_mode = KISS_MODE_STARTED;
+			driver->rx_first = 1;
+			break;
+
+		case KISS_MODE_STARTED:
+
+			/* Escape char */
+			if (inputbyte == FESC) {
+				driver->rx_mode = KISS_MODE_ESCAPED;
+				break;
+			}
+
+			/* End Char */
+			if (inputbyte == FEND) {
+
+				/* Accept message */
+				if (driver->rx_length > 0) {
+
+					/* Check for valid length */
+					if (driver->rx_length < CSP_HEADER_LENGTH + sizeof(uint32_t)) {
+						//csp_log_warn("KISS short frame skipped, len: %u", driver->rx_length);
+						interface->rx_error++;
+						driver->rx_mode = KISS_MODE_NOT_STARTED;
+						break;
+					}
+
+					/* Count received frame */
+					interface->frame++;
+
+					/* The CSP packet length is without the header */
+					driver->rx_packet->length = driver->rx_length - CSP_HEADER_LENGTH;
+
+					/* Convert the packet from network to host order */
+					driver->rx_packet->id.ext = csp_ntoh32(driver->rx_packet->id.ext);
+
+					/* Validate CRC */
+					if (csp_crc32_verify(driver->rx_packet, false) != CSP_ERR_NONE) {
+						//csp_log_warn("KISS invalid crc frame skipped, len: %u", driver->rx_packet->length);
+						interface->rx_error++;
+						driver->rx_mode = KISS_MODE_NOT_STARTED;
+						break;
+					}
+
+					/* Send back into CSP, notice calling from task so last argument must be NULL! */
+					csp_qfifo_write(driver->rx_packet, interface, pxTaskWoken);
+					driver->rx_packet = NULL;
+					driver->rx_mode = KISS_MODE_NOT_STARTED;
+					break;
+
+				}
+
+				/* Break after the end char */
+				break;
+			}
+
+			/* Skip the first char after FEND which is TNC_DATA (0x00) */
+			if (driver->rx_first) {
+				driver->rx_first = 0;
+				break;
+			}
+
+			/* Valid data char */
+			((char *) &driver->rx_packet->id.ext)[driver->rx_length++] = inputbyte;
+
+			break;
+
+		case KISS_MODE_ESCAPED:
+
+			/* Escaped escape char */
+			if (inputbyte == TFESC)
+				((char *) &driver->rx_packet->id.ext)[driver->rx_length++] = FESC;
+
+			/* Escaped fend char */
+			if (inputbyte == TFEND)
+				((char *) &driver->rx_packet->id.ext)[driver->rx_length++] = FEND;
+
+			/* Go back to started mode */
+			driver->rx_mode = KISS_MODE_STARTED;
+			break;
+
+		case KISS_MODE_SKIP_FRAME:
+
+			/* Just wait for end char */
+			if (inputbyte == FEND)
+				driver->rx_mode = KISS_MODE_NOT_STARTED;
+
+			break;
+
+		}
+
+	}
+
+}
+
+void csp_kiss_init(csp_iface_t * csp_iface, csp_kiss_handle_t * csp_kiss_handle, csp_kiss_putc_f kiss_putc_f, csp_kiss_discard_f kiss_discard_f, const char * name) {
+
+	/* Init lock only once */
+	if (kiss_lock_init == 0) {
+		csp_bin_sem_create(&kiss_lock);
+		kiss_lock_init = 1;
+	}
+
+	/* Register device handle as member of interface */
+	csp_iface->driver = csp_kiss_handle;
+	csp_kiss_handle->kiss_discard = kiss_discard_f;
+	csp_kiss_handle->kiss_putc = kiss_putc_f;
+	csp_kiss_handle->rx_packet = NULL;
+	csp_kiss_handle->rx_mode = KISS_MODE_NOT_STARTED;
+
+        /* Set default MTU if not given */
+        if (csp_iface->mtu == 0) {
+            csp_iface->mtu = KISS_MTU;
+        }
+
+	/* Setup other mandatories */
+	csp_iface->nexthop = csp_kiss_tx;
+	csp_iface->name = name;
+
+	/* Regsiter interface */
+	csp_iflist_add(csp_iface);
+
+}

thirdparty/libcsp/src/interfaces/csp_if_lo.c

@@ -0,0 +1,61 @@
+/*
+Cubesat Space Protocol - A small network-layer protocol designed for Cubesats
+Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com)
+Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk) 
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+/* CSP includes */
+#include <csp/csp.h>
+#include <csp/csp_platform.h>
+#include <csp/csp_interface.h>
+#include <csp/interfaces/csp_if_lo.h>
+
+#include <csp/arch/csp_semaphore.h>
+#include <csp/arch/csp_queue.h>
+
+#include "../csp_route.h"
+
+/**
+ * Loopback interface transmit function
+ * @param packet Packet to transmit
+ * @param timeout Timout in ms
+ * @return 1 if packet was successfully transmitted, 0 on error
+ */
+static int csp_lo_tx(csp_iface_t * interface, csp_packet_t * packet, uint32_t timeout) {
+
+	/* Drop packet silently if not destined for us. This allows
+	 * blackhole routing addresses by setting their nexthop to
+	 * the loopback interface.
+	 */
+	if (packet->id.dst != csp_get_address()) {
+		/* Consume and drop packet */
+		csp_buffer_free(packet);
+		return CSP_ERR_NONE;
+	}
+
+	/* Send back into CSP, notice calling from task so last argument must be NULL! */
+	csp_qfifo_write(packet, &csp_if_lo, NULL);
+
+	return CSP_ERR_NONE;
+
+}
+
+/* Interface definition */
+csp_iface_t csp_if_lo = {
+	.name = "LOOP",
+	.nexthop = csp_lo_tx,
+};