quadrotor/event.c

#include "event.h"
#include "interrupt.h"
#include "types.h"

event_handler *event_handler_table[EVENT_MAX+1];

#define EVENT_WORDS ((EVENT_MAX+1+31)/32)

unsigned int event_pending[EVENT_WORDS];

#define EVENT_WORDLEN	32
#define EVENT_WORD(x)	(x/EVENT_WORDLEN)
#define EVENT_BIT(x)	(x%EVENT_WORDLEN)
#define EVENT_MASK(x)	(1<<EVENT_BIT(x))

void event_set(unsigned int event)
{
	if (event > EVENT_MAX)
		return;

	interrupt_block();
	event_pending[EVENT_WORD(event)] |= EVENT_MASK(event);
	interrupt_unblock();
}

static int bitset(unsigned int x)
{
	x = 0xffffffff - (x-1);
	x = x - ((x >> 1) & 0x55555555);
	x = (x & 0x33333333) + ((x >> 2) & 0x33333333);
	x = (x + (x >> 4)) & 0x0F0F0F0F;
	x = x + (x << 8);
	x = x + (x << 16);
	return 32 - (x >> 24);
}

bool event_get(unsigned int *event)
{
	int i;
	unsigned int p;

	for (i = 0; i < EVENT_WORDS; i++) {
		if ((p = event_pending[i]) != 0) {
			p = p & (-p);
			*event = bitset(p) + EVENT_WORDLEN*i;
			return TRUE;
		}
	}
	return FALSE;
}

void event_clear(unsigned int event)
{
	interrupt_block();
	event_pending[EVENT_WORD(event)] &= ~EVENT_MASK(event);
	interrupt_unblock();
}

void event_dispatch(void)
{
	unsigned int event;

	if (event_get(&event)) {
		event_clear(event);
		(event_handler_table[event])();
	}
}

void event_register(unsigned int event, event_handler *handler)
{
	if (event > EVENT_MAX)
		return;
	event_handler_table[event] = handler;
}