moved some files

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