wxtimer.c

wimax BS simulation code,implemented under linux.

/*
 * This piece of code is totally free. If any pitfalls found, 
 * please feel free to contact me at [e-mail]
 * THANKS A LOT!
 */

#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <pthread.h>

#include "que.h"
#include "wxtimer.h"


#define NR_WXTIMER_MAX		16


static wxtimer_t * insert_timer(struct rb_root *root, wxtimer_t *new);
static void rb_erase(struct rb_root *root, struct rb_node *node);
static void rb_insert_color(struct rb_root *root, struct rb_node *node);
static void rb_erase_color(struct rb_root *root, struct rb_node *node, struct rb_node *parent);
static struct rb_node * rb_first(struct rb_root *root);
static struct rb_node * rb_last(struct rb_root *root);
static struct rb_node * rb_next(struct rb_node *node);
static struct rb_node * rb_prev(struct rb_node *node);
static void rb_link_node(struct rb_node *node, struct rb_node *parent, struct rb_node **rb_link);
static void rb_rotate_left(struct rb_node *node, struct rb_root *root);
static void rb_rotate_right(struct rb_node * node, struct rb_root * root);


pool_t g_wxtmpool;
uint32_t jiffies = 0;
struct wxtimer_tree_s g_wxtmtree;


int32_t wxtimer_init()
{
	pthread_mutex_init(&g_wxtmtree.mutex, NULL);
	g_wxtmtree.rb_root.rb_node = g_wxtmtree.rb_first = NULL;
	g_wxtmtree.nr = 0;

	return 0;
}


int32_t wxtmpool_init()
{
	wxtimer_t *tm;
	int32_t i;


	pool_init(&g_wxtmpool);

	for ( i = 0; i < NR_WXTIMER_MAX; i++ ) {
		if ( (tm = (wxtimer_t *)malloc(sizeof(wxtimer_t))) == NULL )
			return -1;

		list_add_tail(&tm->link, &g_wxtmpool.link);
		g_wxtmpool.count++;
	}

	return 0;
}


void * do_task_timer(void *arg)
{
	struct rb_node *n;


	for ( ; ; ) {
		usleep(20000);///20000us

		pthread_mutex_lock(&g_wxtmtree.mutex);
		jiffies++;

		n = g_wxtmtree.rb_first;
		while ( n ) {
			wxtimer_t *tm;

			tm = rb_entry(n, wxtimer_t, rb_link);

			if ( jiffies != tm->future )
				break;

			if ( tm->action )
				tm->action(tm->arga, tm->argb);

			rb_erase(&g_wxtmtree.rb_root, n);/*删除n 节点*/
			n->rb_parent = n->rb_left = n->rb_right = NULL;
			g_wxtmtree.nr--;

			if ( tm->type == WXTIMER_PERIODIC ) {
				tm->timestamp	= jiffies;		/*jiffies 是当前值,tm->count  是定时计数值
												*tm->future 则是当前值加上计数值*/
				tm->future	= jiffies + tm->count;
				
				insert_timer(&g_wxtmtree.rb_root, tm);
				rb_insert_color(&g_wxtmtree.rb_root, n);
				g_wxtmtree.rb_first = rb_first(&g_wxtmtree.rb_root);
				g_wxtmtree.nr++;
			}
			else {
				put_timer(tm);
			}

			n = rb_first(&g_wxtmtree.rb_root);
		}
		g_wxtmtree.rb_first = n;//here n = NULL

		pthread_mutex_unlock(&g_wxtmtree.mutex);
	}

	return NULL;
}


wxtimer_t * get_timer(uint8_t type, uint32_t count, uint32_t arga, void *argb, void (*action)(uint32_t arga, void *argb))
{
	wxtimer_t *tm;


	pthread_mutex_lock(&g_wxtmpool.mutex);
	
	if ( g_wxtmpool.count == 0 )
	        tm = NULL;
	else {
		tm = list_entry(g_wxtmpool.link.next, wxtimer_t, link);
                list_del(g_wxtmpool.link.next);
		g_wxtmpool.count--;
		
		memset(&tm->rb_link, 0, sizeof(tm->rb_link));
		tm->type	= type;
		tm->count	= count;
		tm->arga	= arga;
		tm->argb	= argb;
		tm->action	= action;
	}

	pthread_mutex_unlock(&g_wxtmpool.mutex);

	return tm;
}


int32_t start_timer(wxtimer_t *timer)
{
	timer->timestamp = jiffies;
	timer->future = jiffies + timer->count;

	pthread_mutex_lock(&g_wxtmtree.mutex);
	
	insert_timer(&g_wxtmtree.rb_root, timer);
	rb_insert_color(&g_wxtmtree.rb_root, &timer->rb_link);
	g_wxtmtree.rb_first = rb_first(&g_wxtmtree.rb_root);
	g_wxtmtree.nr++;

	pthread_mutex_unlock(&g_wxtmtree.mutex);

	return 0;
}


int32_t cancel_timer(wxtimer_t *timer)
{
	pthread_mutex_lock(&g_wxtmtree.mutex);

	if ( timer->rb_link.rb_parent == NULL 
	     && timer->rb_link.rb_left == NULL && timer->rb_link.rb_right == NULL )
		goto out;
	
	rb_erase(&g_wxtmtree.rb_root, &timer->rb_link);
	g_wxtmtree.nr--;

 out:
	pthread_mutex_unlock(&g_wxtmtree.mutex);

	return 0;
}


int32_t put_timer(wxtimer_t *timer)
{
	pthread_mutex_lock(&g_wxtmpool.mutex);
	
	list_add_tail(&timer->link, &g_wxtmpool.link);
	g_wxtmpool.count++;

	pthread_mutex_unlock(&g_wxtmpool.mutex);

	return 0;
}


static wxtimer_t * insert_timer(struct rb_root *root, wxtimer_t *new)
{
	struct rb_node **p = &root->rb_node;
	struct rb_node *parent = NULL;
	wxtimer_t *tm;


	while ( *p ) {
		parent = *p;
		tm = rb_entry(parent, wxtimer_t, rb_link);

		if ( tm->timestamp <= new->timestamp ) {
			if ( tm->timestamp <= tm->future && new->timestamp <= new->future )
				p = tm->future <= new->future ? &(*p)->rb_right : &(*p)->rb_left;
			else if ( tm->timestamp <= tm->future && new->timestamp > new->future )
				p = &(*p)->rb_right;
			else if ( tm->timestamp > tm->future && new->timestamp <= new->future )
				p = &(*p)->rb_left;
			else if ( tm->timestamp > tm->future && new->timestamp > new->future )
				p = tm->future <= new->future ? &(*p)->rb_right : &(*p)->rb_left;
		}
		else {
			if ( tm->timestamp <= tm->future && new->timestamp <= new->future )
				p = &(*p)->rb_right;
			else if ( tm->timestamp <= tm->future && new->timestamp > new->future )
				p = &(*p)->rb_right;
			else if ( tm->timestamp > tm->future && new->timestamp <= new->future )
				p = tm->future <= new->future ? &(*p)->rb_right : &(*p)->rb_left;
			else if ( tm->timestamp > tm->future && new->timestamp > new->future )
				p = &(*p)->rb_right;
		}
	}

	rb_link_node(&new->rb_link, parent, p);
	
	return tm;
}


static void rb_erase(struct rb_root *root, struct rb_node *node)
{
	struct rb_node *child, *parent;
	int color;


	if ( !node->rb_left )
		child = node->rb_right;
	else if ( !node->rb_right )
		child = node->rb_left;
	else {
		struct rb_node * old = node, * left;

		node = node->rb_right;
		while ( (left = node->rb_left) )
			node = left;

		child = node->rb_right;
		parent = node->rb_parent;
		color = node->rb_color;

		if ( child )
			child->rb_parent = parent;
		if ( parent ) {
			if ( parent->rb_left == node )
				parent->rb_left = child;
			else
				parent->rb_right = child;
		}
		else
			root->rb_node = child;

		if ( node->rb_parent == old )
			parent = node;
		node->rb_parent = old->rb_parent;
		node->rb_color = old->rb_color;
		node->rb_right = old->rb_right;
		node->rb_left = old->rb_left;

		if ( old->rb_parent ) {
			if (old->rb_parent->rb_left == old)
				old->rb_parent->rb_left = node;
			else
				old->rb_parent->rb_right = node;
		} else
			root->rb_node = node;

		old->rb_left->rb_parent = node;
		if ( old->rb_right )
			old->rb_right->rb_parent = node;
		goto color;
	}

	parent = node->rb_parent;
	color = node->rb_color;

	if ( child )
		child->rb_parent = parent;
	if ( parent ) {
		if ( parent->rb_left == node )
			parent->rb_left = child;
		else
			parent->rb_right = child;
	}
	else
		root->rb_node = child;

 color:
	if ( color == RB_BLACK )
		rb_erase_color(root, child, parent);
}


static void rb_insert_color(struct rb_root *root, struct rb_node *node)
{
	struct rb_node *parent, *gparent;


	while ( (parent = node->rb_parent) && parent->rb_color == RB_RED ) {
		gparent = parent->rb_parent;

		if ( parent == gparent->rb_left ) {
			{
				register struct rb_node *uncle = gparent->rb_right;

				if (uncle && uncle->rb_color == RB_RED)	{
					uncle->rb_color = RB_BLACK;
					parent->rb_color = RB_BLACK;
					gparent->rb_color = RB_RED;
					node = gparent;
					continue;
				}
			}

			if ( parent->rb_right == node ) {
				register struct rb_node *tmp;

				rb_rotate_left(parent, root);
				tmp = parent;
				parent = node;
				node = tmp;
			}

			parent->rb_color = RB_BLACK;
			gparent->rb_color = RB_RED;
			rb_rotate_right(gparent, root);

		} else {
			{
				register struct rb_node *uncle = gparent->rb_left;

				if ( uncle && uncle->rb_color == RB_RED ) {
					uncle->rb_color = RB_BLACK;
					parent->rb_color = RB_BLACK;
					gparent->rb_color = RB_RED;
					node = gparent;
					continue;
				}
			}

			if ( parent->rb_left == node ) {
				register struct rb_node *tmp;

				rb_rotate_right(parent, root);
				tmp = parent;
				parent = node;
				node = tmp;
			}

			parent->rb_color = RB_BLACK;
			gparent->rb_color = RB_RED;
			rb_rotate_left(gparent, root);
		}
	}

	root->rb_node->rb_color = RB_BLACK;
}


static void rb_erase_color(struct rb_root *root, struct rb_node *node, struct rb_node *parent)
{
	struct rb_node *other;

	while ( (!node || node->rb_color == RB_BLACK) && node != root->rb_node ) {

		if ( parent->rb_left == node ) {
			other = parent->rb_right;
			if ( other->rb_color == RB_RED ) {
				other->rb_color = RB_BLACK;
				parent->rb_color = RB_RED;
				rb_rotate_left(parent, root);
				other = parent->rb_right;
			}
			if ( (!other->rb_left || other->rb_left->rb_color == RB_BLACK)
			    && (!other->rb_right || other->rb_right->rb_color == RB_BLACK) ) {

				other->rb_color = RB_RED;
				node = parent;
				parent = node->rb_parent;
			}
			else {
				if ( !other->rb_right || other->rb_right->rb_color == RB_BLACK ) {
					register struct rb_node *o_left;
					if ( (o_left = other->rb_left) )
						o_left->rb_color = RB_BLACK;
					other->rb_color = RB_RED;
					rb_rotate_right(other, root);
					other = parent->rb_right;
				}
				other->rb_color = parent->rb_color;
				parent->rb_color = RB_BLACK;
				if ( other->rb_right )
					other->rb_right->rb_color = RB_BLACK;
				rb_rotate_left(parent, root);
				node = root->rb_node;
				break;
			}
		}
		else {
			other = parent->rb_left;
			if ( other->rb_color == RB_RED ) {
				other->rb_color = RB_BLACK;
				parent->rb_color = RB_RED;
				rb_rotate_right(parent, root);
				other = parent->rb_left;
			}
			if ( (!other->rb_left || other->rb_left->rb_color == RB_BLACK)
			    && (!other->rb_right || other->rb_right->rb_color == RB_BLACK) ) {
				other->rb_color = RB_RED;
				node = parent;
				parent = node->rb_parent;
			}
			else {
				if ( !other->rb_left || other->rb_left->rb_color == RB_BLACK ) {
					register struct rb_node *o_right;
					if ( (o_right = other->rb_right) )
						o_right->rb_color = RB_BLACK;
					other->rb_color = RB_RED;
					rb_rotate_left(other, root);
					other = parent->rb_left;
				}
				other->rb_color = parent->rb_color;
				parent->rb_color = RB_BLACK;
				if ( other->rb_left )
					other->rb_left->rb_color = RB_BLACK;
				rb_rotate_right(parent, root);
				node = root->rb_node;
				break;
			}
		}
	}

	if ( node )
		node->rb_color = RB_BLACK;
}


static struct rb_node * rb_first(struct rb_root *root)
{
	struct rb_node *n;


	if ( (n = root->rb_node) == NULL )
		return NULL;

	while ( n->rb_left )
		n = n->rb_left;

	return n;
}


static struct rb_node * rb_last(struct rb_root *root)
{
	struct rb_node *n;
	
	
        if ( (n = root->rb_node) == NULL )
                return NULL;
	
        while ( n->rb_right )
                n = n->rb_right;

        return n;
}


static struct rb_node * rb_next(struct rb_node *node)
{
	/* If we have a right-hand child, go down and then left as far
	 * as we can.
	 */
	if ( node->rb_right ) {
                node = node->rb_right;

                while ( node->rb_left )
                        node = node->rb_left;

                return node;
        }

        /* No right-hand children.  Everything down and left is
	 * smaller than us, so any 'next' node must be in the general
         * direction of our parent. Go up the tree; any time the
	 * ancestor is a right-hand child of its parent, keep going
         * up. First time it's a left-hand child of its parent, said
         * parent is our 'next' node.
	 */
        while ( node->rb_parent && node == node->rb_parent->rb_right )
                node = node->rb_parent;

        return node->rb_parent;
}


static struct rb_node * rb_prev(struct rb_node *node)
{
        /* If we have a left-hand child, go down and then right as far
	 * as we can.
	 */
        if ( node->rb_left ) {
                node = node->rb_left;

                while ( node->rb_right )
                        node = node->rb_right;

                return node;
        }

        /* No left-hand children. Go up till we find an ancestor which
         * is a right-hand child of its parent.
	 */
        while ( node->rb_parent && node == node->rb_parent->rb_left )
                node = node->rb_parent;

        return node->rb_parent;
}


static void rb_link_node(struct rb_node *node, struct rb_node *parent, struct rb_node **rb_link)
{
	node->rb_parent = parent;
	node->rb_color = RB_RED;
	node->rb_left = node->rb_right = NULL;

	*rb_link = node;
}


static void rb_rotate_left(struct rb_node *node, struct rb_root *root)
{
	struct rb_node *right = node->rb_right;

	if ( (node->rb_right = right->rb_left) )
		right->rb_left->rb_parent = node;
	right->rb_left = node;
	
	if ( (right->rb_parent = node->rb_parent) ) {
		if ( node == node->rb_parent->rb_left )
			node->rb_parent->rb_left = right;
		else
			node->rb_parent->rb_right = right;
	}
	else
		root->rb_node = right;

	node->rb_parent = right;
}


static void rb_rotate_right(struct rb_node *node, struct rb_root *root)
{
	struct rb_node *left = node->rb_left;
	
	if ( (node->rb_left = left->rb_right) )
		left->rb_right->rb_parent = node;
	left->rb_right = node;
	
	if ( (left->rb_parent = node->rb_parent) ) {
		if ( node == node->rb_parent->rb_right )
			node->rb_parent->rb_right = left;
		else
			node->rb_parent->rb_left = left;
	}
	else
		root->rb_node = left;

	node->rb_parent = left;
}