/*
* message_claim
*/
int message_claim(message_queue_t * MessageQueue)
{
MessageElem_t * Elem_p;
int i;
BOOL SemWait = FALSE;
if (! MessageInitDone)
{
printf("%s() not initialized !!!\n", __FUNCTION__);
return((int)NULL);
}
if (MessageQueue->Index < QUEUE_NB_MAX)
{
PrintMessageQ_Debug(("COMPAT: Claiming (%d)\n", MessageQueue->Index));
Elem_p = &MessageArray[ MessageQueue->Index ];
do
{
if (SemWait)
{
PrintMessageQ_FullDebug(("COMPAT: Semaphore unlocked (%d)\n", MessageQueue->Index));
}
/* Scan every element to find first free one */
locker_lock(message_mutex);
for (i=0 ; i<Elem_p->NoElements ; i++)
{
if (! Elem_p->Used_p[i])
{
PrintMessageQ_Debug(("COMPAT: Claiming done, elem:%d\n", i));
Elem_p->Used_p[i] = TRUE;
locker_unlock(message_mutex);
return( (int)(Elem_p->Memory_p) + (int)(i * Elem_p->ElementSize) );
}
}
locker_unlock(message_mutex);
/* No free message has been found, waiting for message release */
SemWait = TRUE;
PrintMessageQ_FullDebug(("COMPAT: message_claim, No free message, waiting ...\n"));
}
while (semaphore_wait_timeout(MessageQueue->ClaimSemaphore_p, FYF_WAIT_FOREVER) == 0);
PrintMessageQ_FullDebug(("COMPAT: Claiming timeout\n"));
}
return((int)NULL);
}
/*
* message_release
*/
void message_release (message_queue_t* MessageQueue, void* Message)
{
MessageElem_t * Elem_p;
U32 Index;
if (! MessageInitDone)
{
printf("%s(): not initialized !!!\n", __FUNCTION__);
return;
}
if (MessageQueue->Index < QUEUE_NB_MAX)
{
PrintMessageQ_Debug(("COMPAT: Releasing (%d)\n", MessageQueue->Index));
Elem_p = &MessageArray[ MessageQueue->Index ];
locker_lock(message_mutex);
if ( ((U32)Message >= (U32)(Elem_p->Memory_p))
&& ((U32)Message < (U32)(Elem_p->Memory_p) + (Elem_p->ElementSize*Elem_p->NoElements)) )
{
Index = ((U32)Message - (U32)(Elem_p->Memory_p))/((U32)(Elem_p->ElementSize));
Elem_p->Used_p[Index] = FALSE;
PrintMessageQ_Debug(("COMPAT: %d released\n", Index));
sem_post(MessageQueue->ClaimSemaphore_p);
}
else
{
printf(( "%s() pb !!!\n", __FUNCTION__));
}
locker_unlock(message_mutex);
}
}
int rtp_queue_read(void* queue, void **rtp, int *len, int *lostPacket)
{
struct rtp_queue *q;
struct rtp_frame *frame;
q = (struct rtp_queue *)queue;
locker_lock(&q->locker);
frame = rtp_queue_pop(q);
locker_unlock(&q->locker);
if(!frame)
return -1;
#if 1
if(frame->seq != q->expected)
{
char msg[64];
sprintf(msg, "lost: %u-%u\n", q->expected, frame->seq);
// OutputDebugString(msg);
}
#endif
*rtp = frame->ptr;
*len = frame->len;
*lostPacket = (frame->seq == q->expected) ? 0 : 1;
q->expected = frame->seq + 1; // update expected
return 0;
}
int rtp_queue_unlock(void* queue, void* ptr, int size)
{
unsigned int v;
struct rtp_queue *q;
struct rtp_frame *frame;
// time64_t tnow = time64_now();
q = (struct rtp_queue *)queue;
frame = (struct rtp_frame *)ptr - 1;
assert(frame->ptr == (unsigned char *)ptr);
v = nbo_r32((unsigned char *)ptr);
frame->seq = RTP_SEQ(v);
// frame->timestamp = ntohl(((const unsigned int *)ptr)[1]);
// frame->ssrc = ntohl(((const unsigned int *)ptr)[2]);
frame->clock = rtpclock();
frame->len = size;
assert(!frame->next);
locker_lock(&q->locker);
rtp_queue_push(q, frame);
// rtp_queue_dump(q);
locker_unlock(&q->locker);
return 0;
}
int router_add(struct router_t* router, const uint8_t id[N_NODEID], const struct sockaddr_storage* addr, struct node_t** node)
{
int r;
struct rbitem_t* item;
struct rbtree_node_t **link;
struct rbtree_node_t *parent;
if (node) *node = NULL;
item = calloc(1, sizeof(*item));
if (!item)
return ENOMEM;
item->node = node_create2(id, addr);
if (!item->node)
{
free(item->node);
return ENOMEM;
}
locker_lock(&router->locker);
r = rbtree_find(&router->rbtree, id, &parent);
if (0 == r)
{
if (node)
{
*node = (rbtree_entry(parent, struct rbitem_t, link))->node;
node_addref(*node);
}
locker_unlock(&router->locker);
node_release(item->node);
free(item);
return EEXIST;
}
link = parent ? (r > 0 ? &parent->left : &parent->right) : NULL;
assert(!link || !*link);
rbtree_insert(&router->rbtree, parent, link, &item->link);
router->count += 1;
if (node)
{
node_addref(item->node);
*node = item->node;
}
locker_unlock(&router->locker);
return 0;
}
static void dlist_unlock(DList* thiz)
{
if(thiz->locker != NULL)
{
locker_unlock(thiz->locker);
}
return;
}
void message_init_queue(message_queue_t * MessageQueue,
void * Memory_p,
size_t ElementSize,
unsigned int NoElements)
{
MessageElem_t * Elem_p;
int Index;
printf("message_init_queue 111\n");
locker_lock(message_mutex);
Init_Allqueues();
printf("message_init_queue 222\n");
/* Seeking for unused queue */
Index = 0;
Elem_p = MessageArray;
while ((Index<QUEUE_NB_MAX) && (Elem_p->Memory_p != NULL))
{
Index++;
Elem_p++;
}
printf("message_init_queue 333\n");
MessageQueue->Index = Index;
if (Index < QUEUE_NB_MAX) /* if valid unused queue has been found */
{
Elem_p = &MessageArray[ MessageQueue->Index ];
Elem_p->Used_p = wrapper_allocate(NoElements*sizeof(BOOL)); /* Allocates used/unused element array */
printf("message_init_queue 444\n");
if (Elem_p->Used_p != NULL)
{
memset((void *)Elem_p->Used_p, 0, NoElements*sizeof(BOOL)); /* Sets all elements to unused */
Elem_p->Memory_p = Memory_p;
Elem_p->Pending_p = NULL;
Elem_p->ElementSize = ElementSize;
Elem_p->NoElements = NoElements;
printf("message_init_queue 555\n");
sem_init(MessageQueue->MsgSemaphore_p, NULL, 0);
sem_init(MessageQueue->ClaimSemaphore_p, NULL, 0);
printf("message_init_queue 666\n");
// MessageQueue->MsgSemaphore_p = semaphore_create_fifo(0);
// MessageQueue->ClaimSemaphore_p = semaphore_create_fifo(0);
PrintMessageQ_Debug(("COMPAT: Message_queue initialized: %d\n", MessageQueue->Index));
}
else
{
printf("%s(): No memory !!!\n", __FUNCTION__);
}
}
else
{
printf("%s(): No memory !!!\n", __FUNCTION__);
}
locker_unlock(message_mutex);
}
static void dlist_destroy_locker(DList* thiz)
{
if(thiz->locker != NULL)
{
locker_unlock(thiz->locker);
locker_destroy(thiz->locker);
}
return;
}
int router_remove(struct router_t* router, const uint8_t id[N_NODEID])
{
int r;
struct rbitem_t* item;
struct rbtree_node_t* node;
locker_lock(&router->locker);
r = rbtree_find(&router->rbtree, id, &node);
if (0 != r)
{
locker_unlock(&router->locker);
return ENOENT;
}
router->count -= 1;
item = rbtree_entry(node, struct rbitem_t, link);
rbtree_delete(&router->rbtree, node);
locker_unlock(&router->locker);
node_release(item->node);
free(item);
return 0;
}
/*
* message_send
*/
int message_send(message_queue_t * MessageQueue, void * message)
{
MessageSend_t * Pending_p;
MessageSend_t * New_p;
if (! MessageInitDone)
{
printf("%s(): not initialized !!!\n", __FUNCTION__);
return 0;
}
if (MessageQueue->Index < QUEUE_NB_MAX)
{
PrintMessageQ_Debug(("COMPAT: Sending (%d)\n", MessageQueue->Index));
New_p = wrapper_allocate(sizeof(MessageSend_t));
if (New_p != NULL)
{
New_p->Next_p = NULL;
New_p->Message_p = message;
PrintMessageQ_Debug(("COMPAT: Sending (0x%x)\n", New_p->Message_p));
locker_lock(message_mutex);
Pending_p = MessageArray[ MessageQueue->Index ].Pending_p;
if (Pending_p != NULL)
{
while (Pending_p->Next_p != NULL)
Pending_p = Pending_p->Next_p;
Pending_p->Next_p = New_p;
}
else
{
MessageArray[ MessageQueue->Index ].Pending_p = New_p;
}
locker_unlock(message_mutex);
sem_post(MessageQueue->MsgSemaphore_p);
}
else
{
printf("%s(): no memory !!!\n", __FUNCTION__);
}
}
return 0;
}
int router_list(struct router_t* router, int (*func)(void* param, struct node_t* node), void* param)
{
int r = 0;
struct rbitem_t* item;
const struct rbtree_node_t* node;
locker_lock(&router->locker);
node = rbtree_first(&router->rbtree);
while (node && 0 == r)
{
item = rbtree_entry(node, struct rbitem_t, link);
r = func(param, item->node);
node = rbtree_next(node);
}
locker_unlock(&router->locker);
return r;
}
/*
* message_receive_timeout
*/
void * message_receive_timeout (message_queue_t* MessageQueue, int waittime)
{
MessageSend_t * Pending_p;
MessageElem_t * Elem_p;
void * Message_p = NULL;
BOOL SemWait = FALSE;
if (! MessageInitDone)
{
printf( "%s(): not initialized !!!\n", __FUNCTION__);
return (Message_p);
}
if (MessageQueue->Index < QUEUE_NB_MAX)
{
PrintMessageQ_FullDebug(("COMPAT: Receiving (%d)\n", MessageQueue->Index));
#ifdef MESSAGE_QUEUE_DEBUG
Elem_p = &MessageArray[ MessageQueue->Index ];
locker_lock(message_mutex);
Pending_p = Elem_p->Pending_p;
if (Pending_p != NULL)
{
PrintMessageQ_Debug(("COMPAT: Queue %d: ", MessageQueue->Index));
while (Pending_p != NULL)
{
PrintMessageQ_Debug(("-> 0x%x ", Pending_p->Message_p));
Pending_p = Pending_p->Next_p;
}
PrintMessageQ_Debug(("-> NULL\n"));
}
locker_unlock(message_mutex);
#endif
do
{
if (SemWait)
{
PrintMessageQ_FullDebug(("COMPAT: Semaphore unlocked (%d)\n", MessageQueue->Index));
}
Elem_p = &MessageArray[ MessageQueue->Index ];
locker_lock(message_mutex);
Pending_p = Elem_p->Pending_p;
if (Pending_p != NULL)
{
Elem_p->Pending_p = Pending_p->Next_p;
Message_p = Pending_p->Message_p;
wrapper_deallocate(Pending_p);
PrintMessageQ_Debug(("COMPAT: Queue %d: Received 0x%x \n", MessageQueue->Index, Message_p));
locker_unlock(message_mutex);
return (Message_p);
}
locker_unlock(message_mutex);
SemWait = TRUE;
PrintMessageQ_FullDebug(("COMPAT: message_receive (%d), No message available, waiting ...\n", MessageQueue->Index));
}
while (semaphore_wait_timeout(MessageQueue->MsgSemaphore_p, waittime) == 0);
PrintMessageQ_FullDebug(("COMPAT: Receive timeout (%d) \n", MessageQueue->Index));
}
return (Message_p);
}
/*
* message_create_queue_timeout
*/
message_queue_t * message_create_queue(size_t ElementSize, unsigned int NoElements)
{
MessageQueueList_t * Current_p;
MessageQueueList_t * New_p;
message_queue_t * MesQ_p = NULL;
if ((New_p = wrapper_allocate(sizeof(MessageQueueList_t))) != NULL)
{
if ((New_p->MessageQueue_p = wrapper_allocate(sizeof(message_queue_t))) != NULL)
{
if ((New_p->Memory_p = wrapper_allocate(MESSAGE_MEMSIZE_QUEUE(ElementSize, NoElements))) == NULL)
{
/* Memory allocation pb, deallocate list element */
wrapper_deallocate(New_p->MessageQueue_p);
wrapper_deallocate(New_p);
New_p = NULL;
}
}
else
{
/* Memory allocation pb, deallocate list element */
wrapper_deallocate(New_p);
New_p = NULL;
}
}
printf("\n\nmessage_create_queue 111\n");
if (New_p != NULL)
{
message_init_queue( New_p->MessageQueue_p, New_p->Memory_p,
ElementSize, NoElements);
printf("\n\nmessage_create_queue 222\n");
if (MessageArray[ New_p->MessageQueue_p->Index ].Used_p != NULL)
{
/* At this stage, we know that all allocations have been correctly done */
New_p->Next_p = NULL;
locker_lock(message_mutex);
Current_p = MessageQueueList_p;
if (Current_p != NULL)
{
while (Current_p->Next_p != NULL)
Current_p = Current_p->Next_p;
Current_p->Next_p = New_p;
}
else
{
MessageQueueList_p = New_p;
}
locker_unlock(message_mutex);
MesQ_p = New_p->MessageQueue_p;
PrintMessageQ_Debug(("COMPAT: message_create_queue_timeout done (0x%x)!!!\n", MesQ_p));
}
else
{
message_delete_queue(New_p->MessageQueue_p);
wrapper_deallocate(New_p->MessageQueue_p);
wrapper_deallocate(New_p->Memory_p);
wrapper_deallocate(New_p);
printf("%s(): message queue init problem !!!\n", __FUNCTION__);
}
}
else
{
printf("%s(): no memory !!!\n", __FUNCTION__);
}
/* NULL if Message Queue creation problem, message queue pointer otherwise */
return(MesQ_p);
}
/*
* message_delete_queue
*/
void message_delete_queue(message_queue_t * MessageQueue)
{
MessageQueueList_t * Current_p = NULL; /* to avoid warning */
MessageQueueList_t * Deleted_p = NULL;
MessageSend_t * Pending_p;
MessageElem_t * Elem_p;
if (! MessageInitDone)
{
printf("%s() not initialized !!!\n", __FUNCTION__);
return;
}
if (MessageQueue->Index < QUEUE_NB_MAX)
{
Elem_p = &MessageArray[ MessageQueue->Index ];
/* Delete associated semaphores */
sem_destroy(MessageQueue->MsgSemaphore_p);
sem_destroy(MessageQueue->ClaimSemaphore_p);
locker_lock(message_mutex);
Elem_p->Memory_p = NULL;
Pending_p = Elem_p->Pending_p;
while (Pending_p != NULL)
{
Elem_p->Pending_p = Elem_p->Pending_p->Next_p;
wrapper_deallocate(Pending_p);
Pending_p = Elem_p->Pending_p;
}
Elem_p->ElementSize = 0;
Elem_p->NoElements = 0;
if (Elem_p->Used_p != NULL)
{
wrapper_deallocate(Elem_p->Used_p);
Elem_p->Used_p = NULL;
}
locker_unlock(message_mutex);
}
MessageQueue->Index = QUEUE_NB_MAX;
/* Frees the message queue in the list */
locker_lock(message_mutex);
if (MessageQueueList_p != NULL)
{
if (MessageQueueList_p->MessageQueue_p == MessageQueue)
{
Deleted_p = MessageQueueList_p;
}
else
{
Current_p = MessageQueueList_p;
while ( (Current_p->Next_p != NULL)
&& (Deleted_p == NULL))
{
if (Current_p->Next_p->MessageQueue_p == MessageQueue)
{
/* Element to destroy has been found */
Deleted_p = Current_p->Next_p;
}
else
{
/* Goes to next element */
Current_p = Current_p->Next_p;
}
}
}
if (Deleted_p != NULL)
{
if (Deleted_p == MessageQueueList_p)
{
MessageQueueList_p = Deleted_p->Next_p;
}
else
{
Current_p->Next_p = Deleted_p->Next_p;
}
wrapper_deallocate(Deleted_p->Memory_p);
wrapper_deallocate(Deleted_p->MessageQueue_p);
wrapper_deallocate(Deleted_p);
}
else
{
printf("%s() problem (0x%x), not found !!!\n", __FUNCTION__, (U32)MessageQueue);
}
}
locker_unlock(message_mutex);
PrintMessageQ_Debug(("COMPAT: message_delete_queue done !!!\n"));
}
int router_nearest(struct router_t* router, const uint8_t id[N_NODEID], struct node_t* nodes[], size_t count)
{
int i, min, diff;
uint8_t xor[N_NODEID];
heap_t* heap;
struct rbitem_t* item;
struct rbtree_node_t* node;
const struct rbtree_node_t* prev;
const struct rbtree_node_t* next;
heap = heap_create(node_compare_less, (void*)id);
heap_reserve(heap, count + 1);
min = N_BITS;
locker_lock(&router->locker);
rbtree_find(&router->rbtree, id, &node);
if (NULL == node)
{
locker_unlock(&router->locker);
return 0;
}
item = rbtree_entry(node, struct rbitem_t, link);
bitmap_xor(xor, id, item->node->id, N_BITS);
diff = bitmap_count_leading_zero(xor, N_BITS);
min = min < diff ? min : diff;
heap_push(heap, item->node);
prev = rbtree_prev(node);
next = rbtree_next(node);
do
{
while (prev)
{
item = rbtree_entry(prev, struct rbitem_t, link);
bitmap_xor(xor, id, item->node->id, N_BITS);
diff = bitmap_count_leading_zero(xor, N_BITS);
heap_push(heap, item->node);
if (heap_size(heap) > (int)count)
heap_pop(heap);
prev = rbtree_prev(prev);
if (diff < min)
{
min = diff;
break; // try right
}
}
while (next)
{
item = rbtree_entry(next, struct rbitem_t, link);
bitmap_xor(xor, id, item->node->id, N_BITS);
diff = bitmap_count_leading_zero(xor, N_BITS);
heap_push(heap, item->node);
if (heap_size(heap) > (int)count)
heap_pop(heap);
next = rbtree_next(next);
if (diff < min)
{
min = diff;
break; // try left
}
}
} while (heap_size(heap) < (int)count && (prev || next));
for (i = 0; i < (int)count && !heap_empty(heap); i++)
{
nodes[i] = heap_top(heap);
node_addref(nodes[i]);
heap_pop(heap);
}
locker_unlock(&router->locker);
heap_destroy(heap);
return i;
}
