moved some files

thirdparty/libcsp/src/CMakeLists.txt

@@ -0,0 +1,27 @@
+target_sources(${LIB_CSP_NAME} PRIVATE
+	csp_bridge.c
+	csp_buffer.c
+	csp_conn.c
+	csp_crc32.c
+	csp_debug.c
+	csp_dedup.c
+	csp_endian.c
+	csp_hex_dump.c
+	csp_iflist.c
+	csp_io.c
+	csp_port.c
+	csp_promisc.c
+	csp_qfifo.c
+	csp_route.c
+	csp_service_handler.c
+	csp_services.c
+	csp_sfp.c
+)
+
+add_subdirectory(drivers)
+add_subdirectory(crypto)
+add_subdirectory(interfaces)
+add_subdirectory(rtable)
+add_subdirectory(transport)
+add_subdirectory(arch)
+

thirdparty/libcsp/src/arch/CMakeLists.txt

@@ -0,0 +1,3 @@
+add_subdirectory(posix)
+
+

thirdparty/libcsp/src/arch/freertos/csp_malloc.c

@@ -0,0 +1,33 @@
+/*
+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 <stdint.h>
+#include <csp/arch/csp_malloc.h>
+
+/* FreeRTOS includes */
+#include <FreeRTOS.h>
+
+void * csp_malloc(size_t size) {
+	return pvPortMalloc(size);
+}
+
+void csp_free(void *ptr) {
+	vPortFree(ptr);
+}

thirdparty/libcsp/src/arch/freertos/csp_queue.c

@@ -0,0 +1,66 @@
+/*
+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 <stdint.h>
+
+/* FreeRTOS includes */
+#include <FreeRTOS.h>
+#include <queue.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+
+#include <csp/arch/csp_queue.h>
+
+csp_queue_handle_t csp_queue_create(int length, size_t item_size) {
+	return xQueueCreate(length, item_size);
+}
+
+void csp_queue_remove(csp_queue_handle_t queue) {
+	vQueueDelete(queue);
+}
+
+int csp_queue_enqueue(csp_queue_handle_t handle, void * value, uint32_t timeout) {
+	if (timeout != CSP_MAX_DELAY)
+		timeout = timeout / portTICK_RATE_MS;
+	return xQueueSendToBack(handle, value, timeout);
+}
+
+int csp_queue_enqueue_isr(csp_queue_handle_t handle, void * value, CSP_BASE_TYPE * task_woken) {
+	return xQueueSendToBackFromISR(handle, value, task_woken);
+}
+
+int csp_queue_dequeue(csp_queue_handle_t handle, void * buf, uint32_t timeout) {
+	if (timeout != CSP_MAX_DELAY)
+		timeout = timeout / portTICK_RATE_MS;
+	return xQueueReceive(handle, buf, timeout);
+}
+
+int csp_queue_dequeue_isr(csp_queue_handle_t handle, void * buf, CSP_BASE_TYPE * task_woken) {
+	return xQueueReceiveFromISR(handle, buf, task_woken);
+}
+
+int csp_queue_size(csp_queue_handle_t handle) {
+	return uxQueueMessagesWaiting(handle);
+}
+
+int csp_queue_size_isr(csp_queue_handle_t handle) {
+	return uxQueueMessagesWaitingFromISR(handle);
+}

thirdparty/libcsp/src/arch/freertos/csp_semaphore.c

@@ -0,0 +1,96 @@
+/*
+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 <stdio.h>
+#include <stdint.h>
+
+/* FreeRTOS includes */
+#include <FreeRTOS.h>
+#include <semphr.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+
+#include <csp/arch/csp_malloc.h>
+#include <csp/arch/csp_semaphore.h>
+#include <csp/arch/csp_queue.h>
+
+int csp_mutex_create(csp_mutex_t * mutex) {
+	*mutex = xSemaphoreCreateMutex();
+	if (*mutex) {
+		return CSP_SEMAPHORE_OK;
+	} else {
+		return CSP_SEMAPHORE_ERROR;
+	}
+}
+
+int csp_mutex_remove(csp_mutex_t * mutex) {
+	return csp_bin_sem_remove(mutex);
+}
+
+int csp_mutex_lock(csp_mutex_t * mutex, uint32_t timeout) {
+	return csp_bin_sem_wait(mutex, timeout);
+}
+
+int csp_mutex_unlock(csp_mutex_t * mutex) {
+	return csp_bin_sem_post(mutex);
+}
+
+int csp_bin_sem_create(csp_bin_sem_handle_t * sem) {
+	vSemaphoreCreateBinary(*sem);
+	return CSP_SEMAPHORE_OK;
+}
+
+int csp_bin_sem_remove(csp_bin_sem_handle_t * sem) {
+	if ((sem != NULL) && (*sem != NULL)) {
+		csp_queue_remove(*sem);
+	}
+	return CSP_SEMAPHORE_OK;
+}
+
+int csp_bin_sem_wait(csp_bin_sem_handle_t * sem, uint32_t timeout) {
+	csp_log_lock("Wait: %p", sem);
+	if (timeout != CSP_MAX_DELAY)
+		timeout = timeout / portTICK_RATE_MS;
+	if (xSemaphoreTake(*sem, timeout) == pdPASS) {
+		return CSP_SEMAPHORE_OK;
+	} else {
+		return CSP_SEMAPHORE_ERROR;
+	}
+}
+
+int csp_bin_sem_post(csp_bin_sem_handle_t * sem) {
+	csp_log_lock("Post: %p", sem);
+	if (xSemaphoreGive(*sem) == pdPASS) {
+		return CSP_SEMAPHORE_OK;
+	} else {
+		return CSP_SEMAPHORE_ERROR;
+	}
+}
+
+int csp_bin_sem_post_isr(csp_bin_sem_handle_t * sem, CSP_BASE_TYPE * task_woken) {
+	csp_log_lock("Post: %p", sem);
+	if (xSemaphoreGiveFromISR(*sem, task_woken) == pdPASS) {
+		return CSP_SEMAPHORE_OK;
+	} else {
+		return CSP_SEMAPHORE_ERROR;
+	}
+}
+

thirdparty/libcsp/src/arch/freertos/csp_system.c

@@ -0,0 +1,139 @@
+/*
+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 <stdio.h>
+#include <stdint.h>
+#include <FreeRTOS.h>
+#include <task.h>
+
+#include <csp/csp.h>
+#include <csp/csp_error.h>
+
+#include <csp/arch/csp_system.h>
+
+int csp_sys_tasklist(char * out) {
+#if (tskKERNEL_VERSION_MAJOR < 8)
+	vTaskList((signed portCHAR *) out);
+#else
+	vTaskList(out);
+#endif
+	return CSP_ERR_NONE;
+}
+
+int csp_sys_tasklist_size(void) {
+	return 40 * uxTaskGetNumberOfTasks();
+}
+
+uint32_t csp_sys_memfree(void) {
+
+	uint32_t total = 0, max = UINT32_MAX, size;
+	void * pmem;
+
+	/* If size_t is less than 32 bits, start with 10 KiB */
+	size = sizeof(uint32_t) > sizeof(size_t) ? 10000 : 1000000;
+
+	while (1) {
+		pmem = pvPortMalloc(size + total);
+		if (pmem == NULL) {
+			max = size + total;
+			size = size / 2;
+		} else {
+			total += size;
+			if (total + size >= max)
+				size = size / 2;
+			vPortFree(pmem);
+		}
+		if (size < 32) break;
+	}
+
+	return total;
+}
+
+int csp_sys_reboot(void) {
+
+	extern void __attribute__((weak)) cpu_set_reset_cause(unsigned int);
+	if (cpu_set_reset_cause)
+		cpu_set_reset_cause(1);
+	
+	extern void __attribute__((weak)) cpu_reset(void);
+	if (cpu_reset) {
+		cpu_reset();
+		while (1);
+	}
+	
+	csp_log_error("Failed to reboot");
+
+	return CSP_ERR_INVAL;
+}
+
+int csp_sys_shutdown(void) {
+
+	extern void __attribute__((weak)) cpu_shutdown(void);
+	if (cpu_shutdown) {
+		cpu_shutdown();
+		while (1);
+	}
+
+	csp_log_error("Failed to shutdown");
+
+	return CSP_ERR_INVAL;
+}
+
+void csp_sys_set_color(csp_color_t color) {
+
+	unsigned int color_code, modifier_code;
+	switch (color & COLOR_MASK_COLOR) {
+		case COLOR_BLACK:
+			color_code = 30; break;
+		case COLOR_RED:
+			color_code = 31; break;
+		case COLOR_GREEN:
+			color_code = 32; break;
+		case COLOR_YELLOW:
+			color_code = 33; break;
+		case COLOR_BLUE:
+			color_code = 34; break;
+		case COLOR_MAGENTA:
+			color_code = 35; break;
+		case COLOR_CYAN:
+			color_code = 36; break;
+		case COLOR_WHITE:
+			color_code = 37; break;
+		case COLOR_RESET:
+		default:
+			color_code = 0; break;
+	}
+	
+	switch (color & COLOR_MASK_MODIFIER) {
+		case COLOR_BOLD:
+			modifier_code = 1; break;
+		case COLOR_UNDERLINE:
+			modifier_code = 2; break;
+		case COLOR_BLINK:
+			modifier_code = 3; break;
+		case COLOR_HIDE:
+			modifier_code = 4; break;
+		case COLOR_NORMAL:
+		default:
+			modifier_code = 0; break;
+	}
+
+	printf("\033[%u;%um", modifier_code, color_code);
+}

thirdparty/libcsp/src/arch/freertos/csp_thread.c

@@ -0,0 +1,38 @@
+/*
+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 <FreeRTOS.h>
+#include <task.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+
+#include <csp/arch/csp_thread.h>
+
+int csp_thread_create(csp_thread_return_t (* routine)(void *), const char * const thread_name, unsigned short stack_depth, void * parameters, unsigned int priority, csp_thread_handle_t * handle) {
+#if (tskKERNEL_VERSION_MAJOR >= 8)
+	portBASE_TYPE ret = xTaskCreate(routine, thread_name, stack_depth, parameters, priority, handle);
+#else
+	portBASE_TYPE ret = xTaskCreate(routine, (signed char *) thread_name, stack_depth, parameters, priority, handle);
+#endif
+	if (ret != pdTRUE)
+		return CSP_ERR_NOMEM;
+	return CSP_ERR_NONE;
+}

thirdparty/libcsp/src/arch/freertos/csp_time.c

@@ -0,0 +1,46 @@
+/*
+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 <stdint.h>
+
+/* FreeRTOS includes */
+#include <FreeRTOS.h>
+#include <task.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+
+#include <csp/arch/csp_time.h>
+
+uint32_t csp_get_ms(void) {
+	return (uint32_t)(xTaskGetTickCount() * (1000/configTICK_RATE_HZ));
+}
+
+uint32_t csp_get_ms_isr(void) {
+	return (uint32_t)(xTaskGetTickCountFromISR() * (1000/configTICK_RATE_HZ));
+}
+
+uint32_t csp_get_s(void) {
+	return (uint32_t)(xTaskGetTickCount()/configTICK_RATE_HZ);
+}
+
+uint32_t csp_get_s_isr(void) {
+	return (uint32_t)(xTaskGetTickCountFromISR()/configTICK_RATE_HZ);
+}

thirdparty/libcsp/src/arch/macosx/csp_malloc.c

@@ -0,0 +1,31 @@
+/*
+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 <stdint.h>
+#include <stdlib.h>
+
+void * csp_malloc(size_t size) {
+	return malloc(size);
+}
+
+void csp_free(void *ptr) {
+	free(ptr);
+}
+

thirdparty/libcsp/src/arch/macosx/csp_queue.c

@@ -0,0 +1,64 @@
+/*
+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 <stdint.h>
+#include <pthread.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+
+#include <csp/arch/posix/pthread_queue.h>
+#include <csp/arch/csp_queue.h>
+
+
+csp_queue_handle_t csp_queue_create(int length, size_t item_size) {
+	return pthread_queue_create(length, item_size);
+}
+
+void csp_queue_remove(csp_queue_handle_t queue) {
+	return pthread_queue_delete(queue);
+}
+
+int csp_queue_enqueue(csp_queue_handle_t handle, void *value, uint32_t timeout) {
+	return pthread_queue_enqueue(handle, value, timeout);
+}
+
+int csp_queue_enqueue_isr(csp_queue_handle_t handle, void * value, CSP_BASE_TYPE * task_woken) {
+	if (task_woken != NULL)
+		*task_woken = 0;
+	return csp_queue_enqueue(handle, value, 0);
+}
+
+int csp_queue_dequeue(csp_queue_handle_t handle, void *buf, uint32_t timeout) {
+	return pthread_queue_dequeue(handle, buf, timeout);
+}
+
+int csp_queue_dequeue_isr(csp_queue_handle_t handle, void *buf, CSP_BASE_TYPE * task_woken) {
+	*task_woken = 0;
+	return csp_queue_dequeue(handle, buf, 0);
+}
+
+int csp_queue_size(csp_queue_handle_t handle) {
+	return pthread_queue_items(handle);
+}
+
+int csp_queue_size_isr(csp_queue_handle_t handle) {
+	return pthread_queue_items(handle);
+}

thirdparty/libcsp/src/arch/macosx/csp_semaphore.c

@@ -0,0 +1,105 @@
+/*
+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 <pthread.h>
+#include <sys/time.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <errno.h>
+#include <string.h>
+#include <stdint.h>
+#include <inttypes.h>
+#include <stdio.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+
+#include <csp/arch/csp_semaphore.h>
+
+int csp_mutex_create(csp_mutex_t * mutex) {
+	csp_log_lock("Mutex init: %p", mutex);
+	*mutex = pthread_queue_create(1, sizeof(int));
+	if (mutex) {
+		int dummy = 0;
+		pthread_queue_enqueue(*mutex, &dummy, 0);
+		return CSP_SEMAPHORE_OK;
+	} else {
+		return CSP_SEMAPHORE_ERROR;
+	}
+}
+
+int csp_mutex_remove(csp_mutex_t * mutex) {
+	pthread_queue_delete(*mutex);
+	return CSP_SEMAPHORE_OK;
+}
+
+int csp_mutex_lock(csp_mutex_t * mutex, uint32_t timeout) {
+
+	int ret;
+	csp_log_lock("Wait: %p timeout %"PRIu32, mutex, timeout);
+
+	if (timeout == CSP_INFINITY) {
+		/* TODO: fix this to be infinite */
+		int dummy = 0;
+		if (pthread_queue_dequeue(*mutex, &dummy, timeout) == PTHREAD_QUEUE_OK)
+			ret = CSP_MUTEX_OK;
+		else
+			ret = CSP_MUTEX_ERROR;
+	} else {
+		int dummy = 0;
+		if (pthread_queue_dequeue(*mutex, &dummy, timeout) == PTHREAD_QUEUE_OK)
+			ret = CSP_MUTEX_OK;
+		else
+			ret = CSP_MUTEX_ERROR;
+	}
+
+	return ret == CSP_MUTEX_ERROR ? CSP_SEMAPHORE_ERROR : CSP_SEMAPHORE_OK;
+}
+
+int csp_mutex_unlock(csp_mutex_t * mutex) {
+	int dummy = 0;
+	if (pthread_queue_enqueue(*mutex, &dummy, 0) == PTHREAD_QUEUE_OK) {
+		return CSP_SEMAPHORE_OK;
+	} else {
+		return CSP_SEMAPHORE_ERROR;
+	}
+}
+
+int csp_bin_sem_create(csp_bin_sem_handle_t * sem) {
+	return csp_mutex_create(sem);
+}
+
+int csp_bin_sem_remove(csp_bin_sem_handle_t * sem) {
+	return csp_mutex_remove(sem);
+}
+
+int csp_bin_sem_wait(csp_bin_sem_handle_t * sem, uint32_t timeout) {
+	return csp_mutex_lock(sem, timeout);
+}
+
+int csp_bin_sem_post(csp_bin_sem_handle_t * sem) {
+	return csp_mutex_unlock(sem);
+}
+
+int csp_bin_sem_post_isr(csp_bin_sem_handle_t * sem, CSP_BASE_TYPE * task_woken) {
+	return csp_mutex_unlock(sem);
+}

thirdparty/libcsp/src/arch/macosx/csp_system.c

@@ -0,0 +1,99 @@
+/*
+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 <stdio.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <csp/csp.h>
+#include <csp/csp_error.h>
+
+#include <csp/arch/csp_system.h>
+
+int csp_sys_tasklist(char * out) {
+	strcpy(out, "Tasklist not available on OSX");
+	return CSP_ERR_NONE;
+}
+
+int csp_sys_tasklist_size(void) {
+        return 100;
+}
+
+uint32_t csp_sys_memfree(void) {
+	/* TODO: Fix memory free on OSX */
+	uint32_t total = 0;
+	return total;
+}
+
+int csp_sys_reboot(void) {
+	/* TODO: Fix reboot on OSX */
+	csp_log_error("Failed to reboot");
+
+	return CSP_ERR_INVAL;
+}
+
+int csp_sys_shutdown(void) {
+	/* TODO: Fix shutdown on OSX */
+	csp_log_error("Failed to shutdown");
+
+	return CSP_ERR_INVAL;
+}
+
+void csp_sys_set_color(csp_color_t color) {
+
+	unsigned int color_code, modifier_code;
+	switch (color & COLOR_MASK_COLOR) {
+		case COLOR_BLACK:
+			color_code = 30; break;
+		case COLOR_RED:
+			color_code = 31; break;
+		case COLOR_GREEN:
+			color_code = 32; break;
+		case COLOR_YELLOW:
+			color_code = 33; break;
+		case COLOR_BLUE:
+			color_code = 34; break;
+		case COLOR_MAGENTA:
+			color_code = 35; break;
+		case COLOR_CYAN:
+			color_code = 36; break;
+		case COLOR_WHITE:
+			color_code = 37; break;
+		case COLOR_RESET:
+		default:
+			color_code = 0; break;
+	}
+	
+	switch (color & COLOR_MASK_MODIFIER) {
+		case COLOR_BOLD:
+			modifier_code = 1; break;
+		case COLOR_UNDERLINE:
+			modifier_code = 2; break;
+		case COLOR_BLINK:
+			modifier_code = 3; break;
+		case COLOR_HIDE:
+			modifier_code = 4; break;
+		case COLOR_NORMAL:
+		default:
+			modifier_code = 0; break;
+	}
+
+	printf("\033[%u;%um", modifier_code, color_code);
+}

thirdparty/libcsp/src/arch/macosx/csp_thread.c

@@ -0,0 +1,31 @@
+/*
+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 <stdint.h>
+#include <pthread.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+
+#include <csp/arch/csp_thread.h>
+
+int csp_thread_create(csp_thread_return_t (* routine)(void *), const char * const thread_name, unsigned short stack_depth, void * parameters, unsigned int priority, csp_thread_handle_t * handle) {
+	return pthread_create(handle, NULL, routine, parameters);
+}

thirdparty/libcsp/src/arch/macosx/csp_time.c

@@ -0,0 +1,65 @@
+/*
+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 <time.h>
+#include <sys/time.h>
+#include <mach/clock.h>
+#include <mach/mach.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+
+#include <csp/arch/csp_time.h>
+
+uint32_t csp_get_ms(void) {
+	struct timespec ts;
+	
+	clock_serv_t cclock;
+	mach_timespec_t mts;
+	host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
+	clock_get_time(cclock, &mts);
+	mach_port_deallocate(mach_task_self(), cclock);
+	ts.tv_sec = mts.tv_sec;
+	ts.tv_nsec = mts.tv_nsec;
+
+	return (uint32_t)(ts.tv_sec*1000+ts.tv_nsec/1000000);
+}
+
+uint32_t csp_get_ms_isr(void) {
+	return csp_get_ms();
+}
+
+uint32_t csp_get_s(void) {
+	struct timespec ts;
+
+	clock_serv_t cclock;
+	mach_timespec_t mts;
+	host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
+	clock_get_time(cclock, &mts);
+	mach_port_deallocate(mach_task_self(), cclock);
+	ts.tv_sec = mts.tv_sec;
+	ts.tv_nsec = mts.tv_nsec;
+	
+	return (uint32_t)ts.tv_sec;
+}
+
+uint32_t csp_get_s_isr(void) {
+	return csp_get_s();
+}

thirdparty/libcsp/src/arch/macosx/pthread_queue.c

@@ -0,0 +1,179 @@
+/*
+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
+*/
+
+/*
+Inspired by c-pthread-queue by Matthew Dickinson
+http://code.google.com/p/c-pthread-queue/
+*/
+
+#include <pthread.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+#include <mach/clock.h>
+#include <mach/mach.h>
+
+/* CSP includes */
+#include <csp/arch/posix/pthread_queue.h>
+
+pthread_queue_t * pthread_queue_create(int length, size_t item_size) {
+	
+	pthread_queue_t * q = malloc(sizeof(pthread_queue_t));
+	
+	if (q != NULL) {
+		q->buffer = malloc(length*item_size);
+		if (q->buffer != NULL) {
+			q->size = length;
+			q->item_size = item_size;
+			q->items = 0;
+			q->in = 0;
+			q->out = 0;
+			if (pthread_mutex_init(&(q->mutex), NULL) || pthread_cond_init(&(q->cond_full), NULL) || pthread_cond_init(&(q->cond_empty), NULL)) {
+				free(q->buffer);
+				free(q);
+				q = NULL;
+			}
+		} else {
+			free(q);
+			q = NULL;
+		}
+	}
+
+	return q;
+	
+}
+
+void pthread_queue_delete(pthread_queue_t * q) {
+
+	if (q == NULL)
+		return;
+
+	free(q->buffer);
+	free(q);
+
+	return;
+
+}
+
+int pthread_queue_enqueue(pthread_queue_t * queue, void * value, uint32_t timeout) {
+	
+	int ret;
+
+	/* Calculate timeout */
+	struct timespec ts;
+
+	clock_serv_t cclock;
+	mach_timespec_t mts;
+	host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
+	clock_get_time(cclock, &mts);
+	mach_port_deallocate(mach_task_self(), cclock);
+	ts.tv_sec = mts.tv_sec;
+	ts.tv_nsec = mts.tv_nsec;
+
+	uint32_t sec = timeout / 1000;
+	uint32_t nsec = (timeout - 1000 * sec) * 1000000;
+
+	ts.tv_sec += sec;
+
+	if (ts.tv_nsec + nsec > 1000000000)
+		ts.tv_sec++;
+
+	ts.tv_nsec = (ts.tv_nsec + nsec) % 1000000000;
+
+	/* Get queue lock */
+	pthread_mutex_lock(&(queue->mutex));
+	while (queue->items == queue->size) {
+		ret = pthread_cond_timedwait(&(queue->cond_full), &(queue->mutex), &ts);
+		if (ret != 0) {
+			pthread_mutex_unlock(&(queue->mutex));
+			return PTHREAD_QUEUE_FULL;
+		}
+	}
+
+	/* Coby object from input buffer */
+	memcpy(queue->buffer+(queue->in * queue->item_size), value, queue->item_size);
+	queue->items++;
+	queue->in = (queue->in + 1) % queue->size;
+	pthread_mutex_unlock(&(queue->mutex));
+	
+	/* Nofify blocked threads */
+	pthread_cond_broadcast(&(queue->cond_empty));
+	
+	return PTHREAD_QUEUE_OK;
+	
+}
+
+int pthread_queue_dequeue(pthread_queue_t * queue, void * buf, uint32_t timeout) {
+
+	int ret;
+	
+	/* Calculate timeout */
+	struct timespec ts;
+	clock_serv_t cclock;
+	mach_timespec_t mts;
+	host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
+	clock_get_time(cclock, &mts);
+	mach_port_deallocate(mach_task_self(), cclock);
+	ts.tv_sec = mts.tv_sec;
+	ts.tv_nsec = mts.tv_nsec;
+	
+	uint32_t sec = timeout / 1000;
+	uint32_t nsec = (timeout - 1000 * sec) * 1000000;
+
+	ts.tv_sec += sec;
+	
+	if (ts.tv_nsec + nsec > 1000000000)
+		ts.tv_sec++;
+
+	ts.tv_nsec = (ts.tv_nsec + nsec) % 1000000000;
+	
+	/* Get queue lock */
+	pthread_mutex_lock(&(queue->mutex));
+	while (queue->items == 0) {
+		ret = pthread_cond_timedwait(&(queue->cond_empty), &(queue->mutex), &ts);
+		if (ret != 0) {
+			pthread_mutex_unlock(&(queue->mutex));
+			return PTHREAD_QUEUE_EMPTY;
+		}
+	}
+
+	/* Coby object to output buffer */
+	memcpy(buf, queue->buffer+(queue->out * queue->item_size), queue->item_size);
+	queue->items--;
+	queue->out = (queue->out + 1) % queue->size;
+	pthread_mutex_unlock(&(queue->mutex));
+	
+	/* Nofify blocked threads */
+	pthread_cond_broadcast(&(queue->cond_full));
+
+	return PTHREAD_QUEUE_OK;
+	
+}
+
+int pthread_queue_items(pthread_queue_t * queue) {
+
+	pthread_mutex_lock(&(queue->mutex));
+	int items = queue->items;
+	pthread_mutex_unlock(&(queue->mutex));
+	
+	return items;
+	
+}

thirdparty/libcsp/src/arch/posix/CMakeLists.txt

@@ -0,0 +1,9 @@
+target_sources(${LIB_CSP_NAME} PRIVATE
+	csp_malloc.c
+	csp_queue.c
+	csp_semaphore.c
+	csp_system.c
+	csp_thread.c
+	csp_time.c
+	pthread_queue.c
+)

thirdparty/libcsp/src/arch/posix/csp_malloc.c

@@ -0,0 +1,31 @@
+/*
+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 <stdint.h>
+#include <stdlib.h>
+
+void * csp_malloc(size_t size) {
+	return malloc(size);
+}
+
+void csp_free(void *ptr) {
+	free(ptr);
+}
+

thirdparty/libcsp/src/arch/posix/csp_queue.c

@@ -0,0 +1,64 @@
+/*
+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 <stdint.h>
+#include <pthread.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+
+#include <csp/arch/posix/pthread_queue.h>
+#include <csp/arch/csp_queue.h>
+
+
+csp_queue_handle_t csp_queue_create(int length, size_t item_size) {
+	return pthread_queue_create(length, item_size);
+}
+
+void csp_queue_remove(csp_queue_handle_t queue) {
+	return pthread_queue_delete(queue);
+}
+
+int csp_queue_enqueue(csp_queue_handle_t handle, void *value, uint32_t timeout) {
+	return pthread_queue_enqueue(handle, value, timeout);
+}
+
+int csp_queue_enqueue_isr(csp_queue_handle_t handle, void * value, CSP_BASE_TYPE * task_woken) {
+	if (task_woken != NULL)
+		*task_woken = 0;
+	return csp_queue_enqueue(handle, value, 0);
+}
+
+int csp_queue_dequeue(csp_queue_handle_t handle, void *buf, uint32_t timeout) {
+	return pthread_queue_dequeue(handle, buf, timeout);
+}
+
+int csp_queue_dequeue_isr(csp_queue_handle_t handle, void *buf, CSP_BASE_TYPE * task_woken) {
+	*task_woken = 0;
+	return csp_queue_dequeue(handle, buf, 0);
+}
+
+int csp_queue_size(csp_queue_handle_t handle) {
+	return pthread_queue_items(handle);
+}
+
+int csp_queue_size_isr(csp_queue_handle_t handle) {
+	return pthread_queue_items(handle);
+}

thirdparty/libcsp/src/arch/posix/csp_semaphore.c

@@ -0,0 +1,164 @@
+/*
+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 <semaphore.h>
+#include <pthread.h>
+#include <sys/time.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <errno.h>
+#include <string.h>
+#include <stdint.h>
+#include <inttypes.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+
+#include <csp/arch/csp_semaphore.h>
+
+int csp_mutex_create(csp_mutex_t * mutex) {
+	csp_log_lock("Mutex init: %p", mutex);
+	if (pthread_mutex_init(mutex, NULL) == 0) {
+		return CSP_SEMAPHORE_OK;
+	} else {
+		return CSP_SEMAPHORE_ERROR;
+	}
+}
+
+int csp_mutex_remove(csp_mutex_t * mutex) {
+	if (pthread_mutex_destroy(mutex) == 0) {
+		return CSP_SEMAPHORE_OK;
+	} else {
+		return CSP_SEMAPHORE_ERROR;
+	}
+}
+
+int csp_mutex_lock(csp_mutex_t * mutex, uint32_t timeout) {
+
+	int ret;
+	struct timespec ts;
+	uint32_t sec, nsec;
+
+	csp_log_lock("Wait: %p timeout %"PRIu32, mutex, timeout);
+
+	if (timeout == CSP_INFINITY) {
+		ret = pthread_mutex_lock(mutex);
+	} else {
+		if (clock_gettime(CLOCK_REALTIME, &ts))
+			return CSP_SEMAPHORE_ERROR;
+
+		sec = timeout / 1000;
+		nsec = (timeout - 1000 * sec) * 1000000;
+
+		ts.tv_sec += sec;
+
+		if (ts.tv_nsec + nsec >= 1000000000)
+			ts.tv_sec++;
+
+		ts.tv_nsec = (ts.tv_nsec + nsec) % 1000000000;
+
+		ret = pthread_mutex_timedlock(mutex, &ts);
+	}
+
+	if (ret != 0)
+		return CSP_SEMAPHORE_ERROR;
+
+	return CSP_SEMAPHORE_OK;
+}
+
+int csp_mutex_unlock(csp_mutex_t * mutex) {
+	if (pthread_mutex_unlock(mutex) == 0) {
+		return CSP_SEMAPHORE_OK;
+	} else {
+		return CSP_SEMAPHORE_ERROR;
+	}
+}
+
+int csp_bin_sem_create(csp_bin_sem_handle_t * sem) {
+	csp_log_lock("Semaphore init: %p", sem);
+	if (sem_init(sem, 0, 1) == 0) {
+		return CSP_SEMAPHORE_OK;
+	} else {
+		return CSP_SEMAPHORE_ERROR;
+	}
+}
+
+int csp_bin_sem_remove(csp_bin_sem_handle_t * sem) {
+	if (sem_destroy(sem) == 0)
+		return CSP_SEMAPHORE_OK;
+	else
+		return CSP_SEMAPHORE_ERROR;
+}
+
+int csp_bin_sem_wait(csp_bin_sem_handle_t * sem, uint32_t timeout) {
+
+	int ret;
+	struct timespec ts;
+	uint32_t sec, nsec;
+
+	csp_log_lock("Wait: %p timeout %"PRIu32, sem, timeout);
+
+	if (timeout == CSP_INFINITY) {
+		ret = sem_wait(sem);
+	} else {
+		if (clock_gettime(CLOCK_REALTIME, &ts))
+			return CSP_SEMAPHORE_ERROR;
+		
+		sec = timeout / 1000;
+		nsec = (timeout - 1000 * sec) * 1000000;
+
+		ts.tv_sec += sec;
+
+		if (ts.tv_nsec + nsec >= 1000000000)
+			ts.tv_sec++;
+
+		ts.tv_nsec = (ts.tv_nsec + nsec) % 1000000000;
+
+		ret = sem_timedwait(sem, &ts);
+	}
+
+	if (ret != 0)
+		return CSP_SEMAPHORE_ERROR;
+
+	return CSP_SEMAPHORE_OK;
+}
+
+int csp_bin_sem_post(csp_bin_sem_handle_t * sem) {
+	CSP_BASE_TYPE dummy = 0;
+	return csp_bin_sem_post_isr(sem, &dummy);
+}
+
+int csp_bin_sem_post_isr(csp_bin_sem_handle_t * sem, CSP_BASE_TYPE * task_woken) {
+	csp_log_lock("Post: %p", sem);
+	*task_woken = 0;
+
+	int value;
+	sem_getvalue(sem, &value);
+	if (value > 0)
+		return CSP_SEMAPHORE_OK;
+
+	if (sem_post(sem) == 0) {
+		return CSP_SEMAPHORE_OK;
+	} else {
+		return CSP_SEMAPHORE_ERROR;
+	}
+}

thirdparty/libcsp/src/arch/posix/csp_system.c

@@ -0,0 +1,131 @@
+/*
+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 <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <errno.h>
+#include <sys/sysinfo.h>
+#include <sys/reboot.h>
+#include <linux/reboot.h>
+
+#include <csp/csp.h>
+#include <csp/csp_error.h>
+
+#include <csp/arch/csp_system.h>
+
+int csp_sys_tasklist(char * out) {
+	strcpy(out, "Tasklist not available on POSIX");
+	return CSP_ERR_NONE;
+}
+
+int csp_sys_tasklist_size(void) {
+	return 100;
+}
+
+uint32_t csp_sys_memfree(void) {
+	uint32_t total = 0;
+	struct sysinfo info;
+	sysinfo(&info);
+	total = info.freeram * info.mem_unit;
+	return total;
+}
+
+int csp_sys_reboot(void) {
+#ifdef CSP_USE_INIT_SHUTDOWN
+	/* Let init(1) handle the reboot */
+	int ret = system("reboot");
+	(void) ret; /* Silence warning */
+#else
+	int magic = LINUX_REBOOT_CMD_RESTART;
+
+	/* Sync filesystem before reboot */
+	sync();
+	reboot(magic);
+#endif
+
+	/* If reboot(2) returns, it is an error */
+	csp_log_error("Failed to reboot: %s", strerror(errno));
+
+	return CSP_ERR_INVAL;
+}
+
+int csp_sys_shutdown(void) {
+#ifdef CSP_USE_INIT_SHUTDOWN
+	/* Let init(1) handle the shutdown */
+	int ret = system("halt");
+	(void) ret; /* Silence warning */
+#else
+	int magic = LINUX_REBOOT_CMD_HALT;
+
+	/* Sync filesystem before reboot */
+	sync();
+	reboot(magic);
+#endif
+
+	/* If reboot(2) returns, it is an error */
+	csp_log_error("Failed to shutdown: %s", strerror(errno));
+
+	return CSP_ERR_INVAL;
+}
+
+void csp_sys_set_color(csp_color_t color) {
+
+	unsigned int color_code, modifier_code;
+	switch (color & COLOR_MASK_COLOR) {
+		case COLOR_BLACK:
+			color_code = 30; break;
+		case COLOR_RED:
+			color_code = 31; break;
+		case COLOR_GREEN:
+			color_code = 32; break;
+		case COLOR_YELLOW:
+			color_code = 33; break;
+		case COLOR_BLUE:
+			color_code = 34; break;
+		case COLOR_MAGENTA:
+			color_code = 35; break;
+		case COLOR_CYAN:
+			color_code = 36; break;
+		case COLOR_WHITE:
+			color_code = 37; break;
+		case COLOR_RESET:
+		default:
+			color_code = 0; break;
+	}
+	
+	switch (color & COLOR_MASK_MODIFIER) {
+		case COLOR_BOLD:
+			modifier_code = 1; break;
+		case COLOR_UNDERLINE:
+			modifier_code = 2; break;
+		case COLOR_BLINK:
+			modifier_code = 3; break;
+		case COLOR_HIDE:
+			modifier_code = 4; break;
+		case COLOR_NORMAL:
+		default:
+			modifier_code = 0; break;
+	}
+
+	printf("\033[%u;%um", modifier_code, color_code);
+}

thirdparty/libcsp/src/arch/posix/csp_thread.c

@@ -0,0 +1,55 @@
+/*
+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 <stdint.h>
+#include <pthread.h>
+#include <limits.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+
+#include <csp/arch/csp_thread.h>
+
+int csp_thread_create(csp_thread_return_t (* routine)(void *), const char * const thread_name, unsigned short stack_depth, void * parameters, unsigned int priority, csp_thread_handle_t * handle) {
+	pthread_attr_t attributes, *attr_ref;
+	int return_code;
+	
+	if( pthread_attr_init(&attributes) == 0 )
+	{
+		unsigned int stack_size = PTHREAD_STACK_MIN;// use at least one memory page
+		
+		while(stack_size < stack_depth)// must reach at least the provided size
+		{
+			stack_size += PTHREAD_STACK_MIN;// keep memory page boundary (some systems may fail otherwise))
+		}
+		attr_ref = &attributes;
+		
+		pthread_attr_setdetachstate(attr_ref, PTHREAD_CREATE_DETACHED);// do not waste memory on each call
+		pthread_attr_setstacksize(attr_ref, stack_size);
+	}
+	else
+	{
+		attr_ref = NULL;
+	}
+	return_code = pthread_create(handle, attr_ref, routine, parameters);
+	if( attr_ref != NULL ) pthread_attr_destroy(attr_ref);
+	
+	return return_code;
+}

thirdparty/libcsp/src/arch/posix/csp_time.c

@@ -0,0 +1,54 @@
+/*
+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 <pthread.h>
+#include <time.h>
+#include <sys/time.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+
+#include <csp/arch/csp_time.h>
+
+uint32_t csp_get_ms(void) {
+	struct timespec ts;
+
+	if (clock_gettime(CLOCK_MONOTONIC, &ts) == 0)
+		return (uint32_t)(ts.tv_sec*1000+ts.tv_nsec/1000000);
+	else
+		return 0;
+}
+
+uint32_t csp_get_ms_isr(void) {
+	return csp_get_ms();
+}
+
+uint32_t csp_get_s(void) {
+	struct timespec ts;
+
+	if (clock_gettime(CLOCK_MONOTONIC, &ts) == 0)
+		return (uint32_t)ts.tv_sec;
+	else
+		return 0;
+}
+
+uint32_t csp_get_s_isr(void) {
+	return csp_get_s();
+}

thirdparty/libcsp/src/arch/posix/pthread_queue.c

@@ -0,0 +1,243 @@
+/*
+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
+*/
+
+/*
+Inspired by c-pthread-queue by Matthew Dickinson
+http://code.google.com/p/c-pthread-queue/
+*/
+
+#include <pthread.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+#include <stdint.h>
+#include <sys/time.h>
+
+/* CSP includes */
+#include <csp/arch/posix/pthread_queue.h>
+
+static inline int get_deadline(struct timespec *ts, uint32_t timeout_ms)
+{
+	int ret = clock_gettime(CLOCK_MONOTONIC, ts);
+
+	if (ret < 0) {
+		return ret;
+	}
+
+	uint32_t sec = timeout_ms / 1000;
+	uint32_t nsec = (timeout_ms - 1000 * sec) * 1000000;
+
+	ts->tv_sec += sec;
+
+	if (ts->tv_nsec + nsec >= 1000000000) {
+		ts->tv_sec++;
+	}
+
+	ts->tv_nsec = (ts->tv_nsec + nsec) % 1000000000;
+
+	return ret;
+}
+
+static inline int init_cond_clock_monotonic(pthread_cond_t * cond)
+{
+
+	int ret;
+	pthread_condattr_t attr;
+
+	pthread_condattr_init(&attr);
+	ret = pthread_condattr_setclock(&attr, CLOCK_MONOTONIC);
+
+	if (ret == 0) {
+		ret = pthread_cond_init(cond, &attr);
+	}
+
+	pthread_condattr_destroy(&attr);
+	return ret;
+
+}
+
+pthread_queue_t * pthread_queue_create(int length, size_t item_size) {
+	
+	pthread_queue_t * q = malloc(sizeof(pthread_queue_t));
+	
+	if (q != NULL) {
+		q->buffer = malloc(length*item_size);
+		if (q->buffer != NULL) {
+			q->size = length;
+			q->item_size = item_size;
+			q->items = 0;
+			q->in = 0;
+			q->out = 0;
+			if (pthread_mutex_init(&(q->mutex), NULL) || init_cond_clock_monotonic(&(q->cond_full)) || init_cond_clock_monotonic(&(q->cond_empty))) {
+				free(q->buffer);
+				free(q);
+				q = NULL;
+			}
+		} else {
+			free(q);
+			q = NULL;
+		}
+	}
+
+	return q;
+	
+}
+
+void pthread_queue_delete(pthread_queue_t * q) {
+
+	if (q == NULL)
+		return;
+
+	free(q->buffer);
+	free(q);
+
+	return;
+
+}
+	
+
+static inline int wait_slot_available(pthread_queue_t * queue, struct timespec *ts) {
+
+	int ret;
+
+	while (queue->items == queue->size) {
+
+		if (ts != NULL) {
+			ret = pthread_cond_timedwait(&(queue->cond_full), &(queue->mutex), ts);
+		} else {
+			ret = pthread_cond_wait(&(queue->cond_full), &(queue->mutex));
+		}
+
+		if (ret != 0 && errno != EINTR) {
+			return PTHREAD_QUEUE_FULL; //Timeout
+		}
+	}
+
+	return PTHREAD_QUEUE_OK;
+
+}
+
+int pthread_queue_enqueue(pthread_queue_t * queue, void * value, uint32_t timeout) {
+
+	int ret;
+	struct timespec ts;
+	struct timespec *pts = NULL;
+
+	/* Calculate timeout */
+	if (timeout != CSP_MAX_DELAY) {
+		if (get_deadline(&ts, timeout) != 0) {
+			return PTHREAD_QUEUE_ERROR;
+		}
+		pts = &ts;
+	} else {
+		pts = NULL;
+	}
+
+	/* Get queue lock */
+	pthread_mutex_lock(&(queue->mutex));
+
+	ret = wait_slot_available(queue, pts);
+	if (ret == PTHREAD_QUEUE_OK) {
+		/* Copy object from input buffer */
+		memcpy(queue->buffer+(queue->in * queue->item_size), value, queue->item_size);
+		queue->items++;
+		queue->in = (queue->in + 1) % queue->size;
+	}
+
+	pthread_mutex_unlock(&(queue->mutex));
+
+	if (ret == PTHREAD_QUEUE_OK) {
+		/* Nofify blocked threads */
+		pthread_cond_broadcast(&(queue->cond_empty));
+	}
+
+	return ret;
+
+}
+
+static inline int wait_item_available(pthread_queue_t * queue, struct timespec *ts) {
+
+	int ret;
+
+	while (queue->items == 0) {
+
+		if (ts != NULL) {
+			ret = pthread_cond_timedwait(&(queue->cond_empty), &(queue->mutex), ts);
+		} else {
+			ret = pthread_cond_wait(&(queue->cond_empty), &(queue->mutex));
+		}
+
+		if (ret != 0 && errno != EINTR) {
+			return PTHREAD_QUEUE_EMPTY; //Timeout
+		}
+	}
+
+	return PTHREAD_QUEUE_OK;
+
+}
+
+int pthread_queue_dequeue(pthread_queue_t * queue, void * buf, uint32_t timeout) {
+
+	int ret;
+	struct timespec ts;
+	struct timespec *pts;
+
+	/* Calculate timeout */
+	if (timeout != CSP_MAX_DELAY) {
+		if (get_deadline(&ts, timeout) != 0) {
+			return PTHREAD_QUEUE_ERROR;
+		}
+		pts = &ts;
+	} else {
+		pts = NULL;
+	}
+
+	/* Get queue lock */
+	pthread_mutex_lock(&(queue->mutex));
+
+	ret = wait_item_available(queue, pts);
+	if (ret == PTHREAD_QUEUE_OK) {
+		/* Coby object to output buffer */
+		memcpy(buf, queue->buffer+(queue->out * queue->item_size), queue->item_size);
+		queue->items--;
+		queue->out = (queue->out + 1) % queue->size;
+	}
+
+	pthread_mutex_unlock(&(queue->mutex));
+
+	if (ret == PTHREAD_QUEUE_OK) {
+		/* Nofify blocked threads */
+		pthread_cond_broadcast(&(queue->cond_full));
+	}
+
+	return ret;
+
+}
+
+int pthread_queue_items(pthread_queue_t * queue) {
+
+	pthread_mutex_lock(&(queue->mutex));
+	int items = queue->items;
+	pthread_mutex_unlock(&(queue->mutex));
+	
+	return items;
+	
+}

thirdparty/libcsp/src/arch/windows/README

@@ -0,0 +1,18 @@
+This directory contains files specific to the windows port of libcsp.
+
+To compile and create a static library, execute:
+
+  python waf configure --with-os=windows build
+
+from the root of this project. Note python must be in PATH.
+
+The build requirements are:
+    * Windows Vista SP1
+    * A recent version of MinGW _or_ MinGW-w64
+    * Windows API headers
+    * cPython 2.5 or newer
+
+What provides the Windows API headers depends on the development environment:
+Using MinGW: Headers provided by w32api package. windows_glue.h header is needed because these headers do not declare condition variables.
+Using MinGW-w64: Headers should be available in the default configuration. You may have to compile the distribution from source. windows_glue.h should not be needed.
+

thirdparty/libcsp/src/arch/windows/csp_malloc.c

@@ -0,0 +1,9 @@
+#include <stdlib.h>
+
+void * csp_malloc(size_t size) {
+	return malloc(size);
+}
+
+void csp_free(void * ptr) {
+	free(ptr);
+}

thirdparty/libcsp/src/arch/windows/csp_queue.c

@@ -0,0 +1,40 @@
+#include <stdint.h>
+#include <csp/csp.h>
+#include <csp/arch/csp_queue.h>
+#include "windows_queue.h"
+
+csp_queue_handle_t csp_queue_create(int length, size_t item_size) {
+	return windows_queue_create(length, item_size);
+}
+
+void csp_queue_remove(csp_queue_handle_t queue) {
+	windows_queue_delete(queue);
+}
+
+int csp_queue_enqueue(csp_queue_handle_t handle, void *value, uint32_t timeout) {
+	return windows_queue_enqueue(handle, value, timeout);
+}
+
+int csp_queue_enqueue_isr(csp_queue_handle_t handle, void * value, CSP_BASE_TYPE * task_woken) {
+	if( task_woken != NULL )
+		*task_woken = 0;
+	return windows_queue_enqueue(handle, value, 0);
+}
+
+int csp_queue_dequeue(csp_queue_handle_t handle, void *buf, uint32_t timeout) {
+	return windows_queue_dequeue(handle, buf, timeout);
+}
+
+int csp_queue_dequeue_isr(csp_queue_handle_t handle, void * buf, CSP_BASE_TYPE * task_woken) {
+	if( task_woken != NULL )
+		*task_woken = 0;
+	return windows_queue_dequeue(handle, buf, 0);
+}
+
+int csp_queue_size(csp_queue_handle_t handle) {
+	return windows_queue_items(handle);
+}
+
+int csp_queue_size_isr(csp_queue_handle_t handle) {
+	return windows_queue_items(handle);
+}

thirdparty/libcsp/src/arch/windows/csp_semaphore.c

@@ -0,0 +1,74 @@
+#include <Windows.h>
+#include <csp/csp.h>
+#include <csp/arch/csp_semaphore.h>
+
+int csp_mutex_create(csp_mutex_t * mutex) {
+    HANDLE mutexHandle = CreateMutex(NULL, FALSE, FALSE);
+    if( mutexHandle == NULL ) {
+        return CSP_MUTEX_ERROR;
+    }
+    *mutex = mutexHandle;
+    return CSP_MUTEX_OK;
+}
+
+int csp_mutex_remove(csp_mutex_t * mutex) {
+    if( !CloseHandle(*mutex) ) {
+        return CSP_MUTEX_ERROR;
+    }
+    return CSP_MUTEX_OK;
+}
+
+int csp_mutex_lock(csp_mutex_t * mutex, uint32_t timeout) {
+    if(WaitForSingleObject(*mutex, timeout) == WAIT_OBJECT_0) {
+            return CSP_MUTEX_OK;
+    }
+    return CSP_MUTEX_ERROR;
+
+}
+
+int csp_mutex_unlock(csp_mutex_t * mutex) {
+    if( !ReleaseMutex(*mutex) ) {
+        return CSP_MUTEX_ERROR;
+    }
+    return CSP_MUTEX_OK;
+}
+
+int csp_bin_sem_create(csp_bin_sem_handle_t * sem) {
+    HANDLE semHandle = CreateSemaphore(NULL, 1, 1, NULL);
+    if( semHandle == NULL ) {
+        return CSP_SEMAPHORE_ERROR;
+    }
+    *sem = semHandle;
+    return CSP_SEMAPHORE_OK;
+}
+
+int csp_bin_sem_remove(csp_bin_sem_handle_t * sem) {
+    if( !CloseHandle(*sem) ) {
+        return CSP_SEMAPHORE_ERROR;
+    }
+    return CSP_SEMAPHORE_OK;
+}
+
+int csp_bin_sem_wait(csp_bin_sem_handle_t * sem, uint32_t timeout) {
+    if( WaitForSingleObject(*sem, timeout) == WAIT_OBJECT_0 ) {
+            return CSP_SEMAPHORE_OK;
+    }
+    return CSP_SEMAPHORE_ERROR;
+
+}
+
+int csp_bin_sem_post(csp_bin_sem_handle_t * sem) {
+    if( !ReleaseSemaphore(*sem, 1, NULL) ) {
+        return CSP_SEMAPHORE_ERROR;
+    }
+    return CSP_SEMAPHORE_OK;
+}
+
+int csp_bin_sem_post_isr(csp_bin_sem_handle_t * sem, CSP_BASE_TYPE * task_woken) {
+    if( task_woken != NULL ) {
+        *task_woken = 0;
+    }
+    return csp_bin_sem_post(sem);
+}
+
+

thirdparty/libcsp/src/arch/windows/csp_system.c

@@ -0,0 +1,60 @@
+/*
+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 <stdint.h>
+#include <string.h>
+#include <Windows.h>
+
+#include <csp/csp.h>
+#include <csp/csp_error.h>
+
+#include <csp/arch/csp_system.h>
+
+int csp_sys_tasklist(char * out) {
+	strcpy(out, "Tasklist not available on Windows");
+	return CSP_ERR_NONE;
+}
+
+uint32_t csp_sys_memfree(void) {
+	MEMORYSTATUSEX statex;
+	statex.dwLength = sizeof(statex);
+	GlobalMemoryStatusEx(&statex);
+	DWORDLONG freePhysicalMem = statex.ullAvailPhys;
+	size_t total = (size_t) freePhysicalMem;
+	return (uint32_t)total;
+}
+
+int csp_sys_reboot(void) {
+	/* TODO: Fix reboot on Windows */
+	csp_log_error("Failed to reboot");
+
+	return CSP_ERR_INVAL;
+}
+
+int csp_sys_shutdown(void) {
+	/* TODO: Fix shutdown on Windows */
+	csp_log_error("Failed to shutdown");
+
+	return CSP_ERR_INVAL;
+}
+
+void csp_sys_set_color(csp_color_t color) {
+	/* TODO: Add Windows color output here */
+}

thirdparty/libcsp/src/arch/windows/csp_thread.c

@@ -0,0 +1,11 @@
+#include <Windows.h>
+#include <process.h>
+#include <csp/arch/csp_thread.h>
+
+int csp_thread_create(csp_thread_return_t (* routine)(void *)__attribute__((stdcall)), const char * const thread_name, unsigned short stack_depth, void * parameters, unsigned int priority, csp_thread_handle_t * handle) {
+    HANDLE taskHandle = (HANDLE) _beginthreadex(NULL, stack_depth, routine, parameters, 0, 0);
+    if( taskHandle == 0 )
+        return CSP_ERR_NOMEM; // Failure
+    *handle = taskHandle;
+    return CSP_ERR_NONE;
+}

thirdparty/libcsp/src/arch/windows/csp_time.c

@@ -0,0 +1,20 @@
+#include <Windows.h>
+#include <stdint.h>
+#include <csp/arch/csp_time.h>
+
+uint32_t csp_get_ms(void) {
+	return (uint32_t)GetTickCount();
+}
+
+uint32_t csp_get_ms_isr(void) {
+	return csp_get_ms();
+}
+
+uint32_t csp_get_s(void) {
+	uint32_t time_ms = csp_get_ms();
+	return time_ms/1000;
+}
+
+uint32_t csp_get_s_isr(void) {
+	return csp_get_s();
+}

thirdparty/libcsp/src/arch/windows/windows_glue.h

@@ -0,0 +1,23 @@
+#ifndef WINDOWS_GLUE_H
+#define WINDOWS_GLUE_H
+
+#include <Windows.h>
+#undef interface
+
+#if (_WIN32_WINNT >= 0x0600)
+
+#define RTL_CONDITION_VARIABLE_INIT 0
+#define RTL_CONDITION_VARIABLE_LOCKMODE_SHARED 1
+#define CONDITION_VARIABLE_INIT RTL_CONDITION_VARIABLE_INIT
+#define CONDITION_VARIABLE_LOCKMODE_SHARED RTL_CONDITION_VARIABLE_LOCKMODE_SHARED
+
+typedef PVOID RTL_CONDITION_VARIABLE;
+typedef RTL_CONDITION_VARIABLE CONDITION_VARIABLE, *PCONDITION_VARIABLE;
+
+WINBASEAPI VOID WINAPI InitializeConditionVariable(PCONDITION_VARIABLE ConditionVariable);
+WINBASEAPI WINBOOL WINAPI SleepConditionVariableCS(PCONDITION_VARIABLE ConditionVariable, PCRITICAL_SECTION CriticalSection, DWORD dwMilliseconds);
+WINBASEAPI VOID WINAPI WakeAllConditionVariable(PCONDITION_VARIABLE ConditionVariable);
+WINBASEAPI VOID WINAPI WakeConditionVariable(PCONDITION_VARIABLE ConditionVariable);
+
+#endif // _WIN#"_WINNT
+#endif

thirdparty/libcsp/src/arch/windows/windows_queue.c

@@ -0,0 +1,91 @@
+#include "windows_queue.h"
+#include "windows_glue.h"
+#include <Windows.h>
+
+static int queueFull(windows_queue_t * queue) {
+	return queue->items == queue->size;
+}
+
+static int queueEmpty(windows_queue_t * queue) {
+	return queue->items == 0;
+}
+
+windows_queue_t * windows_queue_create(int length, size_t item_size) {
+	windows_queue_t *queue = (windows_queue_t*)malloc(sizeof(windows_queue_t));
+	if(queue == NULL)
+		goto queue_malloc_failed;
+
+	queue->buffer = malloc(length*item_size);
+	if(queue->buffer == NULL)
+		goto buffer_malloc_failed;
+
+	queue->size = length;
+	queue->item_size = item_size;
+	queue->items = 0;
+	queue->head_idx = 0;
+
+	InitializeCriticalSection(&(queue->mutex));
+	InitializeConditionVariable(&(queue->cond_full));
+	InitializeConditionVariable(&(queue->cond_empty));
+	goto queue_init_success;
+
+buffer_malloc_failed:
+	free(queue);
+	queue = NULL;
+queue_malloc_failed:
+queue_init_success:
+	return queue;
+}
+
+void windows_queue_delete(windows_queue_t * q) {
+	if(q==NULL) return;
+	DeleteCriticalSection(&(q->mutex));
+	free(q->buffer);
+	free(q);
+}
+
+int windows_queue_enqueue(windows_queue_t * queue, void * value, int timeout) {
+	int offset;
+	EnterCriticalSection(&(queue->mutex));
+	while(queueFull(queue)) {
+		int ret = SleepConditionVariableCS(&(queue->cond_full), &(queue->mutex), timeout);
+		if( !ret ) {
+			LeaveCriticalSection(&(queue->mutex));
+			return ret == WAIT_TIMEOUT ? WINDOWS_QUEUE_FULL : WINDOWS_QUEUE_ERROR;
+		}
+	}
+	offset = ((queue->head_idx+queue->items) % queue->size) * queue->item_size;
+	memcpy((unsigned char*)queue->buffer + offset, value, queue->item_size);
+	queue->items++;
+
+	LeaveCriticalSection(&(queue->mutex));
+	WakeAllConditionVariable(&(queue->cond_empty));
+	return WINDOWS_QUEUE_OK;
+}
+
+int windows_queue_dequeue(windows_queue_t * queue, void * buf, int timeout) {
+	EnterCriticalSection(&(queue->mutex));
+	while(queueEmpty(queue)) {
+		int ret = SleepConditionVariableCS(&(queue->cond_empty), &(queue->mutex), timeout);
+		if( !ret ) {
+			LeaveCriticalSection(&(queue->mutex));
+			return ret == WAIT_TIMEOUT ? WINDOWS_QUEUE_EMPTY : WINDOWS_QUEUE_ERROR;
+		}
+	}
+	memcpy(buf, (unsigned char*)queue->buffer+(queue->head_idx%queue->size*queue->item_size), queue->item_size);
+	queue->items--;
+	queue->head_idx = (queue->head_idx + 1) % queue->size;
+
+	LeaveCriticalSection(&(queue->mutex));
+	WakeAllConditionVariable(&(queue->cond_full));
+	return WINDOWS_QUEUE_OK;
+}
+
+int windows_queue_items(windows_queue_t * queue) {
+	int items;
+	EnterCriticalSection(&(queue->mutex));
+	items = queue->items;
+	LeaveCriticalSection(&(queue->mutex));
+
+	return items;
+}

thirdparty/libcsp/src/arch/windows/windows_queue.h

@@ -0,0 +1,41 @@
+#ifndef _WINDOWS_QUEUE_H_
+#define _WINDOWS_QUEUE_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <Windows.h>
+#include "windows_glue.h"
+#undef interface
+
+#include <csp/arch/csp_queue.h>
+
+#define WINDOWS_QUEUE_ERROR CSP_QUEUE_ERROR
+#define WINDOWS_QUEUE_EMPTY CSP_QUEUE_ERROR
+#define WINDOWS_QUEUE_FULL CSP_QUEUE_ERROR
+#define WINDOWS_QUEUE_OK CSP_QUEUE_OK
+
+typedef struct windows_queue_s {
+    void * buffer;
+    int size;
+    int item_size;
+    int items;
+    int head_idx;
+    CRITICAL_SECTION mutex;
+    CONDITION_VARIABLE cond_full;
+    CONDITION_VARIABLE cond_empty;
+} windows_queue_t;
+
+windows_queue_t * windows_queue_create(int length, size_t item_size);
+void windows_queue_delete(windows_queue_t * q);
+int windows_queue_enqueue(windows_queue_t * queue, void * value, int timeout);
+int windows_queue_dequeue(windows_queue_t * queue, void * buf, int timeout);
+int windows_queue_items(windows_queue_t * queue);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // _WINDOWS_QUEUE_H_
+

thirdparty/libcsp/src/crypto/CMakeLists.txt

@@ -0,0 +1,5 @@
+target_sources(${LIB_CSP_NAME} PRIVATE
+	csp_hmac.c
+	csp_sha1.c
+	csp_xtea.c
+)

thirdparty/libcsp/src/crypto/csp_hmac.c

@@ -0,0 +1,202 @@
+/*
+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
+*/
+
+/* Hash-based Message Authentication Code - based on code from libtom.org */
+
+#include <stdint.h>
+#include <string.h>
+#include <stdbool.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+
+#include <csp/crypto/csp_hmac.h>
+#include <csp/crypto/csp_sha1.h>
+
+#ifdef CSP_USE_HMAC
+
+#define HMAC_KEY_LENGTH	16
+
+/* HMAC key */
+static uint8_t csp_hmac_key[HMAC_KEY_LENGTH];
+
+/* HMAC state structure */
+typedef struct {
+	csp_sha1_state	md;
+	uint8_t		key[SHA1_BLOCKSIZE];
+} hmac_state;
+
+static int csp_hmac_init(hmac_state * hmac, const uint8_t * key, uint32_t keylen) {
+	uint32_t i;
+	uint8_t buf[SHA1_BLOCKSIZE];
+
+	/* NULL pointer and key check */
+	if (!hmac || !key || keylen < 1)
+		return CSP_ERR_INVAL;
+
+	/* Make sure we have a large enough key */
+	if(keylen > SHA1_BLOCKSIZE) {
+		csp_sha1_memory(key, keylen, hmac->key);
+		if(SHA1_DIGESTSIZE < SHA1_BLOCKSIZE)
+			memset((hmac->key) + SHA1_DIGESTSIZE, 0, (size_t)(SHA1_BLOCKSIZE - SHA1_DIGESTSIZE));
+	} else {
+		memcpy(hmac->key, key, (size_t)keylen);
+		if(keylen < SHA1_BLOCKSIZE)
+			memset((hmac->key) + keylen, 0, (size_t)(SHA1_BLOCKSIZE - keylen));
+	}
+
+	/* Create the initial vector */
+	for(i = 0; i < SHA1_BLOCKSIZE; i++)
+	   buf[i] = hmac->key[i] ^ 0x36;
+
+	/* Prepend to the hash data */
+	csp_sha1_init(&hmac->md);
+	csp_sha1_process(&hmac->md, buf, SHA1_BLOCKSIZE);
+
+	return CSP_ERR_NONE;
+}
+
+static int csp_hmac_process(hmac_state * hmac, const uint8_t * in, uint32_t inlen) {
+
+	/* NULL pointer check */
+	if (!hmac || !in)
+		return CSP_ERR_INVAL;
+
+	/* Process data */
+	csp_sha1_process(&hmac->md, in, inlen);
+
+	return CSP_ERR_NONE;
+}
+
+static int csp_hmac_done(hmac_state * hmac, uint8_t * out) {
+
+	uint32_t i;
+	uint8_t buf[SHA1_BLOCKSIZE];
+	uint8_t isha[SHA1_DIGESTSIZE];
+
+	if (!hmac || !out)
+		return CSP_ERR_INVAL;
+
+	/* Get the hash of the first HMAC vector plus the data */
+	csp_sha1_done(&hmac->md, isha);
+
+	/* Create the second HMAC vector vector */
+	for(i = 0; i < SHA1_BLOCKSIZE; i++)
+		buf[i] = hmac->key[i] ^ 0x5C;
+
+	/* Now calculate the outer hash */
+	csp_sha1_init(&hmac->md);
+	csp_sha1_process(&hmac->md, buf, SHA1_BLOCKSIZE);
+	csp_sha1_process(&hmac->md, isha, SHA1_DIGESTSIZE);
+	csp_sha1_done(&hmac->md, buf);
+
+	/* Copy to output  */
+	for (i = 0; i < SHA1_DIGESTSIZE; i++)
+		out[i] = buf[i];
+
+	return CSP_ERR_NONE;
+}
+
+int csp_hmac_memory(const uint8_t * key, uint32_t keylen, const uint8_t * data, uint32_t datalen, uint8_t * hmac) {
+	hmac_state state;
+
+	/* NULL pointer check */
+	if (!key || !data || !hmac)
+		return CSP_ERR_INVAL;
+
+	/* Init HMAC state */
+	if (csp_hmac_init(&state, key, keylen) != 0)
+		return CSP_ERR_INVAL;
+
+	/* Process data */
+	if (csp_hmac_process(&state, data, datalen) != 0)
+		return CSP_ERR_INVAL;
+
+	/* Output HMAC */
+	if (csp_hmac_done(&state, hmac) != 0)
+		return CSP_ERR_INVAL;
+
+	return CSP_ERR_NONE;
+}
+
+int csp_hmac_set_key(char * key, uint32_t keylen) {
+
+	/* Use SHA1 as KDF */
+	uint8_t hash[SHA1_DIGESTSIZE];
+	csp_sha1_memory((uint8_t *)key, keylen, hash);
+
+	/* Copy key */
+	memcpy(csp_hmac_key, hash, HMAC_KEY_LENGTH);
+
+	return CSP_ERR_NONE;
+
+}
+
+int csp_hmac_append(csp_packet_t * packet, bool include_header) {
+
+	/* NULL pointer check */
+	if (packet == NULL)
+		return CSP_ERR_INVAL;
+
+	uint8_t hmac[SHA1_DIGESTSIZE];
+
+	/* Calculate HMAC */
+	if (include_header) {
+		csp_hmac_memory(csp_hmac_key, HMAC_KEY_LENGTH, (uint8_t *) &packet->id, packet->length + sizeof(packet->id), hmac);
+	} else {
+		csp_hmac_memory(csp_hmac_key, HMAC_KEY_LENGTH, packet->data, packet->length, hmac);
+	}
+
+	/* Truncate hash and copy to packet */
+	memcpy(&packet->data[packet->length], hmac, CSP_HMAC_LENGTH);
+	packet->length += CSP_HMAC_LENGTH;
+
+	return CSP_ERR_NONE;
+
+}
+
+int csp_hmac_verify(csp_packet_t * packet, bool include_header) {
+
+	/* NULL pointer check */
+	if (packet == NULL)
+		return CSP_ERR_INVAL;
+
+	uint8_t hmac[SHA1_DIGESTSIZE];
+
+	/* Calculate HMAC */
+	if (include_header) {
+		csp_hmac_memory(csp_hmac_key, HMAC_KEY_LENGTH, (uint8_t *) &packet->id, packet->length + sizeof(packet->id) - CSP_HMAC_LENGTH, hmac);
+	} else {
+		csp_hmac_memory(csp_hmac_key, HMAC_KEY_LENGTH, packet->data, packet->length - CSP_HMAC_LENGTH, hmac);
+	}
+
+	/* Compare calculated HMAC with packet header */
+	if (memcmp(&packet->data[packet->length] - CSP_HMAC_LENGTH, hmac, CSP_HMAC_LENGTH) != 0) {
+		/* HMAC failed */
+		return CSP_ERR_HMAC;
+	} else {
+		/* Strip HMAC */
+		packet->length -= CSP_HMAC_LENGTH;
+		return CSP_ERR_NONE;
+	}
+
+}
+
+#endif // CSP_USE_HMAC

thirdparty/libcsp/src/crypto/csp_sha1.c

@@ -0,0 +1,217 @@
+/*
+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
+*/
+
+/* Code originally from Python's SHA1 Module, who based it on libtom.org */
+
+#include <stdint.h>
+#include <string.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+
+#include <csp/crypto/csp_sha1.h>
+
+#if defined(CSP_USE_HMAC) || defined(CSP_USE_XTEA)
+
+/* Rotate left macro */
+#define ROL(x,y)	(((x) << (y)) | ((x) >> (32-y)))
+
+/* Endian Neutral macros that work on all platforms */
+#define STORE32H(x, y) do { (y)[0] = (uint8_t)(((x) >> 24) & 0xff); \
+							(y)[1] = (uint8_t)(((x) >> 16) & 0xff); \
+							(y)[2] = (uint8_t)(((x) >>  8) & 0xff); \
+							(y)[3] = (uint8_t)(((x) >>  0) & 0xff); } while (0)
+
+#define LOAD32H(x, y)  do { (x) = ((uint32_t)((y)[0] & 0xff) << 24) | \
+							   	  ((uint32_t)((y)[1] & 0xff) << 16) | \
+							   	  ((uint32_t)((y)[2] & 0xff) <<  8) | \
+							   	  ((uint32_t)((y)[3] & 0xff) <<  0); } while (0)
+
+#define STORE64H(x, y) do {	(y)[0] = (uint8_t)(((x) >> 56) & 0xff); \
+							(y)[1] = (uint8_t)(((x) >> 48) & 0xff); \
+							(y)[2] = (uint8_t)(((x) >> 40) & 0xff); \
+							(y)[3] = (uint8_t)(((x) >> 32) & 0xff); \
+							(y)[4] = (uint8_t)(((x) >> 24) & 0xff); \
+							(y)[5] = (uint8_t)(((x) >> 16) & 0xff); \
+							(y)[6] = (uint8_t)(((x) >>  8) & 0xff); \
+							(y)[7] = (uint8_t)(((x) >>  0) & 0xff); } while (0)
+
+#define MIN(x, y) (((x) < (y)) ? (x) : (y))
+
+/* SHA1 macros */
+#define F0(x,y,z)  (z ^ (x & (y ^ z)))
+#define F1(x,y,z)  (x ^ y ^ z)
+#define F2(x,y,z)  ((x & y) | (z & (x | y)))
+#define F3(x,y,z)  (x ^ y ^ z)
+
+#define FF_0(a, b, c, d, e, i) do {e = (ROL(a, 5) + F0(b,c,d) + e + W[i] + 0x5a827999UL); b = ROL(b, 30);} while (0)
+#define FF_1(a, b, c, d, e, i) do {e = (ROL(a, 5) + F1(b,c,d) + e + W[i] + 0x6ed9eba1UL); b = ROL(b, 30);} while (0)
+#define FF_2(a, b, c, d, e, i) do {e = (ROL(a, 5) + F2(b,c,d) + e + W[i] + 0x8f1bbcdcUL); b = ROL(b, 30);} while (0)
+#define FF_3(a, b, c, d, e, i) do {e = (ROL(a, 5) + F3(b,c,d) + e + W[i] + 0xca62c1d6UL); b = ROL(b, 30);} while (0)
+
+static void csp_sha1_compress(csp_sha1_state * sha1, const uint8_t * buf) {
+
+	uint32_t a, b, c, d, e, W[80], i;
+
+	/* Copy the state into 512-bits into W[0..15] */
+	for (i = 0; i < 16; i++)
+		LOAD32H(W[i], buf + (4*i));
+
+	/* Copy state */
+	a = sha1->state[0];
+	b = sha1->state[1];
+	c = sha1->state[2];
+	d = sha1->state[3];
+	e = sha1->state[4];
+
+	/* Expand it */
+	for (i = 16; i < 80; i++)
+		W[i] = ROL(W[i-3] ^ W[i-8] ^ W[i-14] ^ W[i-16], 1);
+
+	/* Compress */
+	i = 0;
+
+	/* Round one */
+	for (; i < 20;) {
+	   FF_0(a, b, c, d, e, i++);
+	   FF_0(e, a, b, c, d, i++);
+	   FF_0(d, e, a, b, c, i++);
+	   FF_0(c, d, e, a, b, i++);
+	   FF_0(b, c, d, e, a, i++);
+	}
+
+	/* Round two */
+	for (; i < 40;)  {
+	   FF_1(a, b, c, d, e, i++);
+	   FF_1(e, a, b, c, d, i++);
+	   FF_1(d, e, a, b, c, i++);
+	   FF_1(c, d, e, a, b, i++);
+	   FF_1(b, c, d, e, a, i++);
+	}
+
+	/* Round three */
+	for (; i < 60;)  {
+	   FF_2(a, b, c, d, e, i++);
+	   FF_2(e, a, b, c, d, i++);
+	   FF_2(d, e, a, b, c, i++);
+	   FF_2(c, d, e, a, b, i++);
+	   FF_2(b, c, d, e, a, i++);
+	}
+
+	/* Round four */
+	for (; i < 80;)  {
+	   FF_3(a, b, c, d, e, i++);
+	   FF_3(e, a, b, c, d, i++);
+	   FF_3(d, e, a, b, c, i++);
+	   FF_3(c, d, e, a, b, i++);
+	   FF_3(b, c, d, e, a, i++);
+	}
+
+	/* Store */
+	sha1->state[0] += a;
+	sha1->state[1] += b;
+	sha1->state[2] += c;
+	sha1->state[3] += d;
+	sha1->state[4] += e;
+
+}
+
+void csp_sha1_init(csp_sha1_state * sha1) {
+
+   sha1->state[0] = 0x67452301UL;
+   sha1->state[1] = 0xefcdab89UL;
+   sha1->state[2] = 0x98badcfeUL;
+   sha1->state[3] = 0x10325476UL;
+   sha1->state[4] = 0xc3d2e1f0UL;
+   sha1->curlen = 0;
+   sha1->length = 0;
+
+}
+
+void csp_sha1_process(csp_sha1_state * sha1, const uint8_t * in, uint32_t inlen) {
+
+	uint32_t n;
+	while (inlen > 0) {
+		if (sha1->curlen == 0 && inlen >= SHA1_BLOCKSIZE) {
+		   csp_sha1_compress(sha1, in);
+		   sha1->length += SHA1_BLOCKSIZE * 8;
+		   in += SHA1_BLOCKSIZE;
+		   inlen -= SHA1_BLOCKSIZE;
+		} else {
+		   n = MIN(inlen, (SHA1_BLOCKSIZE - sha1->curlen));
+		   memcpy(sha1->buf + sha1->curlen, in, (size_t)n);
+		   sha1->curlen += n;
+		   in += n;
+		   inlen -= n;
+		   if (sha1->curlen == SHA1_BLOCKSIZE) {
+			  csp_sha1_compress(sha1, sha1->buf);
+			  sha1->length += 8*SHA1_BLOCKSIZE;
+			  sha1->curlen = 0;
+		   }
+	   }
+	}
+
+}
+
+void csp_sha1_done(csp_sha1_state * sha1, uint8_t * out) {
+
+	uint32_t i;
+
+	/* Increase the length of the message */
+	sha1->length += sha1->curlen * 8;
+
+	/* Append the '1' bit */
+	sha1->buf[sha1->curlen++] = 0x80;
+
+	/* If the length is currently above 56 bytes we append zeros
+	 * then compress. Then we can fall back to padding zeros and length
+	 * encoding like normal.
+	 */
+	if (sha1->curlen > 56) {
+		while (sha1->curlen < 64)
+			sha1->buf[sha1->curlen++] = 0;
+		csp_sha1_compress(sha1, sha1->buf);
+		sha1->curlen = 0;
+	}
+
+	/* Pad up to 56 bytes of zeroes */
+	while (sha1->curlen < 56)
+		sha1->buf[sha1->curlen++] = 0;
+
+	/* Store length */
+	STORE64H(sha1->length, sha1->buf + 56);
+	csp_sha1_compress(sha1, sha1->buf);
+
+	/* Copy output */
+	for (i = 0; i < 5; i++)
+		STORE32H(sha1->state[i], out + (4 * i));
+
+}
+
+void csp_sha1_memory(const uint8_t * msg, uint32_t len, uint8_t * hash) {
+
+	csp_sha1_state md;
+	csp_sha1_init(&md);
+	csp_sha1_process(&md, msg, len);
+	csp_sha1_done(&md, hash);
+
+}
+
+#endif // CSP_USE_HMAC

thirdparty/libcsp/src/crypto/csp_xtea.c

@@ -0,0 +1,134 @@
+/*
+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
+*/
+
+/* Simple implementation of XTEA in CTR mode */
+
+#include <stdint.h>
+#include <string.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+#include <csp/csp_endian.h>
+#include <csp/crypto/csp_sha1.h>
+#include <csp/crypto/csp_xtea.h>
+
+#ifdef CSP_USE_XTEA
+
+#define XTEA_BLOCKSIZE 	8
+#define XTEA_ROUNDS 	32
+#define XTEA_KEY_LENGTH	16
+
+/* XTEA key */
+static uint32_t csp_xtea_key[XTEA_KEY_LENGTH/sizeof(uint32_t)] __attribute__ ((aligned(sizeof(uint32_t))));
+
+#define STORE32L(x, y) do { (y)[3] = (uint8_t)(((x) >> 24) & 0xff); \
+							(y)[2] = (uint8_t)(((x) >> 16) & 0xff); \
+							(y)[1] = (uint8_t)(((x) >> 8) & 0xff); \
+							(y)[0] = (uint8_t)(((x) >> 0) & 0xff); } while (0)
+
+#define LOAD32L(x, y) do { (x) = ((uint32_t)((y)[3] & 0xff) << 24) | \
+								 ((uint32_t)((y)[2] & 0xff) << 16) | \
+								 ((uint32_t)((y)[1] & 0xff) << 8)  | \
+								 ((uint32_t)((y)[0] & 0xff) << 0); } while (0)
+
+/* This function takes 64 bits of data in block and the 128 bits key in key */
+static inline void csp_xtea_encrypt_block(uint8_t *block, uint8_t const *key) {
+
+	uint32_t i, v0, v1, delta = 0x9E3779B9, sum = 0, k[4];
+
+	LOAD32L(k[0], &key[0]);
+	LOAD32L(k[1], &key[4]);
+	LOAD32L(k[2], &key[8]);
+	LOAD32L(k[3], &key[12]);
+
+	LOAD32L(v0, &block[0]);
+	LOAD32L(v1, &block[4]);
+
+	for (i = 0; i < XTEA_ROUNDS; i++) {
+		v0 += (((v1 << 4) ^ (v1 >> 5)) + v1) ^ (sum + k[sum & 3]);
+		sum += delta;
+		v1 += (((v0 << 4) ^ (v0 >> 5)) + v0) ^ (sum + k[(sum >> 11) & 3]);
+	}
+
+	STORE32L(v0, &block[0]);
+	STORE32L(v1, &block[4]);
+
+}
+
+static inline void csp_xtea_xor_byte(uint8_t * dst, uint8_t * src, uint32_t len) {
+
+	unsigned int i;
+	for (i = 0; i < len; i++)
+		dst[i] ^= src[i];
+
+}
+
+int csp_xtea_set_key(char * key, uint32_t keylen) {
+
+	/* Use SHA1 as KDF */
+	uint8_t hash[SHA1_DIGESTSIZE];
+	csp_sha1_memory((uint8_t *)key, keylen, hash);
+
+	/* Copy key */
+	memcpy(csp_xtea_key, hash, XTEA_KEY_LENGTH);
+
+	return CSP_ERR_NONE;
+
+}
+
+int csp_xtea_encrypt(uint8_t * plain, const uint32_t len, uint32_t iv[2]) {
+
+	unsigned int i;
+	uint32_t stream[2];
+
+	uint32_t blocks = (len + XTEA_BLOCKSIZE - 1)/ XTEA_BLOCKSIZE;
+	uint32_t remain;
+
+	/* Initialize stream */
+	stream[0] = csp_htobe32(iv[0]);
+	stream[1] = csp_htobe32(iv[1]);
+
+	for (i = 0; i < blocks; i++) {
+		/* Create stream */
+		csp_xtea_encrypt_block((uint8_t *)stream, (uint8_t *)csp_xtea_key);
+
+		/* Calculate remaining bytes */
+		remain = len - i * XTEA_BLOCKSIZE;
+
+		/* XOR plain text with stream to generate cipher text */
+		csp_xtea_xor_byte(&plain[len - remain], (uint8_t *)stream, remain < XTEA_BLOCKSIZE ? remain : XTEA_BLOCKSIZE);
+
+		/* Increment counter */
+		stream[0] = csp_htobe32(iv[0]);
+		stream[1] = csp_htobe32(iv[1]++);
+	}
+
+	return CSP_ERR_NONE;
+
+}
+
+int csp_xtea_decrypt(uint8_t * cipher, const uint32_t len, uint32_t iv[2]) {
+
+	/* Since we use counter mode, we can reuse the encryption function */
+	return csp_xtea_encrypt(cipher, len, iv);
+
+}
+
+#endif // CSP_USE_XTEA

thirdparty/libcsp/src/csp_bridge.c

@@ -0,0 +1,94 @@
+/*
+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 <csp/csp.h>
+#include <csp/csp_interface.h>
+#include <csp/arch/csp_thread.h>
+#include "csp_route.h"
+#include "csp_qfifo.h"
+#include "csp_io.h"
+#include "csp_promisc.h"
+
+static csp_iface_t* if_a = NULL;
+static csp_iface_t* if_b = NULL;
+
+static CSP_DEFINE_TASK(csp_bridge) {
+
+	csp_qfifo_t input;
+	csp_packet_t * packet;
+
+	/* Here there be bridging */
+	while (1) {
+
+		/* Get next packet to route */
+		if (csp_qfifo_read(&input) != CSP_ERR_NONE)
+			continue;
+
+		packet = input.packet;
+
+		csp_log_packet("Input: Src %u, Dst %u, Dport %u, Sport %u, Pri %u, Flags 0x%02X, Size %"PRIu16,
+				packet->id.src, packet->id.dst, packet->id.dport,
+				packet->id.sport, packet->id.pri, packet->id.flags, packet->length);
+
+		/* Here there be promiscuous mode */
+#ifdef CSP_USE_PROMISC
+		csp_promisc_add(packet);
+#endif
+
+		/* Find the opposing interface */
+		csp_iface_t * ifout;
+		if (input.interface == if_a) {
+			ifout = if_b;
+		} else {
+			ifout = if_a;
+		}
+
+		/* Send to the interface directly, no hassle */
+		if (csp_send_direct(packet->id, packet, ifout, 0) != CSP_ERR_NONE) {
+			csp_log_warn("Router failed to send");
+			csp_buffer_free(packet);
+		}
+
+		/* Next message, please */
+		continue;
+
+	}
+
+	return CSP_TASK_RETURN;
+
+}
+
+int csp_bridge_start(unsigned int task_stack_size, unsigned int task_priority, csp_iface_t * _if_a, csp_iface_t * _if_b) {
+
+	/* Set static references to A/B side of bridge */
+	if_a = _if_a;
+	if_b = _if_b;
+
+	static csp_thread_handle_t handle;
+	int ret = csp_thread_create(csp_bridge, "BRIDGE", task_stack_size, NULL, task_priority, &handle);
+
+	if (ret != 0) {
+		csp_log_error("Failed to start task");
+		return CSP_ERR_NOMEM;
+	}
+
+	return CSP_ERR_NONE;
+
+}

thirdparty/libcsp/src/csp_buffer.c

@@ -0,0 +1,224 @@
+/*
+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 <stdio.h>
+#include <stdint.h>
+#include <string.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+#include <csp/csp_error.h>
+#include <csp/arch/csp_queue.h>
+#include <csp/arch/csp_malloc.h>
+#include <csp/arch/csp_semaphore.h>
+
+#ifndef CSP_BUFFER_ALIGN
+#define CSP_BUFFER_ALIGN	(sizeof(int *))
+#endif
+
+typedef struct csp_skbf_s {
+	unsigned int refcount;
+	void * skbf_addr;
+	char skbf_data[];
+} csp_skbf_t;
+
+static csp_queue_handle_t csp_buffers;
+static char * csp_buffer_pool;
+static unsigned int count, size;
+
+CSP_DEFINE_CRITICAL(csp_critical_lock);
+
+int csp_buffer_init(int buf_count, int buf_size) {
+
+	unsigned int i;
+	csp_skbf_t * buf;
+
+	count = buf_count;
+	size = buf_size + CSP_BUFFER_PACKET_OVERHEAD;
+	unsigned int skbfsize = (sizeof(csp_skbf_t) + size);
+	skbfsize = CSP_BUFFER_ALIGN * ((skbfsize + CSP_BUFFER_ALIGN - 1) / CSP_BUFFER_ALIGN);
+	unsigned int poolsize = count * skbfsize;
+
+	csp_buffer_pool = csp_malloc(poolsize);
+	if (csp_buffer_pool == NULL)
+		goto fail_malloc;
+
+	csp_buffers = csp_queue_create(count, sizeof(void *));
+	if (!csp_buffers)
+		goto fail_queue;
+
+	if (CSP_INIT_CRITICAL(csp_critical_lock) != CSP_ERR_NONE)
+		goto fail_critical;
+
+	memset(csp_buffer_pool, 0, poolsize);
+
+	for (i = 0; i < count; i++) {
+
+		/* We have already taken care of pointer alignment since
+		 * skbfsize is an integer multiple of sizeof(int *)
+		 * but the explicit cast to a void * is still necessary
+		 * to tell the compiler so.
+		 */
+		buf = (void *) &csp_buffer_pool[i * skbfsize];
+		buf->refcount = 0;
+		buf->skbf_addr = buf;
+
+		csp_queue_enqueue(csp_buffers, &buf, 0);
+
+	}
+
+	return CSP_ERR_NONE;
+
+fail_critical:
+	csp_queue_remove(csp_buffers);
+fail_queue:
+	csp_free(csp_buffer_pool);
+fail_malloc:
+	return CSP_ERR_NOMEM;
+
+}
+
+void *csp_buffer_get_isr(size_t buf_size) {
+
+	csp_skbf_t * buffer = NULL;
+	CSP_BASE_TYPE task_woken = 0;
+
+	if (buf_size + CSP_BUFFER_PACKET_OVERHEAD > size)
+		return NULL;
+
+	csp_queue_dequeue_isr(csp_buffers, &buffer, &task_woken);
+	if (buffer == NULL)
+		return NULL;
+
+	if (buffer != buffer->skbf_addr)
+		return NULL;
+
+	buffer->refcount++;
+	return buffer->skbf_data;
+
+}
+
+void *csp_buffer_get(size_t buf_size) {
+
+	csp_skbf_t * buffer = NULL;
+
+	if (buf_size + CSP_BUFFER_PACKET_OVERHEAD > size) {
+		csp_log_error("Attempt to allocate too large block %u", buf_size);
+		return NULL;
+	}
+
+	csp_queue_dequeue(csp_buffers, &buffer, 0);
+	if (buffer == NULL) {
+		csp_log_error("Out of buffers");
+		return NULL;
+	}
+
+	csp_log_buffer("GET: %p %p", buffer, buffer->skbf_addr);
+
+	if (buffer != buffer->skbf_addr) {
+		csp_log_error("Corrupt CSP buffer");
+		return NULL;
+	}
+
+	buffer->refcount++;
+	return buffer->skbf_data;
+}
+
+void csp_buffer_free_isr(void *packet) {
+	CSP_BASE_TYPE task_woken = 0;
+	if (!packet)
+		return;
+
+	csp_skbf_t * buf = packet - sizeof(csp_skbf_t);
+
+	if (((uintptr_t) buf % CSP_BUFFER_ALIGN) > 0)
+		return;
+
+	if (buf->skbf_addr != buf)
+		return;
+
+	if (buf->refcount == 0) {
+		return;
+	} else if (buf->refcount > 1) {
+		buf->refcount--;
+		return;
+	} else {
+		buf->refcount = 0;
+		csp_queue_enqueue_isr(csp_buffers, &buf, &task_woken);
+	}
+
+}
+
+void csp_buffer_free(void *packet) {
+	if (!packet) {
+		csp_log_error("Attempt to free null pointer");
+		return;
+	}
+
+	csp_skbf_t * buf = packet - sizeof(csp_skbf_t);
+
+	if (((uintptr_t) buf % CSP_BUFFER_ALIGN) > 0) {
+		csp_log_error("FREE: Unaligned CSP buffer pointer %p", packet);
+		return;
+	}
+
+	if (buf->skbf_addr != buf) {
+		csp_log_error("FREE: Invalid CSP buffer pointer %p", packet);
+		return;
+	}
+
+	if (buf->refcount == 0) {
+		csp_log_error("FREE: Buffer already free %p", buf);
+		return;
+	} else if (buf->refcount > 1) {
+		buf->refcount--;
+		csp_log_error("FREE: Buffer %p in use by %u users", buf, buf->refcount);
+		return;
+	} else {
+		buf->refcount = 0;
+		csp_log_buffer("FREE: %p", buf);
+		csp_queue_enqueue(csp_buffers, &buf, 0);
+	}
+
+}
+
+void *csp_buffer_clone(void *buffer) {
+
+	csp_packet_t *packet = (csp_packet_t *) buffer;
+
+	if (!packet)
+		return NULL;
+
+	csp_packet_t *clone = csp_buffer_get(packet->length);
+
+	if (clone)
+		memcpy(clone, packet, size);
+
+	return clone;
+
+}
+
+int csp_buffer_remaining(void) {
+	return csp_queue_size(csp_buffers);
+}
+
+int csp_buffer_size(void) {
+	return size;
+}

thirdparty/libcsp/src/csp_conn.c

@@ -0,0 +1,498 @@
+/*
+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 <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+#include <csp/csp_error.h>
+
+#include <csp/arch/csp_thread.h>
+#include <csp/arch/csp_queue.h>
+#include <csp/arch/csp_semaphore.h>
+#include <csp/arch/csp_malloc.h>
+#include <csp/arch/csp_time.h>
+
+#include "csp_conn.h"
+#include "transport/csp_transport.h"
+
+/* Static connection pool */
+static csp_conn_t arr_conn[CSP_CONN_MAX];
+
+/* Connection pool lock */
+static csp_bin_sem_handle_t conn_lock;
+
+/* Source port */
+static uint8_t sport;
+
+/* Source port lock */
+static csp_bin_sem_handle_t sport_lock;
+
+void csp_conn_check_timeouts(void) {
+#ifdef CSP_USE_RDP
+	int i;
+	for (i = 0; i < CSP_CONN_MAX; i++)
+		if (arr_conn[i].state == CONN_OPEN)
+			if (arr_conn[i].idin.flags & CSP_FRDP)
+				csp_rdp_check_timeouts(&arr_conn[i]);
+#endif
+}
+
+int csp_conn_get_rxq(int prio) {
+
+#ifdef CSP_USE_QOS
+	return prio;
+#else
+	return 0;
+#endif
+
+}
+
+int csp_conn_lock(csp_conn_t * conn, uint32_t timeout) {
+
+	if (csp_mutex_lock(&conn->lock, timeout) != CSP_MUTEX_OK)
+		return CSP_ERR_TIMEDOUT;
+
+	return CSP_ERR_NONE;
+
+}
+
+int csp_conn_unlock(csp_conn_t * conn) {
+
+	csp_mutex_unlock(&conn->lock);
+
+	return CSP_ERR_NONE;
+
+}
+
+int csp_conn_enqueue_packet(csp_conn_t * conn, csp_packet_t * packet) {
+
+	if (!conn)
+		return CSP_ERR_INVAL;
+
+	int rxq;
+	if (packet != NULL) {
+		rxq = csp_conn_get_rxq(packet->id.pri);
+	} else {
+		rxq = CSP_RX_QUEUES - 1;
+	}
+
+	if (csp_queue_enqueue(conn->rx_queue[rxq], &packet, 0) != CSP_QUEUE_OK) {
+		csp_log_error("RX queue %p full with %u items", conn->rx_queue[rxq], csp_queue_size(conn->rx_queue[rxq]));
+		return CSP_ERR_NOMEM;
+	}
+
+#ifdef CSP_USE_QOS
+	int event = 0;
+	if (csp_queue_enqueue(conn->rx_event, &event, 0) != CSP_QUEUE_OK) {
+		csp_log_error("QOS event queue full");
+		return CSP_ERR_NOMEM;
+	}
+#endif
+
+	return CSP_ERR_NONE;
+}
+
+int csp_conn_init(void) {
+
+	/* Initialize source port */
+	srand(csp_get_ms());
+	sport = (rand() % (CSP_ID_PORT_MAX - CSP_MAX_BIND_PORT)) + (CSP_MAX_BIND_PORT + 1);
+
+	if (csp_bin_sem_create(&sport_lock) != CSP_SEMAPHORE_OK) {
+		csp_log_error("No more memory for sport semaphore");
+		return CSP_ERR_NOMEM;
+	}
+
+	int i, prio;
+	for (i = 0; i < CSP_CONN_MAX; i++) {
+		for (prio = 0; prio < CSP_RX_QUEUES; prio++)
+			arr_conn[i].rx_queue[prio] = csp_queue_create(CSP_RX_QUEUE_LENGTH, sizeof(csp_packet_t *));
+
+#ifdef CSP_USE_QOS
+		arr_conn[i].rx_event = csp_queue_create(CSP_CONN_QUEUE_LENGTH, sizeof(int));
+#endif
+		arr_conn[i].state = CONN_CLOSED;
+
+		if (csp_mutex_create(&arr_conn[i].lock) != CSP_MUTEX_OK) {
+			csp_log_error("Failed to create connection lock");
+			return CSP_ERR_NOMEM;
+		}
+
+#ifdef CSP_USE_RDP
+		if (csp_rdp_allocate(&arr_conn[i]) != CSP_ERR_NONE) {
+			csp_log_error("Failed to create queues for RDP in csp_conn_init");
+			return CSP_ERR_NOMEM;
+		}
+#endif
+	}
+
+	if (csp_bin_sem_create(&conn_lock) != CSP_SEMAPHORE_OK) {
+		csp_log_error("No more memory for conn semaphore");
+		return CSP_ERR_NOMEM;
+	}
+
+	return CSP_ERR_NONE;
+
+}
+
+csp_conn_t * csp_conn_find(uint32_t id, uint32_t mask) {
+
+	/* Search for matching connection */
+	int i;
+	csp_conn_t * conn;
+	id = (id & mask);
+	for (i = 0; i < CSP_CONN_MAX; i++) {
+		conn = &arr_conn[i];
+		if ((conn->state != CONN_CLOSED) && (conn->type == CONN_CLIENT) && ((conn->idin.ext & mask) == id))
+			return conn;
+	}
+	
+	return NULL;
+
+}
+
+static int csp_conn_flush_rx_queue(csp_conn_t * conn) {
+
+	csp_packet_t * packet;
+
+	int prio;
+
+	/* Flush packet queues */
+	for (prio = 0; prio < CSP_RX_QUEUES; prio++) {
+		while (csp_queue_dequeue(conn->rx_queue[prio], &packet, 0) == CSP_QUEUE_OK)
+			if (packet != NULL)
+				csp_buffer_free(packet);
+	}
+
+	/* Flush event queue */
+#ifdef CSP_USE_QOS
+	int event;
+	while (csp_queue_dequeue(conn->rx_event, &event, 0) == CSP_QUEUE_OK);
+#endif
+
+	return CSP_ERR_NONE;
+
+}
+
+csp_conn_t * csp_conn_allocate(csp_conn_type_t type) {
+
+	int i, j;
+	static uint8_t csp_conn_last_given = 0;
+	csp_conn_t * conn;
+
+	if (csp_bin_sem_wait(&conn_lock, 100) != CSP_SEMAPHORE_OK) {
+		csp_log_error("Failed to lock conn array");
+		return NULL;
+	}
+
+	/* Search for free connection */
+	i = csp_conn_last_given;
+	i = (i + 1) % CSP_CONN_MAX;
+
+	for (j = 0; j < CSP_CONN_MAX; j++) {
+		conn = &arr_conn[i];
+		if (conn->state == CONN_CLOSED)
+			break;
+		i = (i + 1) % CSP_CONN_MAX;
+	}
+
+	if (conn->state == CONN_OPEN) {
+		csp_log_error("No more free connections");
+		csp_bin_sem_post(&conn_lock);
+		return NULL;
+	}
+
+	conn->idin.ext = 0;
+	conn->idout.ext = 0;
+	conn->socket = NULL;
+        conn->timestamp = 0;
+	conn->type = type;
+	conn->state = CONN_OPEN;
+
+	csp_conn_last_given = i;
+	csp_bin_sem_post(&conn_lock);
+
+	return conn;
+
+}
+
+csp_conn_t * csp_conn_new(csp_id_t idin, csp_id_t idout) {
+
+	/* Allocate connection structure */
+	csp_conn_t * conn = csp_conn_allocate(CONN_CLIENT);
+
+	if (conn) {
+		/* No lock is needed here, because nobody else *
+		 * has a reference to this connection yet.     */
+		conn->idin.ext = idin.ext;
+		conn->idout.ext = idout.ext;
+		conn->timestamp = csp_get_ms();
+
+		/* Ensure connection queue is empty */
+		csp_conn_flush_rx_queue(conn);
+	}
+
+	return conn;
+
+}
+
+int csp_close(csp_conn_t * conn) {
+
+	if (conn == NULL) {
+		csp_log_error("NULL Pointer given to csp_close");
+		return CSP_ERR_INVAL;
+	}
+
+	if (conn->state == CONN_CLOSED) {
+		csp_log_protocol("Conn already closed");
+		return CSP_ERR_NONE;
+	}
+
+#ifdef CSP_USE_RDP
+	/* Ensure RDP knows this connection is closing */
+	if (conn->idin.flags & CSP_FRDP || conn->idout.flags & CSP_FRDP)
+		if (csp_rdp_close(conn) == CSP_ERR_AGAIN)
+			return CSP_ERR_NONE;
+#endif
+
+	/* Lock connection array while closing connection */
+	if (csp_bin_sem_wait(&conn_lock, 100) != CSP_SEMAPHORE_OK) {
+		csp_log_error("Failed to lock conn array");
+		return CSP_ERR_TIMEDOUT;
+	}
+
+	/* Set to closed */
+	conn->state = CONN_CLOSED;
+
+	/* Ensure connection queue is empty */
+	csp_conn_flush_rx_queue(conn);
+
+        if (conn->socket && (conn->type == CONN_SERVER) && (conn->opts & (CSP_SO_CONN_LESS | CSP_SO_INTERNAL_LISTEN))) {
+		csp_queue_remove(conn->socket);
+		conn->socket = NULL;
+        }
+
+	/* Reset RDP state */
+#ifdef CSP_USE_RDP
+	if (conn->idin.flags & CSP_FRDP)
+		csp_rdp_flush_all(conn);
+#endif
+
+	/* Unlock connection array */
+	csp_bin_sem_post(&conn_lock);
+
+	return CSP_ERR_NONE;
+}
+
+csp_conn_t * csp_connect(uint8_t prio, uint8_t dest, uint8_t dport, uint32_t timeout, uint32_t opts) {
+
+	/* Force options on all connections */
+	opts |= CSP_CONNECTION_SO;
+
+	/* Generate identifier */
+	csp_id_t incoming_id, outgoing_id;
+	incoming_id.pri = prio;
+	incoming_id.dst = csp_get_address();
+	incoming_id.src = dest;
+	incoming_id.sport = dport;
+	incoming_id.flags = 0;
+	outgoing_id.pri = prio;
+	outgoing_id.dst = dest;
+	outgoing_id.src = csp_get_address();
+	outgoing_id.dport = dport;
+	outgoing_id.flags = 0;
+
+	/* Set connection options */
+        if (opts & CSP_O_NOCRC32) {
+                opts &= ~CSP_O_CRC32;
+        }
+
+	if (opts & CSP_O_RDP) {
+#ifdef CSP_USE_RDP
+		incoming_id.flags |= CSP_FRDP;
+		outgoing_id.flags |= CSP_FRDP;
+#else
+		csp_log_error("Attempt to create RDP connection, but CSP was compiled without RDP support");
+		return NULL;
+#endif
+	}
+
+	if (opts & CSP_O_HMAC) {
+#ifdef CSP_USE_HMAC
+		outgoing_id.flags |= CSP_FHMAC;
+		incoming_id.flags |= CSP_FHMAC;
+#else
+		csp_log_error("Attempt to create HMAC authenticated connection, but CSP was compiled without HMAC support");
+		return NULL;
+#endif
+	}
+
+	if (opts & CSP_O_XTEA) {
+#ifdef CSP_USE_XTEA
+		outgoing_id.flags |= CSP_FXTEA;
+		incoming_id.flags |= CSP_FXTEA;
+#else
+		csp_log_error("Attempt to create XTEA encrypted connection, but CSP was compiled without XTEA support");
+		return NULL;
+#endif
+	}
+
+	if (opts & CSP_O_CRC32) {
+#ifdef CSP_USE_CRC32
+		outgoing_id.flags |= CSP_FCRC32;
+		incoming_id.flags |= CSP_FCRC32;
+#else
+		csp_log_error("Attempt to create CRC32 validated connection, but CSP was compiled without CRC32 support");
+		return NULL;
+#endif
+	}
+
+	/* Find an unused ephemeral port */
+	csp_conn_t * conn = NULL;
+
+	/* Wait for sport lock - note that csp_conn_new(..) is called inside the lock! */
+	if (csp_bin_sem_wait(&sport_lock, 1000) != CSP_SEMAPHORE_OK)
+		return NULL;
+
+	const uint8_t start = sport;
+	while (++sport != start) {
+		if (sport > CSP_ID_PORT_MAX)
+			sport = CSP_MAX_BIND_PORT + 1;
+
+		outgoing_id.sport = sport;
+		incoming_id.dport = sport;
+
+		/* Match on destination port of _incoming_ identifier */
+		if (csp_conn_find(incoming_id.ext, CSP_ID_DPORT_MASK) == NULL) {
+			/* Break - we found an unused ephemeral port
+                           allocate connection while locked to mark port in use */
+			conn = csp_conn_new(incoming_id, outgoing_id);
+			break;
+		}
+	}
+
+	/* Post sport lock */
+	csp_bin_sem_post(&sport_lock);
+
+	if (conn == NULL)
+		return NULL;
+
+	/* Set connection options */
+	conn->opts = opts;
+
+#ifdef CSP_USE_RDP
+	/* Call Transport Layer connect */
+	if (outgoing_id.flags & CSP_FRDP) {
+		/* If the transport layer has failed to connect
+		 * deallocate connection structure again and return NULL */
+		if (csp_rdp_connect(conn, timeout) != CSP_ERR_NONE) {
+			csp_close(conn);
+			return NULL;
+		}
+	}
+#endif
+
+	/* We have a successful connection */
+	return conn;
+
+}
+
+inline int csp_conn_dport(csp_conn_t * conn) {
+
+	return conn->idin.dport;
+
+}
+
+inline int csp_conn_sport(csp_conn_t * conn) {
+
+	return conn->idin.sport;
+
+}
+
+inline int csp_conn_dst(csp_conn_t * conn) {
+
+	return conn->idin.dst;
+
+}
+
+inline int csp_conn_src(csp_conn_t * conn) {
+
+	return conn->idin.src;
+
+}
+
+inline int csp_conn_flags(csp_conn_t * conn) {
+
+	return conn->idin.flags;
+
+}
+
+#ifdef CSP_DEBUG
+void csp_conn_print_table(void) {
+
+	int i;
+	csp_conn_t * conn;
+
+	for (i = 0; i < CSP_CONN_MAX; i++) {
+		conn = &arr_conn[i];
+		printf("[%02u %p] S:%u, %u -> %u, %u -> %u, sock: %p\r\n",
+				i, conn, conn->state, conn->idin.src, conn->idin.dst,
+				conn->idin.dport, conn->idin.sport, conn->socket);
+#ifdef CSP_USE_RDP
+		if (conn->idin.flags & CSP_FRDP)
+			csp_rdp_conn_print(conn);
+#endif
+	}
+}
+
+int csp_conn_print_table_str(char * str_buf, int str_size) {
+
+	int i, start = 0;
+	csp_conn_t * conn;
+	char buf[100];
+
+	/* Display up to 10 connections */
+	if (CSP_CONN_MAX - 10 > 0)
+		start = CSP_CONN_MAX - 10;
+
+	for (i = start; i < CSP_CONN_MAX; i++) {
+		conn = &arr_conn[i];
+		snprintf(buf, sizeof(buf), "[%02u %p] S:%u, %u -> %u, %u -> %u, sock: %p\n",
+				i, conn, conn->state, conn->idin.src, conn->idin.dst,
+				conn->idin.dport, conn->idin.sport, conn->socket);
+
+		strncat(str_buf, buf, str_size);
+		if ((str_size -= strlen(buf)) <= 0)
+			break;
+	}
+
+	return CSP_ERR_NONE;
+}
+#endif
+
+const csp_conn_t * csp_conn_get_array(size_t * size)
+{
+	*size = CSP_CONN_MAX;    
+	return arr_conn;
+}

thirdparty/libcsp/src/csp_conn.h

@@ -0,0 +1,112 @@
+/*
+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
+*/
+
+#ifndef _CSP_CONN_H_
+#define _CSP_CONN_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+#include <csp/csp.h>
+
+#include <csp/arch/csp_queue.h>
+#include <csp/arch/csp_semaphore.h>
+
+/** @brief Connection states */
+typedef enum {
+	CONN_CLOSED = 0,
+	CONN_OPEN = 1,
+} csp_conn_state_t;
+
+/** @brief Connection types */
+typedef enum {
+	CONN_CLIENT = 0,
+	CONN_SERVER = 1,
+} csp_conn_type_t;
+
+typedef enum {
+	RDP_CLOSED = 0,
+	RDP_SYN_SENT,
+	RDP_SYN_RCVD,
+	RDP_OPEN,
+	RDP_CLOSE_WAIT,
+} csp_rdp_state_t;
+
+/** @brief RDP Connection header
+ *  @note Do not try to pack this struct, the posix sem handle will stop working */
+typedef struct {
+	csp_rdp_state_t state;		/**< Connection state */
+	uint16_t snd_nxt;		/**< The sequence number of the next segment that is to be sent */
+	uint16_t snd_una;		/**< The sequence number of the oldest unacknowledged segment */
+	uint16_t snd_iss;		/**< The initial send sequence number */
+	uint16_t rcv_cur;		/**< The sequence number of the last segment received correctly and in sequence */
+	uint16_t rcv_irs;		/**< The initial receive sequence number */
+	uint16_t rcv_lsa;		/**< The last sequence number acknowledged by the receiver */
+	uint32_t window_size;
+	uint32_t conn_timeout;
+	uint32_t packet_timeout;
+	uint32_t delayed_acks;
+	uint32_t ack_timeout;
+	uint32_t ack_delay_count;
+	uint32_t ack_timestamp;
+	csp_bin_sem_handle_t tx_wait;
+	csp_queue_handle_t tx_queue;
+	csp_queue_handle_t rx_queue;
+} csp_rdp_t;
+
+/** @brief Connection struct */
+struct csp_conn_s {
+	csp_conn_type_t type;		/* Connection type (CONN_CLIENT or CONN_SERVER) */
+	csp_conn_state_t state;		/* Connection state (CONN_OPEN or CONN_CLOSED) */
+	csp_mutex_t lock;		/* Connection structure lock */
+	csp_id_t idin;			/* Identifier received */
+	csp_id_t idout;			/* Identifier transmitted */
+#ifdef CSP_USE_QOS
+	csp_queue_handle_t rx_event;	/* Event queue for RX packets */
+#endif
+	csp_queue_handle_t rx_queue[CSP_RX_QUEUES]; /* Queue for RX packets */
+	csp_queue_handle_t socket;	/* Socket to be "woken" when first packet is ready */
+	uint32_t timestamp;		/* Time the connection was opened */
+	uint32_t opts;			/* Connection or socket options */
+#ifdef CSP_USE_RDP
+	csp_rdp_t rdp;			/* RDP state */
+#endif
+};
+
+int csp_conn_lock(csp_conn_t * conn, uint32_t timeout);
+int csp_conn_unlock(csp_conn_t * conn);
+int csp_conn_enqueue_packet(csp_conn_t * conn, csp_packet_t * packet);
+int csp_conn_init(void);
+csp_conn_t * csp_conn_allocate(csp_conn_type_t type);
+csp_conn_t * csp_conn_find(uint32_t id, uint32_t mask);
+csp_conn_t * csp_conn_new(csp_id_t idin, csp_id_t idout);
+void csp_conn_check_timeouts(void);
+int csp_conn_get_rxq(int prio);
+
+const csp_conn_t * csp_conn_get_array(size_t * size); // for test purposes only!
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // _CSP_CONN_H_

thirdparty/libcsp/src/csp_crc32.c

@@ -0,0 +1,140 @@
+/*
+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 <stdint.h>
+#include <string.h>
+#include <inttypes.h>
+
+#include <csp/csp.h>
+#include <csp/csp_endian.h>
+
+#include <csp/csp_crc32.h>
+
+#ifdef CSP_USE_CRC32
+
+#ifdef __AVR__
+#include <avr/pgmspace.h>
+static const uint32_t crc_tab[256] PROGMEM = {
+#else
+static const uint32_t crc_tab[256] = {
+#endif
+		0x00000000, 0xF26B8303, 0xE13B70F7, 0x1350F3F4, 0xC79A971F, 0x35F1141C, 0x26A1E7E8, 0xD4CA64EB,
+		0x8AD958CF, 0x78B2DBCC, 0x6BE22838, 0x9989AB3B, 0x4D43CFD0, 0xBF284CD3, 0xAC78BF27, 0x5E133C24,
+		0x105EC76F, 0xE235446C, 0xF165B798, 0x030E349B, 0xD7C45070, 0x25AFD373, 0x36FF2087, 0xC494A384,
+		0x9A879FA0, 0x68EC1CA3, 0x7BBCEF57, 0x89D76C54, 0x5D1D08BF, 0xAF768BBC, 0xBC267848, 0x4E4DFB4B,
+		0x20BD8EDE, 0xD2D60DDD, 0xC186FE29, 0x33ED7D2A, 0xE72719C1, 0x154C9AC2, 0x061C6936, 0xF477EA35,
+		0xAA64D611,	0x580F5512, 0x4B5FA6E6, 0xB93425E5, 0x6DFE410E, 0x9F95C20D, 0x8CC531F9, 0x7EAEB2FA,
+		0x30E349B1,	0xC288CAB2, 0xD1D83946, 0x23B3BA45, 0xF779DEAE, 0x05125DAD, 0x1642AE59, 0xE4292D5A,
+		0xBA3A117E,	0x4851927D, 0x5B016189, 0xA96AE28A, 0x7DA08661, 0x8FCB0562, 0x9C9BF696, 0x6EF07595,
+		0x417B1DBC,	0xB3109EBF, 0xA0406D4B, 0x522BEE48, 0x86E18AA3, 0x748A09A0, 0x67DAFA54, 0x95B17957,
+		0xCBA24573,	0x39C9C670, 0x2A993584, 0xD8F2B687, 0x0C38D26C, 0xFE53516F, 0xED03A29B, 0x1F682198,
+		0x5125DAD3,	0xA34E59D0, 0xB01EAA24, 0x42752927, 0x96BF4DCC, 0x64D4CECF, 0x77843D3B, 0x85EFBE38,
+		0xDBFC821C,	0x2997011F, 0x3AC7F2EB, 0xC8AC71E8, 0x1C661503, 0xEE0D9600, 0xFD5D65F4, 0x0F36E6F7,
+		0x61C69362,	0x93AD1061, 0x80FDE395, 0x72966096, 0xA65C047D, 0x5437877E, 0x4767748A, 0xB50CF789,
+		0xEB1FCBAD,	0x197448AE, 0x0A24BB5A, 0xF84F3859, 0x2C855CB2, 0xDEEEDFB1, 0xCDBE2C45, 0x3FD5AF46,
+		0x7198540D,	0x83F3D70E, 0x90A324FA, 0x62C8A7F9, 0xB602C312, 0x44694011, 0x5739B3E5, 0xA55230E6,
+		0xFB410CC2,	0x092A8FC1, 0x1A7A7C35, 0xE811FF36, 0x3CDB9BDD, 0xCEB018DE, 0xDDE0EB2A, 0x2F8B6829,
+		0x82F63B78,	0x709DB87B, 0x63CD4B8F, 0x91A6C88C, 0x456CAC67, 0xB7072F64, 0xA457DC90, 0x563C5F93,
+		0x082F63B7, 0xFA44E0B4, 0xE9141340, 0x1B7F9043, 0xCFB5F4A8, 0x3DDE77AB, 0x2E8E845F, 0xDCE5075C,
+		0x92A8FC17, 0x60C37F14, 0x73938CE0, 0x81F80FE3, 0x55326B08, 0xA759E80B, 0xB4091BFF, 0x466298FC,
+		0x1871A4D8, 0xEA1A27DB, 0xF94AD42F, 0x0B21572C, 0xDFEB33C7, 0x2D80B0C4, 0x3ED04330, 0xCCBBC033,
+		0xA24BB5A6, 0x502036A5, 0x4370C551, 0xB11B4652, 0x65D122B9, 0x97BAA1BA, 0x84EA524E, 0x7681D14D,
+		0x2892ED69, 0xDAF96E6A, 0xC9A99D9E, 0x3BC21E9D, 0xEF087A76, 0x1D63F975, 0x0E330A81, 0xFC588982,
+		0xB21572C9, 0x407EF1CA, 0x532E023E, 0xA145813D, 0x758FE5D6, 0x87E466D5, 0x94B49521, 0x66DF1622,
+		0x38CC2A06, 0xCAA7A905, 0xD9F75AF1, 0x2B9CD9F2, 0xFF56BD19, 0x0D3D3E1A, 0x1E6DCDEE, 0xEC064EED,
+		0xC38D26C4, 0x31E6A5C7, 0x22B65633, 0xD0DDD530, 0x0417B1DB, 0xF67C32D8, 0xE52CC12C, 0x1747422F,
+		0x49547E0B, 0xBB3FFD08, 0xA86F0EFC, 0x5A048DFF, 0x8ECEE914, 0x7CA56A17, 0x6FF599E3, 0x9D9E1AE0,
+		0xD3D3E1AB, 0x21B862A8, 0x32E8915C, 0xC083125F, 0x144976B4, 0xE622F5B7, 0xF5720643, 0x07198540,
+		0x590AB964, 0xAB613A67, 0xB831C993, 0x4A5A4A90, 0x9E902E7B, 0x6CFBAD78, 0x7FAB5E8C, 0x8DC0DD8F,
+		0xE330A81A, 0x115B2B19, 0x020BD8ED, 0xF0605BEE, 0x24AA3F05, 0xD6C1BC06, 0xC5914FF2, 0x37FACCF1,
+		0x69E9F0D5, 0x9B8273D6, 0x88D28022, 0x7AB90321, 0xAE7367CA, 0x5C18E4C9, 0x4F48173D, 0xBD23943E,
+		0xF36E6F75, 0x0105EC76, 0x12551F82, 0xE03E9C81, 0x34F4F86A, 0xC69F7B69, 0xD5CF889D, 0x27A40B9E,
+		0x79B737BA, 0x8BDCB4B9, 0x988C474D, 0x6AE7C44E, 0xBE2DA0A5, 0x4C4623A6, 0x5F16D052, 0xAD7D5351 };
+
+uint32_t csp_crc32_memory(const uint8_t * data, uint32_t length) {
+   uint32_t crc;
+
+   crc = 0xFFFFFFFF;
+   while (length--)
+#ifdef __AVR__
+	   crc = pgm_read_dword(&crc_tab[(crc ^ *data++) & 0xFFL]) ^ (crc >> 8);
+#else
+	   crc = crc_tab[(crc ^ *data++) & 0xFFL] ^ (crc >> 8);
+#endif
+
+   return (crc ^ 0xFFFFFFFF);
+}
+
+int csp_crc32_append(csp_packet_t * packet, bool include_header) {
+
+	uint32_t crc;
+
+	/* NULL pointer check */
+	if (packet == NULL)
+		return CSP_ERR_INVAL;
+
+	/* Calculate CRC32, convert to network byte order */
+	if (include_header) {
+		crc = csp_crc32_memory((uint8_t *) &packet->id, packet->length + sizeof(packet->id));
+	} else {
+		crc = csp_crc32_memory(packet->data, packet->length);
+	}
+	crc = csp_hton32(crc);
+
+	/* Copy checksum to packet */
+	memcpy(&packet->data[packet->length], &crc, sizeof(uint32_t));
+	packet->length += sizeof(uint32_t);
+
+	return CSP_ERR_NONE;
+
+}
+
+int csp_crc32_verify(csp_packet_t * packet, bool include_header) {
+
+	uint32_t crc;
+
+	/* NULL pointer check */
+	if (packet == NULL)
+		return CSP_ERR_INVAL;
+
+	if (packet->length < sizeof(uint32_t))
+		return CSP_ERR_INVAL;
+
+	/* Calculate CRC32, convert to network byte order */
+	if (include_header) {
+		crc = csp_crc32_memory((uint8_t *) &packet->id, packet->length + sizeof(packet->id) - sizeof(uint32_t));
+	} else {
+		crc = csp_crc32_memory(packet->data, packet->length - sizeof(uint32_t));
+	}
+	crc = csp_hton32(crc);
+
+	/* Compare calculated checksum with packet header */
+	if (memcmp(&packet->data[packet->length] - sizeof(uint32_t), &crc, sizeof(uint32_t)) != 0) {
+		/* CRC32 failed */
+		return CSP_ERR_INVAL;
+	} else {
+		/* Strip CRC32 */
+		packet->length -= sizeof(uint32_t);
+		return CSP_ERR_NONE;
+	}
+
+}
+
+#endif // CSP_USE_CRC32

thirdparty/libcsp/src/csp_debug.c

@@ -0,0 +1,133 @@
+/*
+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 <stdint.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <string.h>
+
+#ifdef __AVR__
+#include <avr/pgmspace.h>
+#endif
+
+/* CSP includes */
+#include <csp/csp.h>
+
+#include <csp/arch/csp_system.h>
+
+/* Custom debug function */
+csp_debug_hook_func_t csp_debug_hook_func = NULL;
+
+/* Debug levels */
+static bool csp_debug_level_enabled[] = {
+	[CSP_ERROR]	= true,
+	[CSP_WARN]	= true,
+	[CSP_INFO]	= false,
+	[CSP_BUFFER]	= false,
+	[CSP_PACKET]	= false,
+	[CSP_PROTOCOL]	= false,
+	[CSP_LOCK]	= false,
+};
+
+/* Some compilers do not support weak symbols, so this function
+ * can be used instead to set a custom debug hook */
+void csp_debug_hook_set(csp_debug_hook_func_t f)
+{
+	csp_debug_hook_func = f;
+}
+
+void do_csp_debug(csp_debug_level_t level, const char *format, ...)
+{
+	int color = COLOR_RESET;
+	va_list args;
+
+	/* Don't print anything if log level is disabled */
+	if (level > CSP_LOCK || !csp_debug_level_enabled[level])
+		return;
+
+	switch(level) {
+	case CSP_INFO:
+		color = COLOR_GREEN | COLOR_BOLD;
+		break;
+	case CSP_ERROR:
+		color = COLOR_RED | COLOR_BOLD;
+		break;
+	case CSP_WARN:
+		color = COLOR_YELLOW | COLOR_BOLD;
+		break;
+	case CSP_BUFFER:
+		color = COLOR_MAGENTA;
+		break;
+	case CSP_PACKET:
+		color = COLOR_GREEN;
+		break;
+	case CSP_PROTOCOL:
+		color = COLOR_BLUE;
+		break;
+	case CSP_LOCK:
+		color = COLOR_CYAN;
+		break;
+	default:
+		return;
+	}
+
+	va_start(args, format);
+
+	/* If csp_debug_hook symbol is defined, pass on the message.
+	 * Otherwise, just print with pretty colors ... */
+	if (csp_debug_hook_func) {
+		csp_debug_hook_func(level, format, args);
+	} else {
+		csp_sys_set_color(color);
+#ifdef __AVR__
+		vfprintf_P(stdout, format, args);
+#else
+		vprintf(format, args);
+#endif
+		printf("\r\n");
+		csp_sys_set_color(COLOR_RESET);
+	}
+
+	va_end(args);
+}
+
+void csp_debug_set_level(csp_debug_level_t level, bool value)
+{
+	if (level > CSP_LOCK)
+		return;
+	csp_debug_level_enabled[level] = value;
+}
+
+int csp_debug_get_level(csp_debug_level_t level)
+{
+	if (level > CSP_LOCK)
+		return 0;
+	return csp_debug_level_enabled[level];
+}
+
+void csp_debug_toggle_level(csp_debug_level_t level)
+{
+	if (level > CSP_LOCK) {
+		printf("Max level is 6\r\n");
+		return;
+	}
+	csp_debug_level_enabled[level] = (csp_debug_level_enabled[level]) ? false : true;
+	printf("Level %u: value %u\r\n", level, csp_debug_level_enabled[level]);
+}

thirdparty/libcsp/src/csp_dedup.c

@@ -0,0 +1,66 @@
+/*
+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 <stdlib.h>
+#include <stddef.h>
+#include <stdio.h>
+#include <inttypes.h>
+
+#include <csp/csp.h>
+#include <csp/arch/csp_time.h>
+#include <csp/csp_crc32.h>
+
+#include "csp_dedup.h"
+
+/* Check the last CSP_DEDUP_COUNT packets for duplicates */
+#define CSP_DEDUP_COUNT		16
+
+/* Only consider packet a duplicate if received under CSP_DEDUP_WINDOW_MS ago */
+#define CSP_DEDUP_WINDOW_MS	1000
+
+/* Store packet CRC's in a ringbuffer */
+static uint32_t csp_dedup_array[CSP_DEDUP_COUNT] = {};
+static uint32_t csp_dedup_timestamp[CSP_DEDUP_COUNT] = {};
+static int csp_dedup_in = 0;
+
+bool csp_dedup_is_duplicate(csp_packet_t *packet)
+{
+	/* Calculate CRC32 for packet */
+	uint32_t crc = csp_crc32_memory((const uint8_t *) &packet->id, packet->length + sizeof(packet->id));
+
+	/* Check if we have received this packet before */
+	for (int i = 0; i < CSP_DEDUP_COUNT; i++) {
+
+		/* Check for match */
+		if (crc == csp_dedup_array[i]) {
+
+			/* Check the timestamp */
+			if (csp_get_ms() < csp_dedup_timestamp[i] + CSP_DEDUP_WINDOW_MS)
+				return true;
+		}
+	}
+
+	/* If not, insert packet into duplicate list */
+	csp_dedup_array[csp_dedup_in] = crc;
+	csp_dedup_timestamp[csp_dedup_in] = csp_get_ms();
+	csp_dedup_in = (csp_dedup_in + 1) % CSP_DEDUP_COUNT;
+
+	return false;
+}

thirdparty/libcsp/src/csp_dedup.h

@@ -0,0 +1,31 @@
+/*
+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
+*/
+
+#ifndef CSP_DEDUP_H_
+#define CSP_DEDUP_H_
+
+/**
+ * Check for a duplicate packet
+ * @param packet pointer to packet
+ * @return false if not a duplicate, true if duplicate
+ */
+bool csp_dedup_is_duplicate(csp_packet_t *packet);
+
+#endif /* CSP_DEDUP_H_ */

thirdparty/libcsp/src/csp_endian.c

@@ -0,0 +1,204 @@
+/*
+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 <stdint.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+#include <csp/csp_endian.h>
+
+/* Convert 16-bit number from host byte order to network byte order */
+inline uint16_t __attribute__ ((__const__)) csp_hton16(uint16_t h16) {
+#ifdef CSP_BIG_ENDIAN
+	return h16;
+#else
+	return (((h16 & 0xff00) >> 8) |
+			((h16 & 0x00ff) << 8));
+#endif
+}
+
+/* Convert 16-bit number from network byte order to host byte order */
+inline uint16_t __attribute__ ((__const__)) csp_ntoh16(uint16_t n16) {
+	return csp_hton16(n16);
+}
+
+/* Convert 32-bit number from host byte order to network byte order */
+inline uint32_t __attribute__ ((__const__)) csp_hton32(uint32_t h32) {
+#ifdef CSP_BIG_ENDIAN
+	return h32;
+#else
+	return (((h32 & 0xff000000) >> 24) |
+			((h32 & 0x000000ff) << 24) |
+			((h32 & 0x0000ff00) <<  8) |
+			((h32 & 0x00ff0000) >>  8));
+#endif
+}
+
+/* Convert 32-bit number from network byte order to host byte order */
+inline uint32_t __attribute__ ((__const__)) csp_ntoh32(uint32_t n32) {
+	return csp_hton32(n32);
+}
+
+/* Convert 64-bit number from host byte order to network byte order */
+inline uint64_t __attribute__ ((__const__)) csp_hton64(uint64_t h64) {
+#ifdef CSP_BIG_ENDIAN
+	return h64;
+#else
+	return (((h64 & 0xff00000000000000LL) >> 56) |
+			((h64 & 0x00000000000000ffLL) << 56) |
+			((h64 & 0x00ff000000000000LL) >> 40) |
+			((h64 & 0x000000000000ff00LL) << 40) |
+			((h64 & 0x0000ff0000000000LL) >> 24) |
+			((h64 & 0x0000000000ff0000LL) << 24) |
+			((h64 & 0x000000ff00000000LL) >>  8) |
+			((h64 & 0x00000000ff000000LL) <<  8));
+#endif
+}
+
+/* Convert 64-bit number from host byte order to network byte order */
+inline uint64_t __attribute__ ((__const__)) csp_ntoh64(uint64_t n64) {
+	return csp_hton64(n64);
+}
+
+/* Convert 16-bit number from host byte order to big endian byte order */
+inline uint16_t __attribute__ ((__const__)) csp_htobe16(uint16_t h16) {
+	return csp_hton16(h16);
+}
+
+/* Convert 16-bit number from host byte order to little endian byte order */
+inline uint16_t __attribute__ ((__const__)) csp_htole16(uint16_t h16) {
+#ifdef CSP_LITTLE_ENDIAN
+	return h16;
+#else
+	return (((h16 & 0xff00) >> 8) |
+			((h16 & 0x00ff) << 8));
+#endif
+}
+
+/* Convert 16-bit number from big endian byte order to little endian byte order */
+inline uint16_t __attribute__ ((__const__)) csp_betoh16(uint16_t be16) {
+	return csp_ntoh16(be16);
+}
+
+/* Convert 16-bit number from little endian byte order to host byte order */
+inline uint16_t __attribute__ ((__const__)) csp_letoh16(uint16_t le16) {
+	return csp_htole16(le16);
+}
+
+/* Convert 32-bit number from host byte order to big endian byte order */
+inline uint32_t __attribute__ ((__const__)) csp_htobe32(uint32_t h32) {
+	return csp_hton32(h32);
+}
+
+/* Convert 32-bit number from little endian byte order to host byte order */
+inline uint32_t __attribute__ ((__const__)) csp_htole32(uint32_t h32) {
+#ifdef CSP_LITTLE_ENDIAN
+	return h32;
+#else
+	return (((h32 & 0xff000000) >> 24) |
+			((h32 & 0x000000ff) << 24) |
+			((h32 & 0x0000ff00) <<  8) |
+			((h32 & 0x00ff0000) >>  8));
+#endif
+}
+
+/* Convert 32-bit number from big endian byte order to host byte order */
+inline uint32_t __attribute__ ((__const__)) csp_betoh32(uint32_t be32) {
+	return csp_ntoh32(be32);
+}
+
+/* Convert 32-bit number from little endian byte order to host byte order */
+inline uint32_t __attribute__ ((__const__)) csp_letoh32(uint32_t le32) {
+	return csp_htole32(le32);
+}
+
+/* Convert 64-bit number from host byte order to big endian byte order */
+inline uint64_t __attribute__ ((__const__)) csp_htobe64(uint64_t h64) {
+	return csp_hton64(h64);
+}
+
+/* Convert 64-bit number from host byte order to little endian byte order */
+inline uint64_t __attribute__ ((__const__)) csp_htole64(uint64_t h64) {
+#ifdef CSP_LITTLE_ENDIAN
+	return h64;
+#else
+	return (((h64 & 0xff00000000000000LL) >> 56) |
+			((h64 & 0x00000000000000ffLL) << 56) |
+			((h64 & 0x00ff000000000000LL) >> 40) |
+			((h64 & 0x000000000000ff00LL) << 40) |
+			((h64 & 0x0000ff0000000000LL) >> 24) |
+			((h64 & 0x0000000000ff0000LL) << 24) |
+			((h64 & 0x000000ff00000000LL) >>  8) |
+			((h64 & 0x00000000ff000000LL) <<  8));
+#endif
+}
+
+/* Convert 64-bit number from big endian byte order to host byte order */
+inline uint64_t __attribute__ ((__const__)) csp_betoh64(uint64_t be64) {
+	return csp_ntoh64(be64);
+}
+
+/* Convert 64-bit number from little endian byte order to host byte order */
+inline uint64_t __attribute__ ((__const__)) csp_letoh64(uint64_t le64) {
+	return csp_htole64(le64);
+}
+
+
+/* Convert float from host byte order to network byte order */
+inline float __attribute__ ((__const__)) csp_htonflt(float f) {
+#ifdef CSP_BIG_ENDIAN
+	return f;
+#else
+	union v {
+		float       f;
+		uint32_t	i;
+	};
+	union v val;
+	val.f = f;
+	val.i = csp_hton32(val.i);
+	return val.f;
+#endif
+}
+
+/* Convert float from host byte order to network byte order */
+inline float __attribute__ ((__const__)) csp_ntohflt(float f) {
+	return csp_htonflt(f);
+}
+
+/* Convert double from host byte order to network byte order */
+inline double __attribute__ ((__const__)) csp_htondbl(double d) {
+#ifdef CSP_BIG_ENDIAN
+	return d;
+#else
+	union v {
+		double       d;
+		uint64_t     i;
+	};
+	union v val;
+	val.d = d;
+	val.i = csp_hton64(val.i);
+	return val.d;
+#endif
+}
+
+/* Convert float from host byte order to network byte order */
+inline double __attribute__ ((__const__)) csp_ntohdbl(double d) {
+	return csp_htondbl(d);
+}

thirdparty/libcsp/src/csp_hex_dump.c

@@ -0,0 +1,55 @@
+#include <stdio.h>
+#include <csp/csp.h>
+
+void csp_hex_dump (const char *desc, void *addr, int len)
+{
+	int i;
+	unsigned char buff[17];
+	unsigned char *pc = (unsigned char*)addr;
+
+	// Output description if given.
+	if (desc != NULL)
+		printf ("%s:\r\n", desc);
+
+	if (len == 0) {
+		printf("  ZERO LENGTH\r\n");
+		return;
+	}
+	if (len < 0) {
+		printf("  NEGATIVE LENGTH: %i\r\n",len);
+		return;
+	}
+
+	// Process every byte in the data.
+	for (i = 0; i < len; i++) {
+		// Multiple of 16 means new line (with line offset).
+
+		if ((i % 16) == 0) {
+			// Just don't print ASCII for the zeroth line.
+			if (i != 0)
+				printf ("  %s\r\n", buff);
+
+			// Output the offset.
+			printf ("  %p ", addr + i);
+		}
+
+		// Now the hex code for the specific character.
+		printf (" %02x", pc[i]);
+
+		// And store a printable ASCII character for later.
+		if ((pc[i] < 0x20) || (pc[i] > 0x7e))
+			buff[i % 16] = '.';
+		else
+			buff[i % 16] = pc[i];
+		buff[(i % 16) + 1] = '\0';
+	}
+
+	// Pad out last line if not exactly 16 characters.
+	while ((i % 16) != 0) {
+		printf ("   ");
+		i++;
+	}
+
+	// And print the final ASCII bit.
+	printf ("  %s\r\n", buff);
+}

thirdparty/libcsp/src/csp_iflist.c

@@ -0,0 +1,100 @@
+/*
+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 <stdio.h>
+#include <csp/csp.h>
+
+/* Interfaces are stored in a linked list*/
+static csp_iface_t * interfaces = NULL;
+
+csp_iface_t * csp_iflist_get_by_name(const char *name) {
+	csp_iface_t *ifc = interfaces;
+	while(ifc) {
+		if (strncmp(ifc->name, name, 10) == 0)
+			break;
+		ifc = ifc->next;
+	}
+	return ifc;
+}
+
+void csp_iflist_add(csp_iface_t *ifc) {
+
+	/* Add interface to pool */
+	if (interfaces == NULL) {
+		/* This is the first interface to be added */
+		interfaces = ifc;
+		ifc->next = NULL;
+	} else {
+		/* One or more interfaces were already added */
+		csp_iface_t * i = interfaces;
+		while (i != ifc && i->next)
+			i = i->next;
+
+		/* Insert interface last if not already in pool */
+		if (i != ifc && i->next == NULL) {
+			i->next = ifc;
+			ifc->next = NULL;
+		}
+	}
+
+}
+
+csp_iface_t * csp_iflist_get(void)
+{
+    return interfaces;
+}
+
+#ifdef CSP_DEBUG
+static int csp_bytesize(char *buf, int len, unsigned long int n) {
+	char postfix;
+	double size;
+
+	if (n >= 1048576) {
+		size = n/1048576.0;
+		postfix = 'M';
+	} else if (n >= 1024) {
+		size = n/1024.;
+		postfix = 'K';
+	} else {
+		size = n;
+		postfix = 'B';
+ 	}
+ 
+	return snprintf(buf, len, "%.1f%c", size, postfix);
+}
+
+void csp_iflist_print(void) {
+	csp_iface_t * i = interfaces;
+	char txbuf[25], rxbuf[25];
+
+	while (i) {
+		csp_bytesize(txbuf, 25, i->txbytes);
+		csp_bytesize(rxbuf, 25, i->rxbytes);
+		printf("%-5s   tx: %05"PRIu32" rx: %05"PRIu32" txe: %05"PRIu32" rxe: %05"PRIu32"\r\n"
+		       "        drop: %05"PRIu32" autherr: %05"PRIu32 " frame: %05"PRIu32"\r\n"
+		       "        txb: %"PRIu32" (%s) rxb: %"PRIu32" (%s)\r\n\r\n",
+		       i->name, i->tx, i->rx, i->tx_error, i->rx_error, i->drop,
+		       i->autherr, i->frame, i->txbytes, txbuf, i->rxbytes, rxbuf);
+		i = i->next;
+	}
+
+}
+#endif
+

thirdparty/libcsp/src/csp_io.c

@@ -0,0 +1,502 @@
+/*
+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 <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+#include <csp/csp_error.h>
+#include <csp/csp_endian.h>
+#include <csp/csp_crc32.h>
+#include <csp/csp_rtable.h>
+#include <csp/interfaces/csp_if_lo.h>
+
+#include <csp/arch/csp_thread.h>
+#include <csp/arch/csp_queue.h>
+#include <csp/arch/csp_semaphore.h>
+#include <csp/arch/csp_time.h>
+#include <csp/arch/csp_malloc.h>
+
+#include <csp/crypto/csp_hmac.h>
+#include <csp/crypto/csp_xtea.h>
+
+#include "csp_io.h"
+#include "csp_port.h"
+#include "csp_conn.h"
+#include "csp_route.h"
+#include "csp_promisc.h"
+#include "csp_qfifo.h"
+#include "transport/csp_transport.h"
+
+/** CSP address of this node */
+static uint8_t csp_my_address;
+
+/* Hostname, model and build revision */
+static const char *csp_hostname = NULL;
+static const char *csp_model = NULL;
+static const char *csp_revision = GIT_REV;
+
+#ifdef CSP_USE_PROMISC
+extern csp_queue_handle_t csp_promisc_queue;
+#endif
+
+void csp_set_address(uint8_t addr)
+{
+	csp_my_address = addr;
+}
+
+uint8_t csp_get_address(void)
+{
+	return csp_my_address;
+}
+
+void csp_set_hostname(const char *hostname)
+{
+	csp_hostname = hostname;
+}
+
+const char *csp_get_hostname(void)
+{
+	return csp_hostname;
+}
+
+void csp_set_model(const char *model)
+{
+	csp_model = model;
+}
+
+const char *csp_get_model(void)
+{
+	return csp_model;
+}
+
+void csp_set_revision(const char *revision)
+{
+	csp_revision = revision;
+}
+
+const char *csp_get_revision(void)
+{
+	return csp_revision;
+}
+
+int csp_init(unsigned char address) {
+
+	int ret;
+
+	/* Initialize CSP */
+	csp_set_address(address);
+
+	ret = csp_conn_init();
+	if (ret != CSP_ERR_NONE)
+		return ret;
+
+	ret = csp_port_init();
+	if (ret != CSP_ERR_NONE)
+		return ret;
+
+	ret = csp_qfifo_init();
+	if (ret != CSP_ERR_NONE)
+		return ret;
+
+	/* Loopback */
+	csp_iflist_add(&csp_if_lo);
+
+	/* Register loopback route */
+	csp_route_set(csp_get_address(), &csp_if_lo, CSP_NODE_MAC);
+
+	/* Also register loopback as default, until user redefines default route */
+	csp_route_set(CSP_DEFAULT_ROUTE, &csp_if_lo, CSP_NODE_MAC);
+
+	return CSP_ERR_NONE;
+
+}
+
+csp_socket_t * csp_socket(uint32_t opts) {
+	
+	/* Validate socket options */
+#ifndef CSP_USE_RDP
+	if (opts & CSP_SO_RDPREQ) {
+		csp_log_error("Attempt to create socket that requires RDP, but CSP was compiled without RDP support");
+		return NULL;
+	}
+#endif
+
+#ifndef CSP_USE_XTEA
+	if (opts & CSP_SO_XTEAREQ) {
+		csp_log_error("Attempt to create socket that requires XTEA, but CSP was compiled without XTEA support");
+		return NULL;
+	}
+#endif
+
+#ifndef CSP_USE_HMAC
+	if (opts & CSP_SO_HMACREQ) {
+		csp_log_error("Attempt to create socket that requires HMAC, but CSP was compiled without HMAC support");
+		return NULL;
+	} 
+#endif
+
+#ifndef CSP_USE_CRC32
+	if (opts & CSP_SO_CRC32REQ) {
+		csp_log_error("Attempt to create socket that requires CRC32, but CSP was compiled without CRC32 support");
+		return NULL;
+	} 
+#endif
+	
+	/* Drop packet if reserved flags are set */
+	if (opts & ~(CSP_SO_RDPREQ | CSP_SO_XTEAREQ | CSP_SO_HMACREQ | CSP_SO_CRC32REQ | CSP_SO_CONN_LESS)) {
+		csp_log_error("Invalid socket option");
+		return NULL;
+	}
+
+	/* Use CSP buffers instead? */
+	csp_socket_t * sock = csp_conn_allocate(CONN_SERVER);
+	if (sock == NULL)
+		return NULL;
+
+	/* If connectionless, init the queue to a pre-defined size
+	 * if not, the user must init the queue using csp_listen */
+	if (opts & CSP_SO_CONN_LESS) {
+		sock->socket = csp_queue_create(CSP_CONN_QUEUE_LENGTH, sizeof(csp_packet_t *));
+		if (sock->socket == NULL) {
+			csp_close(sock);
+			return NULL;
+                }
+	} else {
+		sock->socket = NULL;
+	}
+	sock->opts = opts;
+
+	return sock;
+
+}
+
+csp_conn_t * csp_accept(csp_socket_t * sock, uint32_t timeout) {
+
+	if (sock == NULL)
+		return NULL;
+
+	if (sock->socket == NULL)
+		return NULL;
+
+	csp_conn_t * conn;
+	if (csp_queue_dequeue(sock->socket, &conn, timeout) == CSP_QUEUE_OK)
+		return conn;
+
+	return NULL;
+
+}
+
+csp_packet_t * csp_read(csp_conn_t * conn, uint32_t timeout) {
+
+	csp_packet_t * packet = NULL;
+
+	if (conn == NULL || conn->state != CONN_OPEN)
+		return NULL;
+
+#ifdef CSP_USE_QOS
+	int prio, event;
+	if (csp_queue_dequeue(conn->rx_event, &event, timeout) != CSP_QUEUE_OK)
+		return NULL;
+
+	for (prio = 0; prio < CSP_RX_QUEUES; prio++)
+		if (csp_queue_dequeue(conn->rx_queue[prio], &packet, 0) == CSP_QUEUE_OK)
+			break;
+#else
+	if (csp_queue_dequeue(conn->rx_queue[0], &packet, timeout) != CSP_QUEUE_OK)
+		return NULL;
+#endif
+
+#ifdef CSP_USE_RDP
+	/* Packet read could trigger ACK transmission */
+	if (conn->idin.flags & CSP_FRDP && conn->rdp.delayed_acks)
+	    csp_rdp_check_ack(conn);
+
+#endif
+
+	return packet;
+
+}
+
+int csp_send_direct(csp_id_t idout, csp_packet_t * packet, csp_iface_t * ifout, uint32_t timeout) {
+
+	if (packet == NULL) {
+		csp_log_error("csp_send_direct called with NULL packet");
+		goto err;
+	}
+
+	if ((ifout == NULL) || (ifout->nexthop == NULL)) {
+		csp_log_error("No route to host: %#08x", idout.ext);
+		goto err;
+	}
+
+	csp_log_packet("OUT: S %u, D %u, Dp %u, Sp %u, Pr %u, Fl 0x%02X, Sz %u VIA: %s",
+		idout.src, idout.dst, idout.dport, idout.sport, idout.pri, idout.flags, packet->length, ifout->name);
+
+	/* Copy identifier to packet (before crc, xtea and hmac) */
+	packet->id.ext = idout.ext;
+
+#ifdef CSP_USE_PROMISC
+	/* Loopback traffic is added to promisc queue by the router */
+	if (idout.dst != csp_get_address() && idout.src == csp_get_address()) {
+		packet->id.ext = idout.ext;
+		csp_promisc_add(packet);
+	}
+#endif
+
+	/* Only encrypt packets from the current node */
+	if (idout.src == csp_get_address()) {
+		/* Append HMAC */
+		if (idout.flags & CSP_FHMAC) {
+#ifdef CSP_USE_HMAC
+			/* Calculate and add HMAC (does not include header for backwards compatability with csp1.x) */
+			if (csp_hmac_append(packet, false) != 0) {
+				/* HMAC append failed */
+				csp_log_warn("HMAC append failed!");
+				goto tx_err;
+			}
+#else
+			csp_log_warn("Attempt to send packet with HMAC, but CSP was compiled without HMAC support. Discarding packet");
+			goto tx_err;
+#endif
+		}
+
+		/* Append CRC32 */
+		if (idout.flags & CSP_FCRC32) {
+#ifdef CSP_USE_CRC32
+			/* Calculate and add CRC32 (does not include header for backwards compatability with csp1.x) */
+			if (csp_crc32_append(packet, false) != 0) {
+				/* CRC32 append failed */
+				csp_log_warn("CRC32 append failed!");
+				goto tx_err;
+			}
+#else
+			csp_log_warn("Attempt to send packet with CRC32, but CSP was compiled without CRC32 support. Sending without CRC32r");
+			idout.flags &= ~(CSP_FCRC32);
+#endif
+		}
+
+		if (idout.flags & CSP_FXTEA) {
+#ifdef CSP_USE_XTEA
+			/* Create nonce */
+			uint32_t nonce, nonce_n;
+			nonce = (uint32_t)rand();
+			nonce_n = csp_hton32(nonce);
+			memcpy(&packet->data[packet->length], &nonce_n, sizeof(nonce_n));
+
+			/* Create initialization vector */
+			uint32_t iv[2] = {nonce, 1};
+
+			/* Encrypt data */
+			if (csp_xtea_encrypt(packet->data, packet->length, iv) != 0) {
+				/* Encryption failed */
+				csp_log_warn("Encryption failed! Discarding packet");
+				goto tx_err;
+			}
+
+			packet->length += sizeof(nonce_n);
+#else
+			csp_log_warn("Attempt to send XTEA encrypted packet, but CSP was compiled without XTEA support. Discarding packet");
+			goto tx_err;
+#endif
+		}
+	}
+
+	/* Store length before passing to interface */
+	uint16_t bytes = packet->length;
+	uint16_t mtu = ifout->mtu;
+
+	if (mtu > 0 && bytes > mtu)
+		goto tx_err;
+
+	if ((*ifout->nexthop)(ifout, packet, timeout) != CSP_ERR_NONE)
+		goto tx_err;
+
+	ifout->tx++;
+	ifout->txbytes += bytes;
+	return CSP_ERR_NONE;
+
+tx_err:
+	ifout->tx_error++;
+err:
+	return CSP_ERR_TX;
+
+}
+
+int csp_send(csp_conn_t * conn, csp_packet_t * packet, uint32_t timeout) {
+
+	int ret;
+
+	if ((conn == NULL) || (packet == NULL) || (conn->state != CONN_OPEN)) {
+		csp_log_error("Invalid call to csp_send");
+		return 0;
+	}
+
+#ifdef CSP_USE_RDP
+	if (conn->idout.flags & CSP_FRDP) {
+		if (csp_rdp_send(conn, packet, timeout) != CSP_ERR_NONE) {
+			csp_iface_t * ifout = csp_rtable_find_iface(conn->idout.dst);
+			if (ifout != NULL)
+				ifout->tx_error++;
+			csp_log_warn("RDP send failed!");
+			return 0;
+		}
+	}
+#endif
+
+	csp_iface_t * ifout = csp_rtable_find_iface(conn->idout.dst);
+	ret = csp_send_direct(conn->idout, packet, ifout, timeout);
+
+	return (ret == CSP_ERR_NONE) ? 1 : 0;
+
+}
+
+int csp_send_prio(uint8_t prio, csp_conn_t * conn, csp_packet_t * packet, uint32_t timeout) {
+	conn->idout.pri = prio;
+	return csp_send(conn, packet, timeout);
+}
+
+int csp_transaction_persistent(csp_conn_t * conn, uint32_t timeout, void * outbuf, int outlen, void * inbuf, int inlen) {
+
+	int size = (inlen > outlen) ? inlen : outlen;
+	csp_packet_t * packet = csp_buffer_get(size);
+	if (packet == NULL)
+		return 0;
+
+	/* Copy the request */
+	if (outlen > 0 && outbuf != NULL)
+		memcpy(packet->data, outbuf, outlen);
+	packet->length = outlen;
+
+	if (!csp_send(conn, packet, timeout)) {
+		csp_buffer_free(packet);
+		return 0;
+	}
+
+	/* If no reply is expected, return now */
+	if (inlen == 0)
+		return 1;
+
+	packet = csp_read(conn, timeout);
+	if (packet == NULL)
+		return 0;
+
+	if ((inlen != -1) && ((int)packet->length != inlen)) {
+		csp_log_error("Reply length %u expected %u", packet->length, inlen);
+		csp_buffer_free(packet);
+		return 0;
+	}
+
+	memcpy(inbuf, packet->data, packet->length);
+	int length = packet->length;
+	csp_buffer_free(packet);
+	return length;
+
+}
+
+int csp_transaction(uint8_t prio, uint8_t dest, uint8_t port, uint32_t timeout, void * outbuf, int outlen, void * inbuf, int inlen) {
+    return csp_transaction2(prio, dest, port, timeout, outbuf, outlen, inbuf, inlen, 0);
+}
+
+int csp_transaction2(uint8_t prio, uint8_t dest, uint8_t port, uint32_t timeout, void * outbuf, int outlen, void * inbuf, int inlen, uint32_t opts) {
+
+	csp_conn_t * conn = csp_connect(prio, dest, port, 0, opts);
+	if (conn == NULL)
+		return 0;
+
+	int status = csp_transaction_persistent(conn, timeout, outbuf, outlen, inbuf, inlen);
+
+	csp_close(conn);
+
+	return status;
+
+}
+
+csp_packet_t * csp_recvfrom(csp_socket_t * socket, uint32_t timeout) {
+
+	if ((socket == NULL) || (!(socket->opts & CSP_SO_CONN_LESS)))
+		return NULL;
+
+	csp_packet_t * packet = NULL;
+	csp_queue_dequeue(socket->socket, &packet, timeout);
+
+	return packet;
+
+}
+
+int csp_sendto(uint8_t prio, uint8_t dest, uint8_t dport, uint8_t src_port, uint32_t opts, csp_packet_t * packet, uint32_t timeout) {
+
+	packet->id.flags = 0;
+
+	if (opts & CSP_O_RDP) {
+		csp_log_error("Attempt to create RDP packet on connection-less socket");
+		return CSP_ERR_INVAL;
+	}
+
+	if (opts & CSP_O_HMAC) {
+#ifdef CSP_USE_HMAC
+		packet->id.flags |= CSP_FHMAC;
+#else
+		csp_log_error("Attempt to create HMAC authenticated packet, but CSP was compiled without HMAC support");
+		return CSP_ERR_NOTSUP;
+#endif
+	}
+
+	if (opts & CSP_O_XTEA) {
+#ifdef CSP_USE_XTEA
+		packet->id.flags |= CSP_FXTEA;
+#else
+		csp_log_error("Attempt to create XTEA encrypted packet, but CSP was compiled without XTEA support");
+		return CSP_ERR_NOTSUP;
+#endif
+	}
+
+	if (opts & CSP_O_CRC32) {
+#ifdef CSP_USE_CRC32
+		packet->id.flags |= CSP_FCRC32;
+#else
+		csp_log_error("Attempt to create CRC32 validated packet, but CSP was compiled without CRC32 support");
+		return CSP_ERR_NOTSUP;
+#endif
+	}
+
+	packet->id.dst = dest;
+	packet->id.dport = dport;
+	packet->id.src = csp_get_address();
+	packet->id.sport = src_port;
+	packet->id.pri = prio;
+
+	csp_iface_t * ifout = csp_rtable_find_iface(dest);
+	if (csp_send_direct(packet->id, packet, ifout, timeout) != CSP_ERR_NONE)
+		return CSP_ERR_NOTSUP;
+	
+	return CSP_ERR_NONE;
+
+}
+
+int csp_sendto_reply(csp_packet_t * request_packet, csp_packet_t * reply_packet, uint32_t opts, uint32_t timeout) {
+	if (request_packet == NULL)
+		return CSP_ERR_INVAL;
+
+	return csp_sendto(request_packet->id.pri, request_packet->id.src, request_packet->id.sport, request_packet->id.dport, opts, reply_packet, timeout);
+}

thirdparty/libcsp/src/csp_io.h

@@ -0,0 +1,47 @@
+/*
+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
+*/
+
+#ifndef _CSP_IO_H_
+#define _CSP_IO_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+#include <csp/csp.h>
+
+/**
+ * Function to transmit a frame without an existing connection structure.
+ * This function is used for stateless transmissions
+ * @param idout 32bit CSP identifier
+ * @param packet pointer to packet,
+ * @param ifout pointer to output interface
+ * @param timeout a timeout to wait for TX to complete. NOTE: not all underlying drivers supports flow-control.
+ * @return returns 1 if successful and 0 otherwise. you MUST free the frame yourself if the transmission was not successful.
+ */
+int csp_send_direct(csp_id_t idout, csp_packet_t * packet, csp_iface_t * ifout, uint32_t timeout);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // _CSP_IO_H_

thirdparty/libcsp/src/csp_port.c

@@ -0,0 +1,105 @@
+/*
+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 <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+#include <csp/csp_error.h>
+
+#include <csp/arch/csp_thread.h>
+#include <csp/arch/csp_queue.h>
+#include <csp/arch/csp_semaphore.h>
+
+#include "csp_port.h"
+#include "csp_conn.h"
+
+/* Allocation of ports */
+static csp_port_t ports[CSP_MAX_BIND_PORT + 2];
+
+csp_socket_t * csp_port_get_socket(unsigned int port) {
+
+	csp_socket_t * ret = NULL;
+
+	if (port >= CSP_ANY)
+		return NULL;
+
+	/* Match dport to socket or local "catch all" port number */
+	if (ports[port].state == PORT_OPEN)
+		ret = ports[port].socket;
+	else if (ports[CSP_ANY].state == PORT_OPEN)
+		ret = ports[CSP_ANY].socket;
+
+	return ret;
+
+}
+
+int csp_port_init(void) {
+
+	memset(ports, PORT_CLOSED, sizeof(csp_port_t) * (CSP_MAX_BIND_PORT + 2));
+
+	return CSP_ERR_NONE;
+
+}
+
+int csp_listen(csp_socket_t * socket, size_t conn_queue_length) {
+	
+	if (socket == NULL)
+		return CSP_ERR_INVAL;
+
+	socket->socket = csp_queue_create(conn_queue_length, sizeof(csp_conn_t *));
+	if (socket->socket == NULL)
+		return CSP_ERR_NOMEM;
+
+        socket->opts |= CSP_SO_INTERNAL_LISTEN;
+
+	return CSP_ERR_NONE;
+
+}
+
+int csp_bind(csp_socket_t * socket, uint8_t port) {
+	
+	if (socket == NULL)
+		return CSP_ERR_INVAL;
+
+	if (port > CSP_ANY) {
+		csp_log_error("Only ports from 0-%u (and CSP_ANY for default) are available for incoming ports", CSP_ANY);
+		return CSP_ERR_INVAL;
+	}
+
+	/* Check if port number is valid */
+	if (ports[port].state != PORT_CLOSED) {
+		csp_log_error("Port %d is already in use", port);
+		return CSP_ERR_USED;
+	}
+
+	csp_log_info("Binding socket %p to port %u", socket, port);
+
+	/* Save listener */
+	ports[port].socket = socket;
+	ports[port].state = PORT_OPEN;
+
+	return CSP_ERR_NONE;
+
+}
+
+

thirdparty/libcsp/src/csp_port.h

@@ -0,0 +1,55 @@
+/*
+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
+*/
+
+#ifndef _CSP_PORT_H_
+#define _CSP_PORT_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+#include <csp/csp.h>
+
+/** @brief Port states */
+typedef enum {
+	PORT_CLOSED = 0,
+	PORT_OPEN = 1,
+} csp_port_state_t;
+
+/** @brief Port struct */
+typedef struct {
+	csp_port_state_t state;		 // Port state
+	csp_socket_t * socket;		  // New connections are added to this socket's conn queue
+} csp_port_t;
+
+/**
+ * Init ports array
+ */
+int csp_port_init(void);
+
+csp_socket_t * csp_port_get_socket(unsigned int dport);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // _CSP_PORT_H_

thirdparty/libcsp/src/csp_promisc.c

@@ -0,0 +1,82 @@
+/*
+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 <csp/csp.h>
+#include <csp/arch/csp_queue.h>
+
+#ifdef CSP_USE_PROMISC
+
+static csp_queue_handle_t csp_promisc_queue = NULL;
+static int csp_promisc_enabled = 0;
+
+int csp_promisc_enable(unsigned int buf_size) {
+
+	/* If queue already initialised */
+	if (csp_promisc_queue != NULL) {
+		csp_promisc_enabled = 1;
+		return CSP_ERR_NONE;
+	}
+
+	/* Create packet queue */
+	csp_promisc_queue = csp_queue_create(buf_size, sizeof(csp_packet_t *));
+
+	if (csp_promisc_queue == NULL)
+		return CSP_ERR_INVAL;
+
+	csp_promisc_enabled = 1;
+	return CSP_ERR_NONE;
+
+}
+
+void csp_promisc_disable(void) {
+	csp_promisc_enabled = 0;
+}
+
+csp_packet_t * csp_promisc_read(uint32_t timeout) {
+
+	if (csp_promisc_queue == NULL)
+		return NULL;
+
+	csp_packet_t * packet = NULL;
+	csp_queue_dequeue(csp_promisc_queue, &packet, timeout);
+
+	return packet;
+
+}
+
+void csp_promisc_add(csp_packet_t * packet) {
+
+	if (csp_promisc_enabled == 0)
+		return;
+
+	if (csp_promisc_queue != NULL) {
+		/* Make a copy of the message and queue it to the promiscuous task */
+		csp_packet_t *packet_copy = csp_buffer_clone(packet);
+		if (packet_copy != NULL) {
+			if (csp_queue_enqueue(csp_promisc_queue, &packet_copy, 0) != CSP_QUEUE_OK) {
+				csp_log_error("Promiscuous mode input queue full");
+				csp_buffer_free(packet_copy);
+			}
+		}
+	}
+
+}
+
+#endif

thirdparty/libcsp/src/csp_promisc.h

@@ -0,0 +1,30 @@
+/*
+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
+*/
+
+#ifndef CSP_PROMISC_H_
+#define CSP_PROMISC_H_
+
+/**
+ * Add packet to promiscuous mode packet queue
+ * @param packet Packet to add to the queue
+ */
+void csp_promisc_add(csp_packet_t * packet);
+
+#endif /* CSP_PROMISC_H_ */

thirdparty/libcsp/src/csp_qfifo.c

@@ -0,0 +1,149 @@
+/*
+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 <csp/csp.h>
+#include <csp/csp_interface.h>
+#include <csp/arch/csp_queue.h>
+#include "csp_qfifo.h"
+
+static csp_queue_handle_t qfifo[CSP_ROUTE_FIFOS];
+#ifdef CSP_USE_QOS
+static csp_queue_handle_t qfifo_events;
+#endif
+
+int csp_qfifo_init(void) {
+	int prio;
+
+	/* Create router fifos for each priority */
+	for (prio = 0; prio < CSP_ROUTE_FIFOS; prio++) {
+		if (qfifo[prio] == NULL) {
+			qfifo[prio] = csp_queue_create(CSP_FIFO_INPUT, sizeof(csp_qfifo_t));
+			if (!qfifo[prio])
+				return CSP_ERR_NOMEM;
+		}
+	}
+
+#ifdef CSP_USE_QOS
+	/* Create QoS fifo notification queue */
+	qfifo_events = csp_queue_create(CSP_FIFO_INPUT, sizeof(int));
+	if (!qfifo_events)
+		return CSP_ERR_NOMEM;
+#endif
+
+	return CSP_ERR_NONE;
+
+}
+
+int csp_qfifo_read(csp_qfifo_t * input) {
+
+#ifdef CSP_USE_QOS
+	int prio, found, event;
+
+	/* Wait for packet in any queue */
+	if (csp_queue_dequeue(qfifo_events, &event, FIFO_TIMEOUT) != CSP_QUEUE_OK)
+		return CSP_ERR_TIMEDOUT;
+
+	/* Find packet with highest priority */
+	found = 0;
+	for (prio = 0; prio < CSP_ROUTE_FIFOS; prio++) {
+		if (csp_queue_dequeue(qfifo[prio], input, 0) == CSP_QUEUE_OK) {
+			found = 1;
+			break;
+		}
+	}
+
+	if (!found) {
+		csp_log_warn("Spurious wakeup: No packet found");
+		return CSP_ERR_TIMEDOUT;
+	}
+#else
+	if (csp_queue_dequeue(qfifo[0], input, FIFO_TIMEOUT) != CSP_QUEUE_OK)
+		return CSP_ERR_TIMEDOUT;
+#endif
+
+	return CSP_ERR_NONE;
+
+}
+
+void csp_qfifo_write(csp_packet_t * packet, csp_iface_t * interface, CSP_BASE_TYPE * pxTaskWoken) {
+
+	int result;
+
+	if (packet == NULL) {
+		if (pxTaskWoken == NULL) { // Only do logging in non-ISR context
+			csp_log_warn("csp_new packet called with NULL packet");
+		}
+		return;
+	} else if (interface == NULL) {
+		if (pxTaskWoken == NULL) { // Only do logging in non-ISR context
+			csp_log_warn("csp_new packet called with NULL interface");
+		}
+		if (pxTaskWoken == NULL)
+			csp_buffer_free(packet);
+		else
+			csp_buffer_free_isr(packet);
+		return;
+	}
+
+	csp_qfifo_t queue_element;
+	queue_element.interface = interface;
+	queue_element.packet = packet;
+
+#ifdef CSP_USE_QOS
+	int fifo = packet->id.pri;
+#else
+	int fifo = 0;
+#endif
+
+	if (pxTaskWoken == NULL)
+		result = csp_queue_enqueue(qfifo[fifo], &queue_element, 0);
+	else
+		result = csp_queue_enqueue_isr(qfifo[fifo], &queue_element, pxTaskWoken);
+
+#ifdef CSP_USE_QOS
+	static int event = 0;
+
+	if (result == CSP_QUEUE_OK) {
+		if (pxTaskWoken == NULL)
+			csp_queue_enqueue(qfifo_events, &event, 0);
+		else
+			csp_queue_enqueue_isr(qfifo_events, &event, pxTaskWoken);
+	}
+#endif
+
+	if (result != CSP_QUEUE_OK) {
+		if (pxTaskWoken == NULL) { // Only do logging in non-ISR context
+			csp_log_warn("ERROR: Routing input FIFO is FULL. Dropping packet.");
+		}
+		interface->drop++;
+		if (pxTaskWoken == NULL)
+			csp_buffer_free(packet);
+		else
+			csp_buffer_free_isr(packet);
+	}
+
+}
+
+void csp_qfifo_wake_up(void) {
+	csp_qfifo_t queue_element;
+	queue_element.interface = NULL;
+	queue_element.packet = NULL;
+	csp_queue_enqueue(qfifo[0], &queue_element, 0);
+}

thirdparty/libcsp/src/csp_qfifo.h

@@ -0,0 +1,54 @@
+/*
+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
+*/
+
+#ifndef CSP_QFIFO_H_
+#define CSP_QFIFO_H_
+
+#ifdef CSP_USE_RDP
+#define FIFO_TIMEOUT 100				//! If RDP is enabled, the router needs to awake some times to check timeouts
+#else
+#define FIFO_TIMEOUT CSP_MAX_DELAY		//! If no RDP, the router can sleep untill data arrives
+#endif
+
+/**
+ * Init FIFO/QOS queues
+ * @return CSP_ERR type
+ */
+int csp_qfifo_init(void);
+
+typedef struct {
+	csp_iface_t * interface;
+	csp_packet_t * packet;
+} csp_qfifo_t;
+
+/**
+ * Read next packet from router input queue
+ * @param input pointer to router queue item element
+ * @return CSP_ERR type
+ */
+int csp_qfifo_read(csp_qfifo_t * input);
+
+/**
+ * Wake up any task (e.g. router) waiting on messages.
+ * For testing.
+ */
+void csp_qfifo_wake_up(void);
+
+#endif /* CSP_QFIFO_H_ */

thirdparty/libcsp/src/csp_route.c

@@ -0,0 +1,346 @@
+/*
+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 <stdlib.h>
+#include <stdint.h>
+#include <inttypes.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+#include <csp/csp_crc32.h>
+#include <csp/csp_endian.h>
+
+#include <csp/arch/csp_thread.h>
+#include <csp/arch/csp_queue.h>
+
+#include <csp/crypto/csp_hmac.h>
+#include <csp/crypto/csp_xtea.h>
+
+#include "csp_port.h"
+#include "csp_conn.h"
+#include "csp_io.h"
+#include "csp_promisc.h"
+#include "csp_qfifo.h"
+#include "csp_dedup.h"
+#include "transport/csp_transport.h"
+
+/**
+ * Check supported packet options
+ * @param interface pointer to incoming interface
+ * @param packet pointer to packet
+ * @return CSP_ERR_NONE is all options are supported, CSP_ERR_NOTSUP if not
+ */
+static int csp_route_check_options(csp_iface_t *interface, csp_packet_t *packet)
+{
+#ifndef CSP_USE_XTEA
+	/* Drop XTEA packets */
+	if (packet->id.flags & CSP_FXTEA) {
+		csp_log_error("Received XTEA encrypted packet, but CSP was compiled without XTEA support. Discarding packet");
+		interface->autherr++;
+		return CSP_ERR_NOTSUP;
+	}
+#endif
+
+#ifndef CSP_USE_HMAC
+	/* Drop HMAC packets */
+	if (packet->id.flags & CSP_FHMAC) {
+		csp_log_error("Received packet with HMAC, but CSP was compiled without HMAC support. Discarding packet");
+		interface->autherr++;
+		return CSP_ERR_NOTSUP;
+	}
+#endif
+
+#ifndef CSP_USE_RDP
+	/* Drop RDP packets */
+	if (packet->id.flags & CSP_FRDP) {
+		csp_log_error("Received RDP packet, but CSP was compiled without RDP support. Discarding packet");
+		interface->rx_error++;
+		return CSP_ERR_NOTSUP;
+	}
+#endif
+	return CSP_ERR_NONE;
+}
+
+/**
+ * Helper function to decrypt, check auth and CRC32
+ * @param security_opts either socket_opts or conn_opts
+ * @param interface pointer to incoming interface
+ * @param packet pointer to packet
+ * @return -1 Missing feature, -2 XTEA error, -3 CRC error, -4 HMAC error, 0 = OK.
+ */
+static int csp_route_security_check(uint32_t security_opts, csp_iface_t * interface, csp_packet_t * packet) {
+
+#ifdef CSP_USE_XTEA
+	/* XTEA encrypted packet */
+	if (packet->id.flags & CSP_FXTEA) {
+		/* Read nonce */
+		uint32_t nonce;
+		memcpy(&nonce, &packet->data[packet->length - sizeof(nonce)], sizeof(nonce));
+		nonce = csp_ntoh32(nonce);
+		packet->length -= sizeof(nonce);
+
+		/* Create initialization vector */
+		uint32_t iv[2] = {nonce, 1};
+
+		/* Decrypt data */
+		if (csp_xtea_decrypt(packet->data, packet->length, iv) != 0) {
+			/* Decryption failed */
+			csp_log_error("Decryption failed! Discarding packet");
+			interface->autherr++;
+			return CSP_ERR_XTEA;
+		}
+	} else if (security_opts & CSP_SO_XTEAREQ) {
+		csp_log_warn("Received packet without XTEA encryption. Discarding packet");
+		interface->autherr++;
+		return CSP_ERR_XTEA;
+	}
+#endif
+
+	/* CRC32 verified packet */
+	if (packet->id.flags & CSP_FCRC32) {
+#ifdef CSP_USE_CRC32
+		if (packet->length < 4)
+			csp_log_error("Too short packet for CRC32, %u", packet->length);
+		/* Verify CRC32 (does not include header for backwards compatability with csp1.x) */
+		if (csp_crc32_verify(packet, false) != 0) {
+			/* Checksum failed */
+			csp_log_error("CRC32 verification error! Discarding packet");
+			interface->rx_error++;
+			return CSP_ERR_CRC32;
+		}
+	} else if (security_opts & CSP_SO_CRC32REQ) {
+		csp_log_warn("Received packet without CRC32. Accepting packet");
+#else
+		/* Strip CRC32 field and accept the packet */
+		csp_log_warn("Received packet with CRC32, but CSP was compiled without CRC32 support. Accepting packet");
+		packet->length -= sizeof(uint32_t);
+#endif
+	}
+
+#ifdef CSP_USE_HMAC
+	/* HMAC authenticated packet */
+	if (packet->id.flags & CSP_FHMAC) {
+		/* Verify HMAC (does not include header for backwards compatability with csp1.x) */
+		if (csp_hmac_verify(packet, false) != 0) {
+			/* HMAC failed */
+			csp_log_error("HMAC verification error! Discarding packet");
+			interface->autherr++;
+			return CSP_ERR_HMAC;
+		}
+	} else if (security_opts & CSP_SO_HMACREQ) {
+		csp_log_warn("Received packet without HMAC. Discarding packet");
+		interface->autherr++;
+		return CSP_ERR_HMAC;
+	}
+#endif
+
+#ifdef CSP_USE_RDP
+	/* RDP packet */
+	if (!(packet->id.flags & CSP_FRDP)) {
+		if (security_opts & CSP_SO_RDPREQ) {
+			csp_log_warn("Received packet without RDP header. Discarding packet");
+			interface->rx_error++;
+			return CSP_ERR_INVAL;
+		}
+	}
+#endif
+
+	return CSP_ERR_NONE;
+
+}
+
+int csp_route_work(uint32_t timeout) {
+
+	csp_qfifo_t input;
+	csp_packet_t * packet;
+	csp_conn_t * conn;
+	csp_socket_t * socket;
+
+#ifdef CSP_USE_RDP
+	/* Check connection timeouts (currently only for RDP) */
+	csp_conn_check_timeouts();
+#endif
+
+	/* Get next packet to route */
+	if (csp_qfifo_read(&input) != CSP_ERR_NONE)
+		return -1;
+
+	packet = input.packet;
+	if (!packet)
+		return -1;
+
+	csp_log_packet("INP: S %u, D %u, Dp %u, Sp %u, Pr %u, Fl 0x%02X, Sz %"PRIu16" VIA: %s",
+			packet->id.src, packet->id.dst, packet->id.dport,
+			packet->id.sport, packet->id.pri, packet->id.flags, packet->length, input.interface->name);
+
+	/* Here there be promiscuous mode */
+#ifdef CSP_USE_PROMISC
+	csp_promisc_add(packet);
+#endif
+
+#ifdef CSP_USE_DEDUP
+	/* Check for duplicates */
+	if (csp_dedup_is_duplicate(packet)) {
+		/* Discard packet */
+		csp_log_packet("Duplicate packet discarded");
+		input.interface->drop++;
+		csp_buffer_free(packet);
+		return 0;
+	}
+#endif
+
+	/* Now we count the message (since its deduplicated) */
+	input.interface->rx++;
+	input.interface->rxbytes += packet->length;
+
+	/* If the message is not to me, route the message to the correct interface */
+	if ((packet->id.dst != csp_get_address()) && (packet->id.dst != CSP_BROADCAST_ADDR)) {
+
+		/* Find the destination interface */
+		csp_iface_t * dstif = csp_rtable_find_iface(packet->id.dst);
+
+		/* If the message resolves to the input interface, don't loop it back out */
+		if ((dstif == NULL) || ((dstif == input.interface) && (input.interface->split_horizon_off == 0))) {
+			csp_buffer_free(packet);
+			return 0;
+		}
+
+		/* Otherwise, actually send the message */
+		if (csp_send_direct(packet->id, packet, dstif, 0) != CSP_ERR_NONE) {
+			csp_log_warn("Router failed to send");
+			csp_buffer_free(packet);
+		}
+
+		/* Next message, please */
+		return 0;
+	}
+
+	/* Discard packets with unsupported options */
+	if (csp_route_check_options(input.interface, packet) != CSP_ERR_NONE) {
+		csp_buffer_free(packet);
+		return 0;
+	}
+
+	/* The message is to me, search for incoming socket */
+	socket = csp_port_get_socket(packet->id.dport);
+
+	/* If the socket is connection-less, deliver now */
+	if (socket && (socket->opts & CSP_SO_CONN_LESS)) {
+		if (csp_route_security_check(socket->opts, input.interface, packet) < 0) {
+			csp_buffer_free(packet);
+			return 0;
+		}
+		if (csp_queue_enqueue(socket->socket, &packet, 0) != CSP_QUEUE_OK) {
+			csp_log_error("Conn-less socket queue full");
+			csp_buffer_free(packet);
+			return 0;
+		}
+		return 0;
+	}
+
+	/* Search for an existing connection */
+	conn = csp_conn_find(packet->id.ext, CSP_ID_CONN_MASK);
+
+	/* If this is an incoming packet on a new connection */
+	if (conn == NULL) {
+
+		/* Reject packet if no matching socket is found */
+		if (!socket) {
+			csp_buffer_free(packet);
+			return 0;
+		}
+
+		/* Run security check on incoming packet */
+		if (csp_route_security_check(socket->opts, input.interface, packet) < 0) {
+			csp_buffer_free(packet);
+			return 0;
+		}
+
+		/* New incoming connection accepted */
+		csp_id_t idout;
+		idout.pri   = packet->id.pri;
+		idout.src   = csp_get_address();
+
+		idout.dst   = packet->id.src;
+		idout.dport = packet->id.sport;
+		idout.sport = packet->id.dport;
+		idout.flags = packet->id.flags;
+
+		/* Create connection */
+		conn = csp_conn_new(packet->id, idout);
+
+		if (!conn) {
+			csp_log_error("No more connections available");
+			csp_buffer_free(packet);
+			return 0;
+		}
+
+		/* Store the socket queue and options */
+		conn->socket = socket->socket;
+		conn->opts = socket->opts;
+
+	/* Packet to existing connection */
+	} else {
+
+		/* Run security check on incoming packet */
+		if (csp_route_security_check(conn->opts, input.interface, packet) < 0) {
+			csp_buffer_free(packet);
+			return 0;
+		}
+
+	}
+
+#ifdef CSP_USE_RDP
+	/* Pass packet to RDP module */
+	if (packet->id.flags & CSP_FRDP) {
+		csp_rdp_new_packet(conn, packet);
+		return 0;
+	}
+#endif
+
+	/* Pass packet to UDP module */
+	csp_udp_new_packet(conn, packet);
+	return 0;
+}
+
+static CSP_DEFINE_TASK(csp_task_router) {
+
+	/* Here there be routing */
+	while (1) {
+		csp_route_work(FIFO_TIMEOUT);
+	}
+
+	return CSP_TASK_RETURN;
+
+}
+
+int csp_route_start_task(unsigned int task_stack_size, unsigned int priority) {
+
+	static csp_thread_handle_t handle_router;
+	int ret = csp_thread_create(csp_task_router, "RTE", task_stack_size, NULL, priority, &handle_router);
+
+	if (ret != 0) {
+		csp_log_error("Failed to start router task");
+		return CSP_ERR_NOMEM;
+	}
+
+	return CSP_ERR_NONE;
+
+}

thirdparty/libcsp/src/csp_route.h

@@ -0,0 +1,24 @@
+/*
+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
+*/
+
+#ifndef _CSP_ROUTE_H_
+#define _CSP_ROUTE_H_
+
+#endif // _CSP_ROUTE_H_

thirdparty/libcsp/src/csp_service_handler.c

@@ -0,0 +1,334 @@
+/*
+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 <stdio.h>
+#include <string.h>
+#include <stdint.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+#include <csp/csp_cmp.h>
+#include <csp/csp_endian.h>
+#include <csp/csp_platform.h>
+#include <csp/csp_rtable.h>
+
+#include <csp/arch/csp_time.h>
+#include <csp/arch/csp_clock.h>
+#include <csp/arch/csp_malloc.h>
+#include <csp/arch/csp_system.h>
+#include "csp_route.h"
+
+#define CSP_RPS_MTU	196
+
+/**
+ * The CSP CMP mempy function is used to, override the function used to
+ * read/write memory by peek and poke.
+ */
+#ifdef __AVR__
+static uint32_t wrap_32bit_memcpy(uint32_t to, const uint32_t from, size_t size) {
+	return (uint32_t) (uintptr_t) memcpy((void *) (uintptr_t) to, (const void *) (uintptr_t) from, size);
+}
+static csp_memcpy_fnc_t csp_cmp_memcpy_fnc = wrap_32bit_memcpy;
+#else
+static csp_memcpy_fnc_t csp_cmp_memcpy_fnc = (csp_memcpy_fnc_t) memcpy;
+#endif
+
+
+void csp_cmp_set_memcpy(csp_memcpy_fnc_t fnc) {
+	csp_cmp_memcpy_fnc = fnc;
+}
+
+static int do_cmp_ident(struct csp_cmp_message *cmp) {
+
+	/* Copy revision */
+	strncpy(cmp->ident.revision, csp_get_revision(), CSP_CMP_IDENT_REV_LEN);
+	cmp->ident.revision[CSP_CMP_IDENT_REV_LEN - 1] = '\0';
+
+	/* Copy compilation date */
+	strncpy(cmp->ident.date, __DATE__, CSP_CMP_IDENT_DATE_LEN);
+	cmp->ident.date[CSP_CMP_IDENT_DATE_LEN - 1] = '\0';
+
+	/* Copy compilation time */
+	strncpy(cmp->ident.time, __TIME__, CSP_CMP_IDENT_TIME_LEN);
+	cmp->ident.time[CSP_CMP_IDENT_TIME_LEN - 1] = '\0';
+
+	/* Copy hostname */
+	strncpy(cmp->ident.hostname, csp_get_hostname(), CSP_HOSTNAME_LEN);
+	cmp->ident.hostname[CSP_HOSTNAME_LEN - 1] = '\0';
+
+	/* Copy model name */
+	strncpy(cmp->ident.model, csp_get_model(), CSP_MODEL_LEN);
+	cmp->ident.model[CSP_MODEL_LEN - 1] = '\0';
+
+	return CSP_ERR_NONE;
+
+}
+
+static int do_cmp_route_set(struct csp_cmp_message *cmp) {
+
+	csp_iface_t *ifc = csp_iflist_get_by_name(cmp->route_set.interface);
+	if (ifc == NULL)
+		return CSP_ERR_INVAL;
+
+	if (csp_route_set(cmp->route_set.dest_node, ifc, cmp->route_set.next_hop_mac) != CSP_ERR_NONE)
+		return CSP_ERR_INVAL;
+
+	return CSP_ERR_NONE;
+
+}
+
+static int do_cmp_if_stats(struct csp_cmp_message *cmp) {
+
+	csp_iface_t *ifc = csp_iflist_get_by_name(cmp->if_stats.interface);
+	if (ifc == NULL)
+		return CSP_ERR_INVAL;
+
+	cmp->if_stats.tx =       csp_hton32(ifc->tx);
+	cmp->if_stats.rx =       csp_hton32(ifc->rx);
+	cmp->if_stats.tx_error = csp_hton32(ifc->tx_error);
+	cmp->if_stats.rx_error = csp_hton32(ifc->rx_error);
+	cmp->if_stats.drop =     csp_hton32(ifc->drop);
+	cmp->if_stats.autherr =  csp_hton32(ifc->autherr);
+	cmp->if_stats.frame =    csp_hton32(ifc->frame);
+	cmp->if_stats.txbytes =  csp_hton32(ifc->txbytes);
+	cmp->if_stats.rxbytes =  csp_hton32(ifc->rxbytes);
+	cmp->if_stats.irq = 	 csp_hton32(ifc->irq);
+
+	return CSP_ERR_NONE;
+}
+
+static int do_cmp_peek(struct csp_cmp_message *cmp) {
+
+	cmp->peek.addr = csp_hton32(cmp->peek.addr);
+	if (cmp->peek.len > CSP_CMP_PEEK_MAX_LEN)
+		return CSP_ERR_INVAL;
+
+	/* Dangerous, you better know what you are doing */
+	csp_cmp_memcpy_fnc((csp_memptr_t) (uintptr_t) cmp->peek.data, (csp_memptr_t) (unsigned long) cmp->peek.addr, cmp->peek.len);
+
+	return CSP_ERR_NONE;
+
+}
+
+static int do_cmp_poke(struct csp_cmp_message *cmp) {
+
+	cmp->poke.addr = csp_hton32(cmp->poke.addr);
+	if (cmp->poke.len > CSP_CMP_POKE_MAX_LEN)
+		return CSP_ERR_INVAL;
+
+	/* Extremely dangerous, you better know what you are doing */
+	csp_cmp_memcpy_fnc((csp_memptr_t) (unsigned long) cmp->poke.addr, (csp_memptr_t) (uintptr_t) cmp->poke.data, cmp->poke.len);
+
+	return CSP_ERR_NONE;
+
+}
+
+static int do_cmp_clock(struct csp_cmp_message *cmp) {
+
+	cmp->clock.tv_sec = csp_ntoh32(cmp->clock.tv_sec);
+	cmp->clock.tv_nsec = csp_ntoh32(cmp->clock.tv_nsec);
+
+	if ((cmp->clock.tv_sec != 0) && (clock_set_time != NULL)) {
+		clock_set_time(&cmp->clock);
+	}
+
+	if (clock_get_time != NULL) {
+		clock_get_time(&cmp->clock);
+	}
+
+	cmp->clock.tv_sec = csp_hton32(cmp->clock.tv_sec);
+	cmp->clock.tv_nsec = csp_hton32(cmp->clock.tv_nsec);
+	return CSP_ERR_NONE;
+
+}
+
+/* CSP Management Protocol handler */
+static int csp_cmp_handler(csp_conn_t * conn, csp_packet_t * packet) {
+
+	int ret = CSP_ERR_INVAL;
+	struct csp_cmp_message * cmp = (struct csp_cmp_message *) packet->data;
+
+	/* Ignore everything but requests */
+	if (cmp->type != CSP_CMP_REQUEST)
+		return ret;
+
+	switch (cmp->code) {
+		case CSP_CMP_IDENT:
+			ret = do_cmp_ident(cmp);
+			packet->length = CMP_SIZE(ident);
+			break;
+
+		case CSP_CMP_ROUTE_SET:
+			ret = do_cmp_route_set(cmp);
+			packet->length = CMP_SIZE(route_set);
+			break;
+
+		case CSP_CMP_IF_STATS:
+			ret = do_cmp_if_stats(cmp);
+			packet->length = CMP_SIZE(if_stats);
+			break;
+
+		case CSP_CMP_PEEK:
+			ret = do_cmp_peek(cmp);
+			break;
+
+		case CSP_CMP_POKE:
+			ret = do_cmp_poke(cmp);
+			break;
+
+		case CSP_CMP_CLOCK:
+			ret = do_cmp_clock(cmp);
+			break;
+
+		default:
+			ret = CSP_ERR_INVAL;
+			break;
+	}
+
+	cmp->type = CSP_CMP_REPLY;
+
+	return ret;
+}
+
+void csp_service_handler(csp_conn_t * conn, csp_packet_t * packet) {
+
+	switch (csp_conn_dport(conn)) {
+
+	case CSP_CMP:
+		/* Pass to CMP handler */
+		if (csp_cmp_handler(conn, packet) != CSP_ERR_NONE) {
+			csp_buffer_free(packet);
+			return;
+		}
+		break;
+
+	case CSP_PING:
+		/* A ping means, just echo the packet, so no changes */
+		csp_log_info("SERVICE: Ping received");
+		break;
+
+	case CSP_PS: {
+		/* Sanity check on request */
+		if ((packet->length != 1) || (packet->data[0] != 0x55)) {
+			/* Sanity check failed */
+			csp_buffer_free(packet);
+			/* Clear the packet, it has been freed */
+			packet = NULL;
+			break;
+		}
+		/* Start by allocating just the right amount of memory */
+		int task_list_size = csp_sys_tasklist_size();
+		char * pslist = csp_malloc(task_list_size);
+		/* Check for malloc fail */
+		if (pslist == NULL) {
+			/* Send out the data */
+			strcpy((char *)packet->data, "Not enough memory");
+			packet->length = strlen((char *)packet->data);
+			/* Break and let the default handling send packet */
+			break;
+		}
+
+		/* Retrieve the tasklist */
+		csp_sys_tasklist(pslist);
+		int pslen = strnlen(pslist, task_list_size);
+
+		/* Split the potentially very long string into packets */
+		int i = 0;
+		while(i < pslen) {
+
+			/* Allocate packet buffer, if need be */
+			if (packet == NULL)
+				packet = csp_buffer_get(CSP_RPS_MTU);
+			if (packet == NULL)
+				break;
+
+			/* Calculate length, either full MTU or the remainder */
+			packet->length = (pslen - i > CSP_RPS_MTU) ? CSP_RPS_MTU : (pslen - i);
+
+			/* Send out the data */
+			memcpy(packet->data, &pslist[i], packet->length);
+			i += packet->length;
+			if (!csp_send(conn, packet, 0))
+				csp_buffer_free(packet);
+
+			/* Clear the packet reference when sent */
+			packet = NULL;
+
+		}
+		csp_free(pslist);
+		break;
+	}
+
+	case CSP_MEMFREE: {
+		uint32_t total = csp_sys_memfree();
+
+		total = csp_hton32(total);
+		memcpy(packet->data, &total, sizeof(total));
+		packet->length = sizeof(total);
+
+		break;
+	}
+
+	case CSP_REBOOT: {
+		uint32_t magic_word;
+		memcpy(&magic_word, packet->data, sizeof(magic_word));
+
+		magic_word = csp_ntoh32(magic_word);
+
+		/* If the magic word is valid, reboot */
+		if (magic_word == CSP_REBOOT_MAGIC) {
+			csp_sys_reboot();
+		} else if (magic_word == CSP_REBOOT_SHUTDOWN_MAGIC) {
+			csp_sys_shutdown();
+		}
+
+
+		
+		csp_buffer_free(packet);
+		return;
+	}
+
+	case CSP_BUF_FREE: {
+		uint32_t size = csp_buffer_remaining();
+		size = csp_hton32(size);
+		memcpy(packet->data, &size, sizeof(size));
+		packet->length = sizeof(size);
+		break;
+	}
+
+	case CSP_UPTIME: {
+		uint32_t time = csp_get_s();
+		time = csp_hton32(time);
+		memcpy(packet->data, &time, sizeof(time));
+		packet->length = sizeof(time);
+		break;
+	}
+
+	default:
+		csp_buffer_free(packet);
+		return;
+	}
+
+	if (packet != NULL) {
+		if (!csp_send(conn, packet, 0))
+			csp_buffer_free(packet);
+	}
+
+}

thirdparty/libcsp/src/csp_services.c

@@ -0,0 +1,233 @@
+/*
+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 <stdio.h>
+#include <string.h>
+#include <stdint.h>
+#include <inttypes.h>
+
+/* CSP includes */
+#include <csp/csp.h>
+#include <csp/csp_cmp.h>
+#include <csp/csp_endian.h>
+
+#include <csp/arch/csp_time.h>
+
+int csp_ping(uint8_t node, uint32_t timeout, unsigned int size, uint8_t conn_options) {
+
+	unsigned int i;
+	uint32_t start, time, status = 0;
+
+	/* Counter */
+	start = csp_get_ms();
+
+	/* Open connection */
+	csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, node, CSP_PING, timeout, conn_options);
+	if (conn == NULL)
+		return -1;
+
+	/* Prepare data */
+	csp_packet_t * packet;
+	packet = csp_buffer_get(size);
+	if (packet == NULL)
+		goto out;
+
+	/* Set data to increasing numbers */
+	packet->length = size;
+	for (i = 0; i < size; i++)
+		packet->data[i] = i;
+
+	/* Try to send frame */
+	if (!csp_send(conn, packet, 0))
+		goto out;
+
+	/* Read incoming frame */
+	packet = csp_read(conn, timeout);
+	if (packet == NULL)
+		goto out;
+
+	/* Ensure that the data was actually echoed */
+	for (i = 0; i < size; i++)
+		if (packet->data[i] != i % (0xff + 1))
+			goto out;
+
+	status = 1;
+
+out:
+	/* Clean up */
+	if (packet != NULL)
+		csp_buffer_free(packet);
+	csp_close(conn);
+
+	/* We have a reply */
+	time = (csp_get_ms() - start);
+
+	if (status) {
+		return time;
+	} else {
+		return -1;
+	}
+
+}
+
+void csp_ping_noreply(uint8_t node) {
+
+	/* Prepare data */
+	csp_packet_t * packet;
+	packet = csp_buffer_get(1);
+
+	/* Check malloc */
+	if (packet == NULL)
+		return;
+
+	/* Open connection */
+	csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, node, CSP_PING, 0, 0);
+	if (conn == NULL) {
+		csp_buffer_free(packet);
+		return;
+	}
+
+	packet->data[0] = 0x55;
+	packet->length = 1;
+
+	printf("Ping ignore reply node %u.\r\n", (unsigned int) node);
+
+	/* Try to send frame */
+	if (!csp_send(conn, packet, 0))
+		csp_buffer_free(packet);
+
+	csp_close(conn);
+
+}
+
+void csp_reboot(uint8_t node) {
+	uint32_t magic_word = csp_hton32(CSP_REBOOT_MAGIC);
+	csp_transaction(CSP_PRIO_NORM, node, CSP_REBOOT, 0, &magic_word, sizeof(magic_word), NULL, 0);
+}
+
+void csp_shutdown(uint8_t node) {
+	uint32_t magic_word = csp_hton32(CSP_REBOOT_SHUTDOWN_MAGIC);
+	csp_transaction(CSP_PRIO_NORM, node, CSP_REBOOT, 0, &magic_word, sizeof(magic_word), NULL, 0);
+}
+
+void csp_ps(uint8_t node, uint32_t timeout) {
+
+	/* Open connection */
+	csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, node, CSP_PS, 0, 0);
+	if (conn == NULL)
+		return;
+
+	/* Prepare data */
+	csp_packet_t * packet;
+	packet = csp_buffer_get(95);
+
+	/* Check malloc */
+	if (packet == NULL)
+		goto out;
+
+	packet->data[0] = 0x55;
+	packet->length = 1;
+
+	printf("PS node %u: \r\n", (unsigned int) node);
+
+	/* Try to send frame */
+	if (!csp_send(conn, packet, 0))
+		goto out;
+
+	while(1) {
+
+		/* Read incoming frame */
+		packet = csp_read(conn, timeout);
+		if (packet == NULL)
+			break;
+
+		/* We have a reply, add our own NULL char */
+		packet->data[packet->length] = 0;
+		printf("%s", packet->data);
+
+		/* Each packet from csp_read must to be freed by user */
+		csp_buffer_free(packet);
+	}
+
+	printf("\r\n");
+
+	/* Clean up */
+out:
+	if (packet != NULL)
+		csp_buffer_free(packet);
+	csp_close(conn);
+
+}
+
+void csp_memfree(uint8_t node, uint32_t timeout) {
+
+	uint32_t memfree;
+
+	int status = csp_transaction(CSP_PRIO_NORM, node, CSP_MEMFREE, timeout, NULL, 0, &memfree, sizeof(memfree));
+	if (status == 0) {
+		printf("Network error\r\n");
+		return;
+	}
+
+	/* Convert from network to host order */
+	memfree = csp_ntoh32(memfree);
+
+	printf("Free Memory at node %u is %"PRIu32" bytes\r\n", (unsigned int) node, memfree);
+
+}
+
+void csp_buf_free(uint8_t node, uint32_t timeout) {
+
+	uint32_t size = 0;
+
+	int status = csp_transaction(CSP_PRIO_NORM, node, CSP_BUF_FREE, timeout, NULL, 0, &size, sizeof(size));
+	if (status == 0) {
+		printf("Network error\r\n");
+		return;
+	}
+	size = csp_ntoh32(size);
+	printf("Free buffers at node %u is %"PRIu32"\r\n", (unsigned int) node, size);
+
+}
+
+void csp_uptime(uint8_t node, uint32_t timeout) {
+
+	uint32_t uptime = 0;
+
+	int status = csp_transaction(CSP_PRIO_NORM, node, CSP_UPTIME, timeout, NULL, 0, &uptime, sizeof(uptime));
+	if (status == 0) {
+		printf("Network error\r\n");
+		return;
+	}
+	uptime = csp_ntoh32(uptime);
+	printf("Uptime of node %u is %"PRIu32" s\r\n", (unsigned int) node, uptime);
+
+}
+
+int csp_cmp(uint8_t node, uint32_t timeout, uint8_t code, int membsize, struct csp_cmp_message * msg) {
+	msg->type = CSP_CMP_REQUEST;
+	msg->code = code;
+	int status = csp_transaction(CSP_PRIO_NORM, node, CSP_CMP, timeout, msg, membsize, msg, membsize);
+	if (status == 0)
+		return CSP_ERR_TIMEDOUT;
+
+	return CSP_ERR_NONE;
+}
+

thirdparty/libcsp/src/csp_sfp.c

@@ -0,0 +1,170 @@
+/*
+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 <stdint.h>
+#include <stdlib.h>
+#include <csp/csp.h>
+#include <csp/csp_endian.h>
+#include <csp/arch/csp_malloc.h>
+#include "csp_conn.h"
+
+typedef struct __attribute__((__packed__)) {
+	uint32_t offset;
+	uint32_t totalsize;
+} sfp_header_t;
+
+/**
+ * SFP Headers:
+ * The following functions are helper functions that handles the extra SFP
+ * information that needs to be appended to all data packets.
+ */
+static sfp_header_t * csp_sfp_header_add(csp_packet_t * packet) {
+	sfp_header_t * header = (sfp_header_t *) &packet->data[packet->length];
+	packet->length += sizeof(sfp_header_t);
+	memset(header, 0, sizeof(sfp_header_t));
+	return header;
+}
+
+static sfp_header_t * csp_sfp_header_remove(csp_packet_t * packet) {
+	sfp_header_t * header = (sfp_header_t *) &packet->data[packet->length-sizeof(sfp_header_t)];
+	packet->length -= sizeof(sfp_header_t);
+	return header;
+}
+
+int csp_sfp_send_own_memcpy(csp_conn_t * conn, const void * data, int totalsize, int mtu, uint32_t timeout, void * (*memcpyfcn)(void *, const void *, size_t)) {
+
+	int count = 0;
+	while(count < totalsize) {
+
+		/* Allocate packet */
+		csp_packet_t * packet = csp_buffer_get(mtu);
+		if (packet == NULL)
+			return -1;
+
+		/* Calculate sending size */
+		int size = totalsize - count;
+		if (size > mtu)
+			size = mtu;
+
+		/* Print debug */
+		csp_debug(CSP_PROTOCOL, "Sending SFP at %x size %u", data + count, size);
+
+		/* Copy data */
+		(*memcpyfcn)(packet->data, data + count, size);
+		packet->length = size;
+
+		/* Set fragment flag */
+		conn->idout.flags |= CSP_FFRAG;
+
+		/* Add SFP header */
+		sfp_header_t * sfp_header = csp_sfp_header_add(packet);
+		sfp_header->totalsize = csp_hton32(totalsize);
+		sfp_header->offset = csp_hton32(count);
+
+		/* Send data */
+		if (!csp_send(conn, packet, timeout)) {
+			csp_buffer_free(packet);
+			return -1;
+		}
+
+		/* Increment count */
+		count += size;
+
+	}
+
+	return 0;
+
+}
+
+int csp_sfp_send(csp_conn_t * conn, const void * data, int totalsize, int mtu, uint32_t timeout) {
+	return csp_sfp_send_own_memcpy(conn, data, totalsize, mtu, timeout, &memcpy);
+}
+
+int csp_sfp_recv_fp(csp_conn_t * conn, void ** dataout, int * datasize, uint32_t timeout, csp_packet_t * first_packet) {
+
+	unsigned int last_byte = 0;
+
+	*dataout = NULL; /* Allow caller to assume csp_free() can always be called when dataout is non-NULL */
+
+	/* Get first packet from user, or from connection */
+	csp_packet_t * packet = NULL;
+	if (first_packet == NULL) {
+		packet = csp_read(conn, timeout);
+		if (packet == NULL)
+			return -1;
+	} else {
+		packet = first_packet;
+	}
+
+	do {
+
+		/* Check that SFP header is present */
+		if ((packet->id.flags & CSP_FFRAG) == 0) {
+			csp_debug(CSP_ERROR, "Missing SFP header");
+			csp_buffer_free(packet);
+			return -1;
+		}
+
+		/* Read SFP header */
+		sfp_header_t * sfp_header = csp_sfp_header_remove(packet);
+		sfp_header->offset = csp_ntoh32(sfp_header->offset);
+		sfp_header->totalsize = csp_ntoh32(sfp_header->totalsize);
+
+		csp_debug(CSP_PROTOCOL, "SFP fragment %u/%u", sfp_header->offset + packet->length, sfp_header->totalsize);
+
+		if (sfp_header->offset > last_byte + 1) {
+			csp_debug(CSP_ERROR, "SFP missing %u bytes", sfp_header->offset - last_byte);
+			csp_buffer_free(packet);
+			return -1;
+		} else {
+			last_byte = sfp_header->offset + packet->length;
+		}
+
+		/* Allocate memory */
+		if (*dataout == NULL)
+			*dataout = csp_malloc(sfp_header->totalsize);
+		if (*dataout == NULL) {
+			csp_debug(CSP_ERROR, "No dyn-memory for SFP fragment");
+			csp_buffer_free(packet);
+			return -1;
+		}
+
+		/* Copy data to output */
+		*datasize = sfp_header->totalsize;
+		memcpy(*dataout + sfp_header->offset, packet->data, packet->length);
+
+		if (sfp_header->offset + packet->length >= sfp_header->totalsize) {
+			csp_debug(CSP_PROTOCOL, "SFP complete");
+			csp_buffer_free(packet);
+			return 0;
+		} else {
+			csp_buffer_free(packet);
+		}
+
+	} while((packet = csp_read(conn, timeout)) != NULL);
+
+	return -1;
+
+}
+
+int csp_sfp_recv(csp_conn_t * conn, void ** dataout, int * datasize, uint32_t timeout) {
+	return csp_sfp_recv_fp(conn, dataout, datasize, timeout, NULL);
+}
+

thirdparty/libcsp/src/drivers/CMakeLists.txt

@@ -0,0 +1 @@
+add_subdirectory(can)

thirdparty/libcsp/src/drivers/can/CMakeLists.txt

@@ -0,0 +1,3 @@
+target_sources(${LIB_CSP_NAME} PRIVATE
+	can_socketcan.c
+)

thirdparty/libcsp/src/drivers/can/can_socketcan.c

@@ -0,0 +1,201 @@
+/*
+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
+*/
+
+/* SocketCAN driver */
+#include <csp/drivers/can_socketcan.h>
+
+#include <stdint.h>
+#include <stdio.h>
+
+#include <stdlib.h>
+#include <unistd.h>
+#include <time.h>
+#include <string.h>
+#include <errno.h>
+
+#include <pthread.h>
+#include <semaphore.h>
+
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <net/if.h>
+#include <sys/queue.h>
+#include <sys/uio.h>
+#include <sys/time.h>
+#include <net/if.h>
+
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <linux/socket.h>
+
+#include <csp/csp.h>
+#include <csp/interfaces/csp_if_can.h>
+
+#ifdef CSP_HAVE_LIBSOCKETCAN
+#include <libsocketcan.h>
+#endif
+
+static struct can_socketcan_s {
+	int socket;
+	csp_iface_t interface;
+} socketcan[1] = {
+	{
+		.interface = {
+			.name = "CAN",
+			.nexthop = csp_can_tx,
+			.mtu = CSP_CAN_MTU,
+			.driver = &socketcan[0],
+		},
+	},
+};
+
+static void * socketcan_rx_thread(void * parameters)
+{
+	struct can_frame frame;
+	int nbytes;
+
+	while (1) {
+		/* Read CAN frame */
+		nbytes = read(socketcan[0].socket, &frame, sizeof(frame));
+		if (nbytes < 0) {
+			csp_log_error("read: %s", strerror(errno));
+			continue;
+		}
+
+		if (nbytes != sizeof(frame)) {
+			csp_log_warn("Read incomplete CAN frame");
+			continue;
+		}
+
+		/* Frame type */
+		if (frame.can_id & (CAN_ERR_FLAG | CAN_RTR_FLAG) || !(frame.can_id & CAN_EFF_FLAG)) {
+			/* Drop error and remote frames */
+			csp_log_warn("Discarding ERR/RTR/SFF frame");
+			continue;
+		}
+
+		/* Strip flags */
+		frame.can_id &= CAN_EFF_MASK;
+
+		/* Call RX callbacsp_can_rx_frameck */
+		csp_can_rx(&socketcan[0].interface, frame.can_id, frame.data, frame.can_dlc, NULL);
+	}
+
+	/* We should never reach this point */
+	pthread_exit(NULL);
+}
+
+
+int csp_can_tx_frame(csp_iface_t *interface, uint32_t id, const uint8_t * data, uint8_t dlc)
+{
+	struct can_frame frame;
+	int i, tries = 0;
+	memset(&frame, 0, sizeof(frame));
+	if (dlc > 8)
+		return -1;
+
+	/* Copy identifier */
+	frame.can_id = id | CAN_EFF_FLAG;
+
+	/* Copy data to frame */
+	for (i = 0; i < dlc; i++)
+		frame.data[i] = data[i];
+
+	/* Set DLC */
+	frame.can_dlc = dlc;
+
+	/* Send frame */
+	while (write(socketcan[0].socket, &frame, sizeof(frame)) != sizeof(frame)) {
+		if (++tries < 1000 && errno == ENOBUFS) {
+			/* Wait 10 ms and try again */
+			usleep(10000);
+		} else {
+			csp_log_error("write: %s", strerror(errno));
+			break;
+		}
+	}
+
+	return 0;
+}
+
+csp_iface_t * csp_can_socketcan_init(const char * ifc, int bitrate, int promisc)
+{
+	struct ifreq ifr;
+	struct sockaddr_can addr;
+	pthread_t rx_thread;
+
+	printf("-I-: Initiating CAN interface %s\n", ifc);
+
+#ifdef CSP_HAVE_LIBSOCKETCAN
+	/* Set interface up */
+	if (bitrate > 0) {
+		can_do_stop(ifc);
+		can_set_bitrate(ifc, bitrate);
+		can_set_restart_ms(ifc, 100);
+		can_do_start(ifc);
+	}
+#endif
+
+	/* Create socket */
+	if ((socketcan[0].socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
+		csp_log_error("socket: %s", strerror(errno));
+		return NULL;
+	}
+
+	/* Locate interface */
+	strncpy(ifr.ifr_name, ifc, IFNAMSIZ - 1);
+	if (ioctl(socketcan[0].socket, SIOCGIFINDEX, &ifr) < 0) {
+		csp_log_error("ioctl: %s", strerror(errno));
+		return NULL;
+	}
+	memset(&addr, 0, sizeof(addr));
+	/* Bind the socket to CAN interface */
+	addr.can_family = AF_CAN;
+	addr.can_ifindex = ifr.ifr_ifindex;
+	if (bind(socketcan[0].socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+		csp_log_error("bind: %s", strerror(errno));
+		return NULL;
+	}
+
+	/* Set filter mode */
+	if (promisc == 0) {
+
+		struct can_filter filter;
+		filter.can_id = CFP_MAKE_DST(csp_get_address());
+		filter.can_mask = CFP_MAKE_DST((1 << CFP_HOST_SIZE) - 1);
+
+		if (setsockopt(socketcan[0].socket, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) {
+			csp_log_error("setsockopt: %s", strerror(errno));
+			return NULL;
+		}
+
+	}
+
+	/* Create receive thread */
+	if (pthread_create(&rx_thread, NULL, socketcan_rx_thread, NULL) != 0) {
+		csp_log_error("pthread_create: %s", strerror(errno));
+		return NULL;
+	}
+
+	csp_iflist_add(&socketcan[0].interface);
+
+	return &socketcan[0].interface;
+}

thirdparty/libcsp/src/drivers/usart/usart_linux.c

@@ -0,0 +1,254 @@
+/*
+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 <csp/drivers/usart.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+#include <pthread.h>
+#include <unistd.h>
+#include <errno.h>
+#include <termios.h>
+#include <fcntl.h>
+
+#include <csp/csp.h>
+#include <sys/time.h>
+
+int fd;
+usart_callback_t usart_callback = NULL;
+
+static void *serial_rx_thread(void *vptr_args);
+
+int getbaud(int ifd) {
+	struct termios termAttr;
+	int inputSpeed = -1;
+	speed_t baudRate;
+	tcgetattr(ifd, &termAttr);
+	/* Get the input speed. */
+	baudRate = cfgetispeed(&termAttr);
+	switch (baudRate) {
+	case B0:
+		inputSpeed = 0;
+		break;
+	case B50:
+		inputSpeed = 50;
+		break;
+	case B110:
+		inputSpeed = 110;
+		break;
+	case B134:
+		inputSpeed = 134;
+		break;
+	case B150:
+		inputSpeed = 150;
+		break;
+	case B200:
+		inputSpeed = 200;
+		break;
+	case B300:
+		inputSpeed = 300;
+		break;
+	case B600:
+		inputSpeed = 600;
+		break;
+	case B1200:
+		inputSpeed = 1200;
+		break;
+	case B1800:
+		inputSpeed = 1800;
+		break;
+	case B2400:
+		inputSpeed = 2400;
+		break;
+	case B4800:
+		inputSpeed = 4800;
+		break;
+	case B9600:
+		inputSpeed = 9600;
+		break;
+	case B19200:
+		inputSpeed = 19200;
+		break;
+	case B38400:
+		inputSpeed = 38400;
+		break;
+	case B57600:
+		inputSpeed = 57600;
+		break;
+	case B115200:
+		inputSpeed = 115200;
+		break;
+	case B230400:
+		inputSpeed = 230400;
+		break;
+#ifndef CSP_MACOSX
+	case B460800:
+		inputSpeed = 460800;
+		break;
+	case B500000:
+		inputSpeed = 500000;
+		break;
+	case B576000:
+		inputSpeed = 576000;
+		break;
+	case B921600:
+		inputSpeed = 921600;
+		break;
+	case B1000000:
+		inputSpeed = 1000000;
+		break;
+	case B1152000:
+		inputSpeed = 1152000;
+		break;
+	case B1500000:
+		inputSpeed = 1500000;
+		break;
+	case B2000000:
+		inputSpeed = 2000000;
+		break;
+	case B2500000:
+		inputSpeed = 2500000;
+		break;
+	case B3000000:
+		inputSpeed = 3000000;
+		break;
+	case B3500000:
+		inputSpeed = 3500000;
+		break;
+	case B4000000:
+		inputSpeed = 4000000;
+		break;
+#endif
+	}
+
+	return inputSpeed;
+
+}
+
+void usart_init(struct usart_conf * conf) {
+
+	struct termios options;
+	pthread_t rx_thread;
+
+	fd = open(conf->device, O_RDWR | O_NOCTTY | O_NONBLOCK);
+
+	if (fd < 0) {
+		printf("Failed to open %s: %s\r\n", conf->device, strerror(errno));
+		return;
+	}
+
+	int brate = 0;
+    switch(conf->baudrate) {
+    case 4800:    brate=B4800;    break;
+    case 9600:    brate=B9600;    break;
+    case 19200:   brate=B19200;   break;
+    case 38400:   brate=B38400;   break;
+    case 57600:   brate=B57600;   break;
+    case 115200:  brate=B115200;  break;
+    case 230400:  brate=B230400;  break;
+#ifndef CSP_MACOSX
+    case 460800:  brate=B460800;  break;
+    case 500000:  brate=B500000;  break;
+    case 576000:  brate=B576000;  break;
+    case 921600:  brate=B921600;  break;
+    case 1000000: brate=B1000000; break;
+    case 1152000: brate=B1152000; break;
+    case 1500000: brate=B1500000; break;
+    case 2000000: brate=B2000000; break;
+    case 2500000: brate=B2500000; break;
+    case 3000000: brate=B3000000; break;
+    case 3500000: brate=B3500000; break;
+    case 4000000: brate=B4000000; break;
+#endif
+    default:
+      printf("Unsupported baudrate requested, defaulting to 500000, requested baudrate=%u\n", conf->baudrate);
+      brate=B500000;
+      break;
+    }
+
+	tcgetattr(fd, &options);
+	cfsetispeed(&options, brate);
+	cfsetospeed(&options, brate);
+	options.c_cflag |= (CLOCAL | CREAD);
+	options.c_cflag &= ~PARENB;
+	options.c_cflag &= ~CSTOPB;
+	options.c_cflag &= ~CSIZE;
+	options.c_cflag |= CS8;
+	options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+	options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
+	options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST);
+	options.c_cc[VTIME] = 0;
+	options.c_cc[VMIN] = 1;
+	tcsetattr(fd, TCSANOW, &options);
+	if (tcgetattr(fd, &options) == -1)
+		perror("error setting options");
+	fcntl(fd, F_SETFL, 0);
+
+	/* Flush old transmissions */
+	if (tcflush(fd, TCIOFLUSH) == -1)
+		printf("Error flushing serial port - %s(%d).\n", strerror(errno), errno);
+
+	if (pthread_create(&rx_thread, NULL, serial_rx_thread, NULL) != 0)
+		return;
+
+}
+
+void usart_set_callback(usart_callback_t callback) {
+	usart_callback = callback;
+}
+
+void usart_insert(char c, void * pxTaskWoken) {
+	printf("%c", c);
+}
+
+void usart_putstr(char * buf, int len) {
+	if (write(fd, buf, len) != len)
+		return;
+}
+
+void usart_putc(char c) {
+	if (write(fd, &c, 1) != 1)
+		return;
+}
+
+char usart_getc(void) {
+	char c;
+	if (read(fd, &c, 1) != 1) return 0;
+	return c;
+}
+
+static void *serial_rx_thread(void *vptr_args) {
+	unsigned int length;
+	uint8_t * cbuf = malloc(100000);
+
+	// Receive loop
+	while (1) {
+		length = read(fd, cbuf, 300);
+		if (length <= 0) {
+			perror("Error: ");
+			exit(1);
+		}
+		if (usart_callback)
+			usart_callback(cbuf, length, NULL);
+	}
+	return NULL;
+}

thirdparty/libcsp/src/drivers/usart/usart_windows.c

@@ -0,0 +1,230 @@
+#include <stdio.h>
+#include <Windows.h>
+#include <process.h>
+
+#include <csp/csp.h>
+#include <csp/drivers/usart.h>
+
+static HANDLE portHandle = INVALID_HANDLE_VALUE;
+static HANDLE rxThread = INVALID_HANDLE_VALUE;
+static CRITICAL_SECTION txSection;
+static LONG isListening = 0;
+static usart_callback_t usart_callback = NULL;
+
+static void prvSendData(char *buf, int bufsz);
+static int prvTryOpenPort(const char* intf);
+static int prvTryConfigurePort(const struct usart_conf*);
+static int prvTrySetPortTimeouts(void);
+static const char* prvParityToStr(BYTE paritySetting);
+
+#ifdef CSP_DEBUG
+static void prvPrintError(void) {
+    char *messageBuffer = NULL;
+    DWORD errorCode = GetLastError();
+    DWORD formatMessageRet;
+    formatMessageRet = FormatMessageA(
+        FORMAT_MESSAGE_ALLOCATE_BUFFER |
+        FORMAT_MESSAGE_FROM_SYSTEM,
+        NULL,
+        errorCode,
+        MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
+        (char*)&messageBuffer,
+        0,
+        NULL);
+
+    if( !formatMessageRet ) {
+        csp_log_error("FormatMessage error, code: %lu", GetLastError());
+        return;
+    }
+    csp_log_error("%s", messageBuffer);
+    LocalFree(messageBuffer);
+}
+#endif
+
+#ifdef CSP_DEBUG
+#define printError() prvPrintError()
+#else
+#define printError() do {} while(0)
+#endif
+
+static int prvTryOpenPort(const char *intf) {
+    portHandle = CreateFileA(
+        intf, 
+        GENERIC_READ|GENERIC_WRITE,
+        0,
+        NULL,
+        OPEN_EXISTING,
+        0,
+        NULL);
+
+    if( portHandle == INVALID_HANDLE_VALUE ) {
+        DWORD errorCode = GetLastError();
+        if( errorCode == ERROR_FILE_NOT_FOUND ) {
+            csp_log_error("Could not open serial port, because it didn't exist!");
+        }
+        else
+            csp_log_error("Failure opening serial port! Code: %lu", errorCode);
+        return 1;
+    }
+    return 0;
+}
+
+static int prvTryConfigurePort(const struct usart_conf * conf) {
+    DCB portSettings = {0};
+    portSettings.DCBlength = sizeof(DCB);
+    if(!GetCommState(portHandle, &portSettings) ) {
+        csp_log_error("Could not get default settings for open COM port! Code: %lu", GetLastError());
+        return -1;
+    }
+    portSettings.BaudRate = conf->baudrate;
+    portSettings.Parity = conf->paritysetting;
+    portSettings.StopBits = conf->stopbits;
+    portSettings.fParity = conf->checkparity;
+    portSettings.fBinary = TRUE;
+    portSettings.ByteSize = conf->databits;
+    if( !SetCommState(portHandle, &portSettings) ) {
+        csp_log_error("Error when setting COM port settings! Code:%lu", GetLastError());
+        return 1;
+    }
+
+    GetCommState(portHandle, &portSettings);
+
+    csp_log_info("Port: %s, Baudrate: %lu, Data bits: %d, Stop bits: %d, Parity: %s",
+            conf->device, conf->baudrate, conf->databits, conf->stopbits, prvParityToStr(conf->paritysetting));
+    return 0;
+}
+
+static const char* prvParityToStr(BYTE paritySetting) {
+    static const char *parityStr[] = {
+        "None",
+        "Odd",
+        "Even",
+        "N/A"
+    };
+    char const *resultStr = NULL;
+
+    switch(paritySetting) {
+        case NOPARITY:
+            resultStr = parityStr[0];
+            break;
+        case ODDPARITY:
+            resultStr = parityStr[1];
+            break;
+        case EVENPARITY:
+            resultStr = parityStr[2];
+            break;
+        default:
+            resultStr = parityStr[3];
+    };
+    return resultStr;
+}
+
+static int prvTrySetPortTimeouts(void) {
+    COMMTIMEOUTS timeouts = {0};
+
+    if( !GetCommTimeouts(portHandle, &timeouts) ) {
+        csp_log_error("Error gettings current timeout settings");
+        return 1;
+    }
+
+    timeouts.ReadIntervalTimeout = 5;
+    timeouts.ReadTotalTimeoutMultiplier = 1;
+    timeouts.ReadTotalTimeoutConstant = 5;
+    timeouts.WriteTotalTimeoutMultiplier = 1;
+    timeouts.WriteTotalTimeoutConstant = 5;
+
+    if(!SetCommTimeouts(portHandle, &timeouts)) {
+        csp_log_error("Error setting timeouts!");
+        return 1;
+    }
+
+    return 0;
+}
+
+unsigned WINAPI prvRxTask(void* params) {
+    DWORD bytesRead;
+    DWORD eventStatus;
+    uint8_t recvBuffer[24]; 
+    SetCommMask(portHandle, EV_RXCHAR);
+    
+    while(isListening) {
+        WaitCommEvent(portHandle, &eventStatus, NULL);
+        if( !(eventStatus & EV_RXCHAR) ) {
+            continue;
+        }
+        if( !ReadFile(portHandle, recvBuffer, 24, &bytesRead, NULL)) {
+            csp_log_warn("Error receiving data! Code: %lu", GetLastError());
+            continue;
+        }
+        if( usart_callback != NULL )
+            usart_callback(recvBuffer, (size_t)bytesRead, NULL);
+    }
+    return 0;
+}
+
+static void prvSendData(char *buf, int bufsz) {
+    DWORD bytesTotal = 0;
+    DWORD bytesActual;
+    if( !WriteFile(portHandle, buf, bufsz-bytesTotal, &bytesActual, NULL) ) {
+        csp_log_error("Could not write data. Code: %lu", GetLastError());
+        return;
+    }
+    if( !FlushFileBuffers(portHandle) ) {
+        csp_log_warn("Could not flush write buffer. Code: %lu", GetLastError());
+    }
+}
+
+void usart_shutdown(void) {
+    InterlockedExchange(&isListening, 0);
+    CloseHandle(portHandle);
+    portHandle = INVALID_HANDLE_VALUE;
+    if( rxThread != INVALID_HANDLE_VALUE ) {
+        WaitForSingleObject(rxThread, INFINITE);
+        rxThread = INVALID_HANDLE_VALUE;
+    }
+    DeleteCriticalSection(&txSection);  
+}
+
+void usart_listen(void) {
+    InterlockedExchange(&isListening, 1);
+    rxThread = (HANDLE)_beginthreadex(NULL, 0, &prvRxTask, NULL, 0, NULL);
+}
+
+void usart_putstr(char* buf, int bufsz) {
+    EnterCriticalSection(&txSection);
+    prvSendData(buf, bufsz);
+    LeaveCriticalSection(&txSection);
+}
+
+void usart_insert(char c, void *pxTaskWoken) {
+    /* redirect debug output to stdout */
+    printf("%c", c);
+}
+
+void usart_set_callback(usart_callback_t callback) {
+    usart_callback = callback;
+}
+
+void usart_init(struct usart_conf * conf) {
+    if( prvTryOpenPort(conf->device) ) {
+        printError();
+        return;
+    }
+
+    if( prvTryConfigurePort(conf) ) {
+        printError();
+        return;
+    }
+
+    if( prvTrySetPortTimeouts() ) {
+        printError();
+        return;
+    }
+
+    InitializeCriticalSection(&txSection);
+
+    /* Start receiver thread */
+    usart_listen();
+}
+
+

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,
+};

thirdparty/libcsp/src/rtable/CMakeLists.txt

@@ -0,0 +1,3 @@
+target_sources(${LIB_CSP_NAME} PRIVATE
+	csp_rtable_cidr.c
+)

thirdparty/libcsp/src/rtable/csp_rtable_cidr.c

@@ -0,0 +1,233 @@
+/*
+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 <stdio.h>
+#include <string.h>
+#include <csp/csp.h>
+#include <alloca.h>
+#include <csp/arch/csp_malloc.h>
+#include <csp/interfaces/csp_if_lo.h>
+
+/* Local typedef for routing table */
+typedef struct __attribute__((__packed__)) csp_rtable_s {
+	uint8_t address;
+	uint8_t netmask;
+	uint8_t mac;
+	csp_iface_t * interface;
+	struct csp_rtable_s * next;
+} csp_rtable_t;
+
+/* Routing entries are stored in a linked list*/
+static csp_rtable_t * rtable = NULL;
+
+static csp_rtable_t * csp_rtable_find(uint8_t addr, uint8_t netmask, uint8_t exact) {
+
+	/* Remember best result */
+	csp_rtable_t * best_result = NULL;
+	uint8_t best_result_mask = 0;
+
+	/* Start search */
+	csp_rtable_t * i = rtable;
+	while(i) {
+
+		/* Look for exact match */
+		if (i->address == addr && i->netmask == netmask) {
+			best_result = i;
+			break;
+		}
+
+		/* Try a CIDR netmask match */
+		if (!exact) {
+			uint8_t hostbits = (1 << (CSP_ID_HOST_SIZE - i->netmask)) - 1;
+			uint8_t netbits = ~hostbits;
+			//printf("Netbits %x Hostbits %x\r\n", netbits, hostbits);
+
+			/* Match network addresses */
+			uint8_t net_a = i->address & netbits;
+			uint8_t net_b = addr & netbits;
+			//printf("A: %hhx, B: %hhx\r\n", net_a, net_b);
+
+			/* We have a match */
+			if (net_a == net_b) {
+				if (i->netmask >= best_result_mask) {
+					//printf("Match best result %u %u\r\n", best_result_mask, i->netmask);
+					best_result = i;
+					best_result_mask = i->netmask;
+				}
+			}
+
+		}
+
+		i = i->next;
+
+	}
+
+#if 0
+	if (best_result)
+		csp_debug(CSP_PACKET, "Using routing entry: %u/%u dev %s m:%u\r\n", best_result->address, best_result->netmask, best_result->interface->name, best_result->mac);
+#endif
+
+	return best_result;
+
+}
+
+void csp_rtable_clear(void) {
+	for (csp_rtable_t * i = rtable; (i);) {
+		void * freeme = i;
+		i = i->next;
+		csp_free(freeme);
+	}
+	rtable = NULL;
+
+	/* Set loopback up again */
+	csp_rtable_set(csp_get_address(), CSP_ID_HOST_SIZE, &csp_if_lo, CSP_NODE_MAC);
+
+}
+
+static int csp_rtable_parse(const char * buffer, int dry_run) {
+
+	int valid_entries = 0;
+
+	/* Copy string before running strtok */
+	char * str = alloca(strlen(buffer) + 1);
+	memcpy(str, buffer, strlen(buffer) + 1);
+
+	/* Get first token */
+	str = strtok(str, ",");
+
+	while ((str) && (strlen(str) > 1)) {
+		int address = 0, netmask = 0, mac = 255;
+		char name[10] = {};
+		if (sscanf(str, "%u/%u %s %u", &address, &netmask, name, &mac) != 4) {
+			if (sscanf(str, "%u/%u %s", &address, &netmask, name) != 3) {
+				csp_log_error("Parse error %s", str);
+				return -1;
+			}
+		}
+		//printf("Parsed %u/%u %u %s\r\n", address, netmask, mac, name);
+		csp_iface_t * ifc = csp_iflist_get_by_name(name);
+		if (ifc) {
+			if (dry_run == 0)
+				csp_rtable_set(address, netmask, ifc, mac);
+		} else {
+			csp_log_error("Unknown interface %s", name);
+			return -1;
+		}
+		valid_entries++;
+		str = strtok(NULL, ",");
+	}
+
+	return valid_entries;
+}
+
+void csp_rtable_load(const char * buffer) {
+	csp_rtable_parse(buffer, 0);
+}
+
+int csp_rtable_check(const char * buffer) {
+	return csp_rtable_parse(buffer, 1);
+}
+
+int csp_rtable_save(char * buffer, int maxlen) {
+	int len = 0;
+	for (csp_rtable_t * i = rtable; (i); i = i->next) {
+		if (i->mac != CSP_NODE_MAC) {
+			len += snprintf(buffer + len, maxlen - len, "%u/%u %s %u, ", i->address, i->netmask, i->interface->name, i->mac);
+		} else {
+			len += snprintf(buffer + len, maxlen - len, "%u/%u %s, ", i->address, i->netmask, i->interface->name);
+		}
+	}
+	return len;
+}
+
+csp_iface_t * csp_rtable_find_iface(uint8_t id) {
+	csp_rtable_t * entry = csp_rtable_find(id, CSP_ID_HOST_SIZE, 0);
+	if (entry == NULL)
+		return NULL;
+	return entry->interface;
+}
+
+uint8_t csp_rtable_find_mac(uint8_t id) {
+	csp_rtable_t * entry = csp_rtable_find(id, CSP_ID_HOST_SIZE, 0);
+	if (entry == NULL)
+		return 255;
+	return entry->mac;
+}
+
+int csp_rtable_set(uint8_t _address, uint8_t _netmask, csp_iface_t *ifc, uint8_t mac) {
+
+	if (ifc == NULL)
+		return CSP_ERR_INVAL;
+
+	/* Set default route in the old way */
+	int address, netmask;
+	if (_address == CSP_DEFAULT_ROUTE) {
+		netmask = 0;
+		address = 0;
+	} else {
+		netmask = _netmask;
+		address = _address;
+	}
+
+	/* Fist see if the entry exists */
+	csp_rtable_t * entry = csp_rtable_find(address, netmask, 1);
+
+	/* If not, create a new one */
+	if (!entry) {
+		entry = csp_malloc(sizeof(csp_rtable_t));
+		if (entry == NULL)
+			return CSP_ERR_NOMEM;
+
+		entry->next = NULL;
+		/* Add entry to linked-list */
+		if (rtable == NULL) {
+			/* This is the first interface to be added */
+			rtable = entry;
+		} else {
+			/* One or more interfaces were already added */
+			csp_rtable_t * i = rtable;
+			while (i->next)
+				i = i->next;
+			i->next = entry;
+		}
+	}
+
+	/* Fill in the data */
+	entry->address = address;
+	entry->netmask = netmask;
+	entry->interface = ifc;
+	entry->mac = mac;
+
+	return CSP_ERR_NONE;
+}
+
+#ifdef CSP_DEBUG
+void csp_rtable_print(void) {
+
+	for (csp_rtable_t * i = rtable; (i); i = i->next) {
+		if (i->mac == 255) {
+			printf("%u/%u %s\r\n", i->address, i->netmask, i->interface->name);
+		} else {
+			printf("%u/%u %s %u\r\n", i->address, i->netmask, i->interface->name, i->mac);
+		}
+	}
+
+}
+#endif

thirdparty/libcsp/src/rtable/csp_rtable_static.c

@@ -0,0 +1,128 @@
+/*
+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 <stdio.h>
+#include <csp/csp.h>
+#include <stdio.h>
+
+/* Local typedef for routing table */
+typedef struct __attribute__((__packed__)) csp_rtable_s {
+	csp_iface_t * interface;
+	uint8_t mac;
+} csp_rtable_t;
+
+/* Static storage context for routing table */
+static csp_rtable_t routes[CSP_ROUTE_COUNT] = {};
+
+/**
+ * Find entry in static routing table
+ * This is done by table lookup with fallback to the default route
+ * The reason why the csp_rtable_t struct is not returned directly
+ * is that we wish to hide the storage format, mainly because of
+ * the alternative routing table storage (cidr).
+ * @param id Node
+ * @return pointer to found routing entry
+ */
+static csp_rtable_t * csp_rtable_find(uint8_t id) {
+
+	if (routes[id].interface != NULL) {
+		return &routes[id];
+	} else if (routes[CSP_DEFAULT_ROUTE].interface != NULL) {
+		return &routes[CSP_DEFAULT_ROUTE];
+	}
+	return NULL;
+
+}
+
+csp_iface_t * csp_rtable_find_iface(uint8_t id) {
+	csp_rtable_t * route = csp_rtable_find(id);
+	if (route == NULL)
+		return NULL;
+	return route->interface;
+}
+
+uint8_t csp_rtable_find_mac(uint8_t id) {
+	csp_rtable_t * route = csp_rtable_find(id);
+	if (route == NULL)
+		return 255;
+	return route->mac;
+}
+
+void csp_rtable_clear(void) {
+	memset(routes, 0, sizeof(routes[0]) * CSP_ROUTE_COUNT);
+}
+
+void csp_route_table_load(uint8_t route_table_in[CSP_ROUTE_TABLE_SIZE]) {
+	memcpy(routes, route_table_in, sizeof(routes[0]) * CSP_ROUTE_COUNT);
+}
+
+void csp_route_table_save(uint8_t route_table_out[CSP_ROUTE_TABLE_SIZE]) {
+	memcpy(route_table_out, routes, sizeof(routes[0]) * CSP_ROUTE_COUNT);
+}
+
+int csp_rtable_set(uint8_t node, uint8_t mask, csp_iface_t *ifc, uint8_t mac) {
+
+	/* Don't add nothing */
+	if (ifc == NULL)
+		return CSP_ERR_INVAL;
+
+	/**
+	 * Check if the interface has been added.
+	 *
+	 * NOTE: For future implementations, interfaces should call
+	 * csp_route_add_if in its csp_if_<name>_init function, instead
+	 * of registering at first route_set, in order to make the interface
+	 * available to network based (CMP) route configuration.
+	 */
+	csp_iflist_add(ifc);
+
+	/* Set route */
+	if (node <= CSP_DEFAULT_ROUTE) {
+		routes[node].interface = ifc;
+		routes[node].mac = mac;
+	} else {
+		csp_log_error("Failed to set route: invalid node id %u", node);
+		return CSP_ERR_INVAL;
+	}
+
+	return CSP_ERR_NONE;
+
+}
+
+void csp_rtable_load(const char * buffer) {
+}
+
+int csp_rtable_check(const char * buffer) {
+	return -1;
+}
+
+#ifdef CSP_DEBUG
+void csp_rtable_print(void) {
+	int i;
+	printf("Node  Interface  Address\r\n");
+	for (i = 0; i < CSP_DEFAULT_ROUTE; i++)
+		if (routes[i].interface != NULL)
+			printf("%4u  %-9s  %u\r\n", i,
+				routes[i].interface->name,
+				routes[i].mac == CSP_NODE_MAC ? i : routes[i].mac);
+	printf("   *  %-9s  %u\r\n", routes[CSP_DEFAULT_ROUTE].interface->name, routes[CSP_DEFAULT_ROUTE].mac);
+
+}
+#endif

thirdparty/libcsp/src/transport/CMakeLists.txt

@@ -0,0 +1,4 @@
+target_sources(${LIB_CSP_NAME} PRIVATE
+	csp_rdp.c
+	csp_udp.c
+)

thirdparty/libcsp/src/transport/csp_transport.h

@@ -0,0 +1,46 @@
+/*
+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
+*/
+
+#ifndef _CSP_TRANSPORT_H_
+#define _CSP_TRANSPORT_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** ARRIVING SEGMENT */
+void csp_udp_new_packet(csp_conn_t * conn, csp_packet_t * packet);
+void csp_rdp_new_packet(csp_conn_t * conn, csp_packet_t * packet);
+
+/** RDP: USER REQUESTS */
+int csp_rdp_connect(csp_conn_t * conn, uint32_t timeout);
+int csp_rdp_allocate(csp_conn_t * conn);
+int csp_rdp_close(csp_conn_t * conn);
+void csp_rdp_conn_print(csp_conn_t * conn);
+int csp_rdp_send(csp_conn_t * conn, csp_packet_t * packet, uint32_t timeout);
+int csp_rdp_check_ack(csp_conn_t * conn);
+void csp_rdp_check_timeouts(csp_conn_t * conn);
+void csp_rdp_flush_all(csp_conn_t * conn);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif /* _CSP_TRANSPORT_H_ */

thirdparty/libcsp/src/transport/csp_udp.c

@@ -0,0 +1,49 @@
+/*
+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 <csp/csp.h>
+#include <csp/arch/csp_queue.h>
+#include "../csp_port.h"
+#include "../csp_conn.h"
+#include "csp_transport.h"
+
+void csp_udp_new_packet(csp_conn_t * conn, csp_packet_t * packet) {
+
+	/* Enqueue */
+	if (csp_conn_enqueue_packet(conn, packet) < 0) {
+		csp_log_error("Connection buffer queue full!");
+		csp_buffer_free(packet);
+		return;
+	}
+
+	/* Try to queue up the new connection pointer */
+	if (conn->socket != NULL) {
+		if (csp_queue_enqueue(conn->socket, &conn, 0) != CSP_QUEUE_OK) {
+			csp_log_warn("Warning socket connection queue full");
+			csp_close(conn);
+			return;
+		}
+
+		/* Ensure that this connection will not be posted to this socket again */
+		conn->socket = NULL;
+	}
+
+}
+