libxenomai-dev 2.6.3-2 / usr / include / xenomai / nucleus / bheap.h

/*
 * Copyright (C) 2006 Gilles Chanteperdrix <gilles.chanteperdrix@xenomai.org>
 *
 * Xenomai is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published
 * by the Free Software Foundation; either version 2 of the License,
 * or (at your option) any later version.
 *
 * Xenomai is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Xenomai; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
 * 02111-1307, USA.
 */

#ifndef _XENO_NUCLEUS_BHEAP_H
#define _XENO_NUCLEUS_BHEAP_H

#include <nucleus/compiler.h>

/* debug support */
#include <nucleus/assert.h>

/* Priority queue implementation, using a binary heap. */

typedef unsigned long long bheap_key_t;

typedef struct bheaph {
	bheap_key_t key;
	unsigned prio;
	unsigned pos;
} bheaph_t;

#define bheaph_init(holder) do { } while (0)
#define bheaph_key(holder)  ((holder)->key)
#define bheaph_prio(holder) ((holder)->prio)
#define bheaph_pos(holder)  ((holder)->pos)
#define bheaph_lt(h1, h2)   ((long long) ((h1)->key - (h2)->key) < 0 ||	\
			     ((h1)->key == (h2)->key &&			\
			      (h1)->prio > (h2)->prio))

typedef struct bheap {
	unsigned sz;
	unsigned last;
	bheaph_t *elems[];
} bheap_t;

#define DECLARE_BHEAP_CONTAINER(name, sz)       \
	struct {				\
		bheap_t bheap;			\
		bheaph_t *elems[sz + 1];	\
	} name

/* Check the binary heap invariant. */
static inline int bheap_ordered(bheap_t *heap)
{
	unsigned i;
	for (i = 2; i < heap->last; i++)
		if (bheaph_lt(heap->elems[i], heap->elems[i / 2]))
			return 0;
	return 1;
}

#define BHEAP_CHECK(heap)						\
	XENO_BUGON(QUEUES, ((heap)->sz == 0) || !bheap_ordered(heap))

#define bheap_gethead(heap)				\
	({                                              \
		bheap_t *_bheap = &(heap)->bheap;	\
		BHEAP_CHECK(_bheap);			\
		__internal_bheap_gethead(_bheap);	\
	})

static inline bheaph_t *__internal_bheap_gethead(bheap_t *heap)
{
	if (heap->last == 1)
		return NULL;

	return heap->elems[1];
}

#define bheap_next(heap, holder)			\
	({						\
		bheap_t *_bheap = &(heap)->bheap;	\
		BHEAP_CHECK(_bheap);			\
		__internal_bheap_next(_bheap, holder);	\
	})

static inline bheaph_t *__internal_bheap_next(bheap_t *heap, bheaph_t *holder)
{
	unsigned pos;

	if (unlikely(bheaph_pos(holder) >= heap->last
		     || heap->elems[bheaph_pos(holder)] != holder))
		return (bheaph_t *) ERR_PTR(-EINVAL);

	pos = bheaph_pos(holder) + 1;

	return likely(pos < heap->last) ? heap->elems[pos] : NULL;
}

static inline bheaph_t *bheaph_parent(bheap_t *heap, bheaph_t *holder)
{
	const unsigned pos = holder->pos;

	return likely(pos > 1) ? heap->elems[pos / 2] : NULL;
}

static inline bheaph_t *bheaph_child(bheap_t *heap, bheaph_t *holder, int side)
{
	const unsigned pos = 2 * holder->pos + side;

	return likely(pos < heap->last) ? heap->elems[pos] : NULL;
}

#define bheap_init(heap, sz) __internal_bheap_init(&(heap)->bheap, sz)

static inline void __internal_bheap_init(bheap_t *heap, unsigned sz)
{
	heap->sz = sz;
	heap->last = 1;
}

#define bheap_destroy(heap) __internal_bheap_destroy(&(heap)->bheap)

static inline void __internal_bheap_destroy(bheap_t *heap)
{
	heap->sz = 0;
	heap->last = 1;
}

static inline void bheap_swap(bheap_t *heap, bheaph_t *h1, bheaph_t *h2)
{
	const unsigned pos2 = bheaph_pos(h2);

	heap->elems[bheaph_pos(h1)] = h2;
	bheaph_pos(h2) = bheaph_pos(h1);
	heap->elems[pos2] = h1;
	bheaph_pos(h1) = pos2;
}

static inline void bheap_up(bheap_t *heap, bheaph_t *holder)
{
	bheaph_t *parent;

	while ((parent = bheaph_parent(heap, holder)) && bheaph_lt(holder, parent))
		bheap_swap(heap, holder, parent);
}

static inline void bheap_down(bheap_t *heap, bheaph_t *holder)
{
	bheaph_t *left, *right, *minchild;

	for (;;) {
		left = bheaph_child(heap, holder, 0);
		right = bheaph_child(heap, holder, 1);

		if (left && right)
			minchild = bheaph_lt(left, right) ? left : right;
		else
			minchild = left ?: right;

		if (!minchild || bheaph_lt(holder, minchild))
			break;

		bheap_swap(heap, minchild, holder);
	}
}

#define bheap_insert(heap, holder)				\
	({							\
		bheap_t *_bheap = &(heap)->bheap;		\
		BHEAP_CHECK(_bheap);				\
		__internal_bheap_insert(_bheap, holder);	\
	})

static inline int __internal_bheap_insert(bheap_t *heap, bheaph_t *holder)
{
	if (heap->last == heap->sz + 1)
		return EBUSY;

	heap->elems[heap->last] = holder;
	bheaph_pos(holder) = heap->last;
	++heap->last;
	bheap_up(heap, holder);
	return 0;
}

#define bheap_delete(heap, holder)				\
	({							\
		bheap_t *_bheap = &(heap)->bheap;		\
		BHEAP_CHECK(_bheap);				\
		__internal_bheap_delete(_bheap, holder);	\
	})

static inline int __internal_bheap_delete(bheap_t *heap, bheaph_t *holder)
{
	bheaph_t *lasth;

	if (unlikely(bheaph_pos(holder) >= heap->last
		     || heap->elems[bheaph_pos(holder)] != holder))
		return EINVAL;

	--heap->last;
	if (heap->last != bheaph_pos(holder)) {
		bheaph_t *parent;
		lasth = heap->elems[heap->last];
		heap->elems[bheaph_pos(holder)] = lasth;
		bheaph_pos(lasth) = bheaph_pos(holder);
		if ((parent = bheaph_parent(heap, lasth)) && bheaph_lt(lasth, parent))
			bheap_up(heap, lasth);
		else
			bheap_down(heap, lasth);
	}

	return 0;
}

#define bheap_get(heap)					\
	({                                              \
		bheap_t *_bheap = &(heap)->bheap;	\
		BHEAP_CHECK(_bheap);			\
		__internal_bheap_get(_bheap, holder);	\
	})

static inline bheaph_t *__internal_bheap_get(bheap_t *heap)
{
	bheaph_t *holder = __internal_bheap_gethead(heap);

	if (!holder)
		return NULL;

	__internal_bheap_delete(heap, holder);

	return holder;
}

#endif /* _XENO_NUCLEUS_BHEAP_H */