Exemple #1
0
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);
	}
}
Exemple #3
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;
}
Exemple #4
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;
}
Exemple #8
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;
}
Exemple #9
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;
}
Exemple #10
0
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"));
}
Exemple #14
0
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;
}