180 lines
4.4 KiB
C
180 lines
4.4 KiB
C
|
/*
|
||
|
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;
|
||
|
|
||
|
}
|