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; }
/* * 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_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; }
static void dlist_lock(DList* thiz) { if(thiz->locker != NULL) { locker_lock(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); }
/* * 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_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_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; }
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; }
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_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; }