ASR_BASE
Change-Id: Icf3719cc0afe3eeb3edc7fa80a2eb5199ca9dda1
diff --git a/marvell/services/gps/host/sys/src/threadqueue.c b/marvell/services/gps/host/sys/src/threadqueue.c
new file mode 100644
index 0000000..11166fa
--- /dev/null
+++ b/marvell/services/gps/host/sys/src/threadqueue.c
@@ -0,0 +1,196 @@
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <pthread.h>
+#include <sys/time.h>
+#include "threadqueue.h"
+
+
+#define MSGPOOL_SIZE 256
+
+struct msglist {
+ struct threadmsg msg;
+ struct msglist *next;
+};
+
+static inline struct msglist *get_msglist(struct threadqueue *queue)
+{
+struct msglist *tmp;
+
+ if(queue->msgpool != NULL) {
+ tmp = queue->msgpool;
+ queue->msgpool = tmp->next;
+ queue->msgpool_length--;
+ } else {
+ tmp = malloc(sizeof *tmp);
+ }
+
+ return tmp;
+}
+
+static inline void release_msglist(struct threadqueue *queue,struct msglist *node)
+{
+
+ if(queue->msgpool_length > ( queue->length/8 + MSGPOOL_SIZE)) {
+ free(node);
+ } else {
+ node->msg.data = NULL;
+ node->msg.msgtype = 0;
+ node->next = queue->msgpool;
+ queue->msgpool = node;
+ queue->msgpool_length++;
+ }
+ if(queue->msgpool_length > (queue->length/4 + MSGPOOL_SIZE*10)) {
+ struct msglist *tmp = queue->msgpool;
+ queue->msgpool = tmp->next;
+ free(tmp);
+ queue->msgpool_length--;
+ }
+}
+
+int thread_queue_init(struct threadqueue *queue)
+{
+ int ret = 0;
+ if (queue == NULL)
+ return EINVAL;
+
+ memset(queue, 0, sizeof(struct threadqueue));
+ ret = pthread_cond_init(&queue->cond, NULL);
+ if (ret != 0)
+ return ret;
+
+ ret = pthread_mutex_init(&queue->mutex, NULL);
+ if (ret != 0) {
+ pthread_cond_destroy(&queue->cond);
+ return ret;
+ }
+
+ return 0;
+}
+
+int thread_queue_add(struct threadqueue *queue, void *data, long msgtype)
+{
+ struct msglist *newmsg;
+
+ pthread_mutex_lock(&queue->mutex);
+ newmsg = get_msglist(queue);
+ if (newmsg == NULL) {
+ pthread_mutex_unlock(&queue->mutex);
+ return ENOMEM;
+ }
+ newmsg->msg.data = data;
+ newmsg->msg.msgtype = msgtype;
+
+ newmsg->next = NULL;
+ if (queue->last == NULL) {
+ queue->last = newmsg;
+ queue->first = newmsg;
+ } else {
+ queue->last->next = newmsg;
+ queue->last = newmsg;
+ }
+
+ if(queue->length == 0)
+ pthread_cond_broadcast(&queue->cond);
+ queue->length++;
+ pthread_mutex_unlock(&queue->mutex);
+
+ return 0;
+}
+
+int thread_queue_get(struct threadqueue *queue, const struct timespec *timeout, struct threadmsg *msg)
+{
+ struct msglist *firstrec;
+ int ret = 0;
+ struct timespec abstimeout;
+
+ if (queue == NULL || msg == NULL) {
+ return EINVAL;
+ }
+ if (timeout) {
+ struct timeval now;
+
+ gettimeofday(&now, NULL);
+ abstimeout.tv_sec = now.tv_sec + timeout->tv_sec;
+ abstimeout.tv_nsec = (now.tv_usec * 1000) + timeout->tv_nsec;
+ if (abstimeout.tv_nsec >= 1000000000) {
+ abstimeout.tv_sec++;
+ abstimeout.tv_nsec -= 1000000000;
+ }
+ }
+
+ pthread_mutex_lock(&queue->mutex);
+
+ /* Will wait until awakened by a signal or broadcast */
+ while (queue->first == NULL && ret != ETIMEDOUT) { //Need to loop to handle spurious wakeups
+ if (timeout)
+ ret = pthread_cond_timedwait(&queue->cond, &queue->mutex, &abstimeout);
+ else
+ pthread_cond_wait(&queue->cond, &queue->mutex);
+ }
+ if (ret == ETIMEDOUT) {
+ pthread_mutex_unlock(&queue->mutex);
+ return ret;
+ }
+
+ firstrec = queue->first;
+ queue->first = queue->first->next;
+ queue->length--;
+
+ if (queue->first == NULL) {
+ queue->last = NULL; // we know this since we hold the lock
+ queue->length = 0;
+ }
+
+ msg->data = firstrec->msg.data;
+ msg->msgtype = firstrec->msg.msgtype;
+ msg->qlength = queue->length;
+
+ release_msglist(queue,firstrec);
+ pthread_mutex_unlock(&queue->mutex);
+
+ return 0;
+}
+
+//maybe caller should supply a callback for cleaning the elements ?
+int thread_queue_cleanup(struct threadqueue *queue, int freedata)
+{
+ struct msglist *rec;
+ struct msglist *next;
+ struct msglist *recs[2];
+ int ret,i;
+
+ if (queue == NULL)
+ return EINVAL;
+
+ pthread_mutex_lock(&queue->mutex);
+ recs[0] = queue->first;
+ recs[1] = queue->msgpool;
+ for(i = 0; i < 2 ; i++) {
+ rec = recs[i];
+ while (rec) {
+ next = rec->next;
+ if (freedata)
+ free(rec->msg.data);
+ free(rec);
+ rec = next;
+ }
+ }
+
+ pthread_mutex_unlock(&queue->mutex);
+ ret = pthread_mutex_destroy(&queue->mutex);
+ pthread_cond_destroy(&queue->cond);
+
+ return ret;
+}
+
+long thread_queue_length(struct threadqueue *queue)
+{
+ long counter;
+ /* get the length properly */
+ pthread_mutex_lock(&queue->mutex);
+ counter = queue->length;
+ pthread_mutex_unlock(&queue->mutex);
+
+ return counter;
+}